motioncam_decoder/
raw.rs

1use crate::error::{McRawError, Result, read_le_bytes};
2use crate::simd_portable::*;
3use wide::u16x8;
4
5const ENCODING_BLOCK: usize = 64;
6const HEADER_LENGTH: usize = 2;
7const METADATA_OFFSET: usize = 16;
8const ENCODING_BLOCK_LENGTH: [usize; 17] = [
9    0, 8, 16, 24, 32, 40, 48, 64, 64, 80, 80, 128, 128, 128, 128, 128, 128,
10];
11
12#[inline(always)]
13fn decode1(out: &mut [u16], inp: &[u8]) -> Result<usize> {
14    let n = u16x8::splat(0x01);
15    let p = load_u8_to_u16x8(inp)?;
16    store_u16x8(&mut out[0..], p & n)?;
17    store_u16x8(&mut out[8..], (p & (n << 1u32)) >> 1u32)?;
18    store_u16x8(&mut out[16..], (p & (n << 2u32)) >> 2u32)?;
19    store_u16x8(&mut out[24..], (p & (n << 3u32)) >> 3u32)?;
20    store_u16x8(&mut out[32..], (p & (n << 4u32)) >> 4u32)?;
21    store_u16x8(&mut out[40..], (p & (n << 5u32)) >> 5u32)?;
22    store_u16x8(&mut out[48..], (p & (n << 6u32)) >> 6u32)?;
23    store_u16x8(&mut out[56..], (p & (n << 7u32)) >> 7u32)?;
24    Ok(ENCODING_BLOCK_LENGTH[1])
25}
26
27#[inline(always)]
28fn decode2_one(out: &mut [u16], inp: &[u8]) -> Result<usize> {
29    let n = u16x8::splat(0x03);
30    let p = load_u8_to_u16x8(inp)?;
31    store_u16x8(&mut out[0..], p & n)?;
32    store_u16x8(&mut out[8..], (p & (n << 2u32)) >> 2u32)?;
33    store_u16x8(&mut out[16..], (p & (n << 4u32)) >> 4u32)?;
34    store_u16x8(&mut out[24..], (p & (n << 6u32)) >> 6u32)?;
35    Ok(8)
36}
37
38#[inline(always)]
39fn decode2(out: &mut [u16], inp: &[u8]) -> Result<usize> {
40    let mut o = decode2_one(&mut out[0..], inp)?;
41    o += decode2_one(&mut out[32..], &inp[o..])?;
42    Ok(o)
43}
44
45#[inline(always)]
46fn decode3(out: &mut [u16], inp: &[u8]) -> Result<usize> {
47    let n = u16x8::splat(0x07);
48    let t = u16x8::splat(0x03);
49    let r = u16x8::splat(0x01);
50
51    let p0 = load_u8_to_u16x8(inp)?;
52    let p1 = load_u8_to_u16x8(&inp[8..])?;
53    let p2 = load_u8_to_u16x8(&inp[16..])?;
54
55    let r0 = p0 & n;
56    let r1 = (p0 & (n << 3u32)) >> 3u32;
57    let _r2 = (p0 & (t << 6u32)) >> 6u32;
58    let r3 = p1 & n;
59    let r4 = (p1 & (n << 3u32)) >> 3u32;
60    let _r5 = (p1 & (t << 6u32)) >> 6u32;
61    let r6 = p2 & n;
62    let r7 = (p2 & (n << 3u32)) >> 3u32;
63
64    let r2 = _r2 | (((p2 >> 6u32) & r) << 2u32);
65    let r5 = _r5 | (((p2 >> 7u32) & r) << 2u32);
66
67    store_u16x8(&mut out[0..], r0)?;
68    store_u16x8(&mut out[8..], r1)?;
69    store_u16x8(&mut out[16..], r2)?;
70    store_u16x8(&mut out[24..], r3)?;
71    store_u16x8(&mut out[32..], r4)?;
72    store_u16x8(&mut out[40..], r5)?;
73    store_u16x8(&mut out[48..], r6)?;
74    store_u16x8(&mut out[56..], r7)?;
75
76    Ok(ENCODING_BLOCK_LENGTH[3])
77}
78
79#[inline(always)]
80fn decode4_one(out: &mut [u16], inp: &[u8]) -> Result<usize> {
81    let n = u16x8::splat(0x0F);
82    let p = load_u8_to_u16x8(inp)?;
83    store_u16x8(&mut out[0..], p & n)?;
84    store_u16x8(&mut out[8..], (p & (n << 4u32)) >> 4u32)?;
85    Ok(8)
86}
87
88#[inline(always)]
89fn decode4(out: &mut [u16], inp: &[u8]) -> Result<usize> {
90    let mut o = decode4_one(&mut out[0..], inp)?;
91    o += decode4_one(&mut out[16..], &inp[o..])?;
92    o += decode4_one(&mut out[32..], &inp[o..])?;
93    o += decode4_one(&mut out[48..], &inp[o..])?;
94    Ok(o)
95}
96
97#[inline(always)]
98fn decode5(out: &mut [u16], inp: &[u8]) -> Result<usize> {
99    let n = u16x8::splat(0x1F);
100    let l = u16x8::splat(0x07);
101    let u = u16x8::splat(0x03);
102    let f = u16x8::splat(0x01);
103
104    let p0 = load_u8_to_u16x8(inp)?;
105    let p1 = load_u8_to_u16x8(&inp[8..])?;
106    let p2 = load_u8_to_u16x8(&inp[16..])?;
107    let p3 = load_u8_to_u16x8(&inp[24..])?;
108    let p4 = load_u8_to_u16x8(&inp[32..])?;
109
110    store_u16x8(&mut out[0..], p0 & n)?;
111    store_u16x8(&mut out[8..], p1 & n)?;
112    store_u16x8(&mut out[16..], p2 & n)?;
113    store_u16x8(&mut out[24..], p3 & n)?;
114    store_u16x8(&mut out[32..], p4 & n)?;
115    store_u16x8(&mut out[40..], (p0 >> 5u32) & l | (((p3 >> 5u32) & u) << 3u32))?;
116    store_u16x8(&mut out[48..], (p1 >> 5u32) & l | (((p4 >> 5u32) & u) << 3u32))?;
117
118    let tmp0 = (p2 >> 5u32) & l;
119    let tmp1 = tmp0 | ((p3 >> 7u32) & f) << 3u32;
120    store_u16x8(&mut out[56..], tmp1 | (((p4 >> 7u32) & f) << 4u32))?;
121
122    Ok(ENCODING_BLOCK_LENGTH[5])
123}
124
125#[inline(always)]
126fn decode6(out: &mut [u16], inp: &[u8]) -> Result<usize> {
127    let n = u16x8::splat(0x3F);
128    let l = u16x8::splat(0x03);
129
130    let p0 = load_u8_to_u16x8(inp)?;
131    let p1 = load_u8_to_u16x8(&inp[8..])?;
132    let p2 = load_u8_to_u16x8(&inp[16..])?;
133    let p3 = load_u8_to_u16x8(&inp[24..])?;
134    let p4 = load_u8_to_u16x8(&inp[32..])?;
135    let p5 = load_u8_to_u16x8(&inp[40..])?;
136
137    store_u16x8(&mut out[0..], p0 & n)?;
138    store_u16x8(&mut out[8..], p1 & n)?;
139    store_u16x8(&mut out[16..], p2 & n)?;
140    store_u16x8(&mut out[24..], p3 & n)?;
141    store_u16x8(&mut out[32..], p4 & n)?;
142    store_u16x8(&mut out[40..], p5 & n)?;
143
144    store_u16x8(
145        &mut out[48..],
146        ((p0 >> 6u32) & l) | (((p1 >> 6u32) & l) << 2u32) | (((p2 >> 6u32) & l) << 4u32),
147    )?;
148    store_u16x8(
149        &mut out[56..],
150        ((p3 >> 6u32) & l) | (((p4 >> 6u32) & l) << 2u32) | (((p5 >> 6u32) & l) << 4u32),
151    )?;
152
153    Ok(ENCODING_BLOCK_LENGTH[6])
154}
155
156#[inline(always)]
157fn decode8_one(out: &mut [u16], inp: &[u8]) -> Result<usize> {
158    store_u16x8(out, load_u8_to_u16x8(inp)?)?;
159    Ok(8)
160}
161
162#[inline(always)]
163fn decode8(out: &mut [u16], inp: &[u8]) -> Result<usize> {
164    let mut o = 0;
165    for i in (0..64).step_by(8) {
166        o += decode8_one(&mut out[i..], &inp[o..])?;
167    }
168    Ok(o)
169}
170
171#[inline(always)]
172fn decode10(out: &mut [u16], inp: &[u8]) -> Result<usize> {
173    let n = u16x8::splat(0xFF);
174    let l = u16x8::splat(0x03);
175
176    let p0 = load_u8_to_u16x8(inp)?;
177    let p1 = load_u8_to_u16x8(&inp[8..])?;
178    let p2 = load_u8_to_u16x8(&inp[16..])?;
179    let p3 = load_u8_to_u16x8(&inp[24..])?;
180    let p4 = load_u8_to_u16x8(&inp[32..])?;
181    let p5 = load_u8_to_u16x8(&inp[40..])?;
182    let p6 = load_u8_to_u16x8(&inp[48..])?;
183    let p7 = load_u8_to_u16x8(&inp[56..])?;
184    let p8 = load_u8_to_u16x8(&inp[64..])?;
185    let p9 = load_u8_to_u16x8(&inp[72..])?;
186
187    let _r0 = p0 & n;
188    let _r1 = p1 & n;
189    let _r2 = p2 & n;
190    let _r3 = p3 & n;
191
192    store_u16x8(&mut out[0..], _r0 | ((p4 & l) << 8u32))?;
193    store_u16x8(&mut out[8..], _r1 | ((p4 & (l << 2u32)) << 6u32))?;
194    store_u16x8(&mut out[16..], _r2 | ((p4 & (l << 4u32)) << 4u32))?;
195    store_u16x8(&mut out[24..], _r3 | ((p4 & (l << 6u32)) << 2u32))?;
196
197    let _r4 = p5 & n;
198    let _r5 = p6 & n;
199    let _r6 = p7 & n;
200    let _r7 = p8 & n;
201
202    store_u16x8(&mut out[32..], _r4 | ((p9 & l) << 8u32))?;
203    store_u16x8(&mut out[40..], _r5 | ((p9 & (l << 2u32)) << 6u32))?;
204    store_u16x8(&mut out[48..], _r6 | ((p9 & (l << 4u32)) << 4u32))?;
205    store_u16x8(&mut out[56..], _r7 | ((p9 & (l << 6u32)) << 2u32))?;
206
207    Ok(ENCODING_BLOCK_LENGTH[10])
208}
209
210#[inline(always)]
211fn decode16_one(out: &mut [u16], inp: &[u8]) -> Result<usize> {
212    let chunk: &[u8; 16] = inp
213        .get(..16)
214        .ok_or(McRawError::TruncatedData)?
215        .try_into()
216        .map_err(|_| McRawError::TruncatedData)?;
217    let arr = [
218        u16::from_le_bytes([chunk[0], chunk[1]]),
219        u16::from_le_bytes([chunk[2], chunk[3]]),
220        u16::from_le_bytes([chunk[4], chunk[5]]),
221        u16::from_le_bytes([chunk[6], chunk[7]]),
222        u16::from_le_bytes([chunk[8], chunk[9]]),
223        u16::from_le_bytes([chunk[10], chunk[11]]),
224        u16::from_le_bytes([chunk[12], chunk[13]]),
225        u16::from_le_bytes([chunk[14], chunk[15]]),
226    ];
227    store_u16x8(out, u16x8::from(arr))?;
228    Ok(16)
229}
230
231#[inline(always)]
232fn decode16(out: &mut [u16], inp: &[u8]) -> Result<usize> {
233    let mut o = 0;
234    for i in (0..64).step_by(8) {
235        o += decode16_one(&mut out[i..], &inp[o..])?;
236    }
237    Ok(o)
238}
239
240#[inline(always)]
241fn decode_block(out: &mut [u16], bits: u16, inp: &[u8], offset: usize, len: usize) -> Result<usize> {
242    let block_len = ENCODING_BLOCK_LENGTH
243        .get(bits as usize)
244        .copied()
245        .ok_or(McRawError::InvalidCompressionType(bits as i32))?;
246
247    if offset + block_len > len {
248        return Ok(len - offset);
249    }
250
251    let slice = &inp[offset..];
252    match bits {
253        0 => {
254            out[..ENCODING_BLOCK].fill(0);
255            Ok(0)
256        }
257        1 => decode1(out, slice),
258        2 => decode2(out, slice),
259        3 => decode3(out, slice),
260        4 => decode4(out, slice),
261        5 => decode5(out, slice),
262        6 => decode6(out, slice),
263        7 | 8 => decode8(out, slice),
264        9 | 10 => decode10(out, slice),
265        _ => decode16(out, slice),
266    }
267}
268
269fn decode_metadata(inp: &[u8], mut offset: usize, len: usize, out: &mut Vec<u16>) -> Result<usize> {
270    if offset + 4 > len {
271        return Err(McRawError::TruncatedData);
272    }
273
274    let num_blocks = u32::from_le_bytes(read_le_bytes(inp, offset)?) as usize;
275    offset += 4;
276    out.resize(num_blocks, 0);
277
278    let mut ptr = 0;
279    let mut i = 0;
280    while i < num_blocks {
281        let (bits, reference) = decode_header(&inp[offset..])?;
282        offset += HEADER_LENGTH;
283        offset += decode_block(&mut out[ptr..], bits as u16, inp, offset, len)?;
284        for x in 0..ENCODING_BLOCK.min(num_blocks - i) {
285            out[ptr + x] += reference;
286        }
287        ptr += ENCODING_BLOCK;
288        i += ENCODING_BLOCK;
289    }
290    Ok(offset)
291}
292
293/// Decode a raw frame using compression type 7 (current).
294///
295/// Decodes the compressed `input` into `output` (row-major `u16` Bayer data).
296/// Returns the number of `u16` samples written.
297pub fn decode(output: &mut [u16], width: usize, _height: usize, input: &[u8]) -> Result<usize> {
298    if input.len() < METADATA_OFFSET {
299        return Err(McRawError::TruncatedData);
300    }
301
302    let enc_w = u32::from_le_bytes(read_le_bytes(input, 0)?) as usize;
303    let enc_h = u32::from_le_bytes(read_le_bytes(input, 4)?) as usize;
304    let bits_off = u32::from_le_bytes(read_le_bytes(input, 8)?) as usize;
305    let refs_off = u32::from_le_bytes(read_le_bytes(input, 12)?) as usize;
306
307    if bits_off > input.len() || refs_off > input.len() || !enc_w.is_multiple_of(ENCODING_BLOCK) || enc_w < width
308    {
309        return Err(McRawError::DecompressionFailed("invalid raw header".into()));
310    }
311
312    let mut bits = Vec::new();
313    let mut refs = Vec::new();
314    decode_metadata(input, bits_off, input.len(), &mut bits)?;
315    decode_metadata(input, refs_off, input.len(), &mut refs)?;
316
317    let mut offset = METADATA_OFFSET;
318    let mut meta_idx = 0;
319    let mut out_idx = 0;
320
321    let mut row0 = vec![0u16; enc_w];
322    let mut row1 = vec![0u16; enc_w];
323    let mut row2 = vec![0u16; enc_w];
324    let mut row3 = vec![0u16; enc_w];
325
326    for _ in (0..enc_h).step_by(4) {
327        for x in (0..enc_w).step_by(ENCODING_BLOCK) {
328            let b = [bits[meta_idx], bits[meta_idx + 1], bits[meta_idx + 2], bits[meta_idx + 3]];
329            let r = [refs[meta_idx], refs[meta_idx + 1], refs[meta_idx + 2], refs[meta_idx + 3]];
330
331            let mut p0 = [0u16; ENCODING_BLOCK];
332            let mut p1 = [0u16; ENCODING_BLOCK];
333            let mut p2 = [0u16; ENCODING_BLOCK];
334            let mut p3 = [0u16; ENCODING_BLOCK];
335
336            offset += decode_block(&mut p0, b[0], input, offset, input.len())?;
337            offset += decode_block(&mut p1, b[1], input, offset, input.len())?;
338            offset += decode_block(&mut p2, b[2], input, offset, input.len())?;
339            offset += decode_block(&mut p3, b[3], input, offset, input.len())?;
340
341            for i in (0..ENCODING_BLOCK).step_by(2) {
342                row0[x + i] = p0[i / 2] + r[0];
343                row0[x + i + 1] = p1[i / 2] + r[1];
344                row1[x + i] = p2[i / 2] + r[2];
345                row1[x + i + 1] = p3[i / 2] + r[3];
346                row2[x + i] = p0[ENCODING_BLOCK / 2 + i / 2] + r[0];
347                row2[x + i + 1] = p1[ENCODING_BLOCK / 2 + i / 2] + r[1];
348                row3[x + i] = p2[ENCODING_BLOCK / 2 + i / 2] + r[2];
349                row3[x + i + 1] = p3[ENCODING_BLOCK / 2 + i / 2] + r[3];
350            }
351            meta_idx += 4;
352        }
353
354        let c = width.min(enc_w);
355        output[out_idx..out_idx + c].copy_from_slice(&row0[..c]);
356        out_idx += width;
357        output[out_idx..out_idx + c].copy_from_slice(&row1[..c]);
358        out_idx += width;
359        output[out_idx..out_idx + c].copy_from_slice(&row2[..c]);
360        out_idx += width;
361        output[out_idx..out_idx + c].copy_from_slice(&row3[..c]);
362        out_idx += width;
363    }
364    Ok(out_idx)
365}
motioncam_decoder/raw.rs

motioncam_decoder/
raw.rs
