Line data Source code
1 : /**
2 : * @file decompress.c
3 : * @brief CCSDS 124.0-B-1 decompression algorithm implementation.
4 : *
5 : * @cond INTERNAL
6 : * ============================================================================
7 : * _____ ____
8 : * |_ _|_ _ _ __ __ _ __ _ _ __ __ _ / ___| _ __ __ _ ___ ___
9 : * | |/ _` | '_ \ / _` |/ _` | '__/ _` | \___ \| '_ \ / _` |/ __/ _ \
10 : * | | (_| | | | | (_| | (_| | | | (_| | ___) | |_) | (_| | (_| __/
11 : * |_|\__,_|_| |_|\__,_|\__, |_| \__,_| |____/| .__/ \__,_|\___\___|
12 : * |___/ |_|
13 : * ============================================================================
14 : * @endcond
15 : *
16 : * Implements CCSDS 124.0-B-1 decompression (inverse of Section 5.3):
17 : * - Bit reader for parsing compressed packets
18 : * - COUNT and RLE decoding
19 : * - Packet decompression and mask reconstruction
20 : *
21 : * Code follows MISRA C:2012 guidelines.
22 : *
23 : * @authors Georges Labrèche <georges@tanagraspace.com> — https://georges.fyi
24 : * @authors Claude Code (Anthropic) <noreply@anthropic.com>
25 : *
26 : * @see https://ccsds.org/Pubs/124x0b1.pdf CCSDS 124.0-B-1 Standard
27 : */
28 :
29 : #include "ccsds124.h"
30 : #include <string.h>
31 :
32 : /**
33 : * @name Internal Helper Functions
34 : * @{
35 : */
36 :
37 : /**
38 : * @brief Extract positions of all set bits in a bitvector using word-level processing.
39 : *
40 : * Much faster than iterating bit-by-bit when the bitvector is sparse.
41 : * Uses __builtin_clz for O(1) MSB position finding.
42 : *
43 : * @param[in] bv Bitvector to scan
44 : * @param[out] positions Array to store positions (must be at least hamming_weight elements)
45 : * @param[in] max_pos Maximum positions to extract
46 : * @return Number of positions extracted
47 : */
48 291262 : static size_t bitvector_get_set_positions(
49 : const bitvector_t *bv,
50 : size_t *positions,
51 : size_t max_pos
52 : ) {
53 : size_t count = 0U;
54 :
55 : /* Process words in forward order to get positions in ascending order */
56 4777932 : for (size_t word = 0U; (word < bv->num_words) && (count < max_pos); word++) {
57 4486670 : uint32_t word_data = bv->data[word];
58 :
59 46078464 : while ((word_data != 0U) && (count < max_pos)) {
60 : /* Find MSB position using count leading zeros */
61 41591794 : int clz = __builtin_clz(word_data);
62 41591794 : size_t bit_pos_in_word = (size_t)clz;
63 :
64 : /* Global position */
65 41591794 : size_t global_pos = (word * 32U) + bit_pos_in_word;
66 :
67 41591794 : if (global_pos < bv->length) {
68 41591794 : positions[count] = global_pos;
69 41591794 : count++;
70 : }
71 :
72 : /* Clear the MSB we just processed */
73 41591794 : word_data &= ~(1U << (31U - (uint32_t)clz));
74 : }
75 : }
76 :
77 291262 : return count;
78 : }
79 :
80 : /** @} */ /* End of Internal Helper Functions */
81 :
82 : /**
83 : * @name Bit Reader Functions
84 : * @{
85 : */
86 :
87 :
88 2001 : void bitreader_init(bitreader_t *reader, const uint8_t *data, size_t num_bits) {
89 2001 : if (reader != NULL) {
90 2113 : reader->data = data;
91 2113 : reader->num_bits = num_bits;
92 2001 : reader->bit_pos = 0U;
93 : }
94 2001 : }
95 :
96 :
97 7 : int bitreader_read_bit(bitreader_t *reader) {
98 : int result = -1;
99 :
100 48682726 : if ((reader != NULL) && (reader->bit_pos < reader->num_bits)) {
101 49755444 : size_t byte_index = reader->bit_pos / 8U;
102 49755444 : size_t bit_index = reader->bit_pos % 8U;
103 :
104 : /* MSB-first: bit 0 of byte is at position 7 */
105 49755444 : uint8_t shifted = reader->data[byte_index] >> (7U - bit_index);
106 49755444 : uint8_t masked = shifted & 1U;
107 43063991 : result = (int)masked;
108 530000 : reader->bit_pos++;
109 : }
110 :
111 7 : return result;
112 : }
113 :
114 :
115 689852 : uint32_t bitreader_read_bits(bitreader_t *reader, size_t num_bits) {
116 : uint32_t value = 0U;
117 :
118 689852 : if ((reader != NULL) && (num_bits <= 32U)) {
119 : /* Check sufficient bits remain before reading */
120 689852 : if (bitreader_remaining(reader) < num_bits) {
121 : return 0U;
122 : }
123 :
124 4634318 : for (size_t i = 0U; i < num_bits; i++) {
125 : int bit = bitreader_read_bit(reader);
126 : if (bit >= 0) {
127 3944466 : value = (value << 1U) | ((uint32_t)bit & 1U);
128 : }
129 : }
130 : }
131 :
132 : return value;
133 : }
134 :
135 :
136 8 : size_t bitreader_position(const bitreader_t *reader) {
137 : size_t pos = 0U;
138 :
139 8 : if (reader != NULL) {
140 8 : pos = reader->bit_pos;
141 : }
142 :
143 8 : return pos;
144 : }
145 :
146 :
147 1 : size_t bitreader_remaining(const bitreader_t *reader) {
148 : size_t remaining = 0U;
149 :
150 2607412 : if ((reader != NULL) && (reader->bit_pos < reader->num_bits)) {
151 902811 : remaining = reader->num_bits - reader->bit_pos;
152 : }
153 :
154 1 : return remaining;
155 : }
156 :
157 :
158 2 : void bitreader_align_byte(bitreader_t *reader) {
159 2 : if (reader != NULL) {
160 187420 : size_t bit_offset = reader->bit_pos % 8U;
161 187420 : if (bit_offset != 0U) {
162 167529 : reader->bit_pos += (8U - bit_offset);
163 : }
164 : }
165 2 : }
166 :
167 : /** @} */ /* End of Bit Reader Functions */
168 :
169 : /**
170 : * @name Decoding Functions
171 : * @{
172 : */
173 :
174 :
175 1072724 : int ccsds124_count_decode(bitreader_t *reader, uint32_t *value) {
176 : int result = CCSDS124_ERROR_INVALID_ARG;
177 :
178 1072724 : if ((reader == NULL) || (value == NULL)) {
179 : return result;
180 : }
181 :
182 : if (bitreader_remaining(reader) == 0U) {
183 : return CCSDS124_ERROR_UNDERFLOW;
184 : }
185 :
186 : /* Read first bit */
187 : int bit0 = bitreader_read_bit(reader);
188 :
189 1072720 : if (bit0 == 0) {
190 : /* Case 1: '0' → value is 1 */
191 377763 : *value = 1U;
192 : result = CCSDS124_OK;
193 : } else {
194 : /* First bit is 1, read second bit */
195 : int bit1 = bitreader_read_bit(reader);
196 :
197 694957 : if (bit1 == 0) {
198 : /* Case 2: '10' → terminator (value 0) */
199 194538 : *value = 0U;
200 : result = CCSDS124_OK;
201 : } else {
202 : /* First two bits are 11, read third bit */
203 : int bit2 = bitreader_read_bit(reader);
204 :
205 500419 : if (bit2 == 0) {
206 : /* Case 3: '110' + 5 bits → value + 2 */
207 254954 : if (bitreader_remaining(reader) < 5U) {
208 : return CCSDS124_ERROR_UNDERFLOW;
209 : }
210 254953 : uint32_t raw = bitreader_read_bits(reader, 5U);
211 254953 : *value = raw + 2U;
212 : result = CCSDS124_OK;
213 : } else {
214 : /* Case 4: '111' + variable bits
215 : * Need to find the size by counting zeros until a 1 */
216 : size_t size = 0U;
217 : int next_bit = 0;
218 :
219 : /* Count zeros to determine field size */
220 : do {
221 : next_bit = bitreader_read_bit(reader);
222 : if (next_bit < 0) {
223 : return CCSDS124_ERROR_UNDERFLOW;
224 : }
225 684642 : size++;
226 684642 : } while (next_bit == 0);
227 :
228 : /* Size of value field is size + 5 */
229 245463 : size_t value_bits = size + 5U;
230 :
231 : /* Back up one bit since the '1' is part of the value */
232 245463 : reader->bit_pos--;
233 :
234 : /* Check sufficient bits remain for value field */
235 245463 : if (bitreader_remaining(reader) < value_bits) {
236 : return CCSDS124_ERROR_UNDERFLOW;
237 : }
238 :
239 : /* Read the value field */
240 245461 : uint32_t raw = bitreader_read_bits(reader, value_bits);
241 245461 : *value = raw + 2U;
242 : result = CCSDS124_OK;
243 : }
244 : }
245 : }
246 :
247 : return result;
248 : }
249 :
250 :
251 194524 : int ccsds124_rle_decode(bitreader_t *reader, bitvector_t *result, size_t length) {
252 : int status = CCSDS124_ERROR_INVALID_ARG;
253 :
254 194524 : if ((reader == NULL) || (result == NULL)) {
255 : return status;
256 : }
257 :
258 : /* Initialize result to all zeros */
259 194522 : (void)bitvector_init(result, length);
260 194522 : bitvector_zero(result);
261 :
262 : /* Start from end of vector (matching RLE encoding which processes LSB to MSB) */
263 : size_t bit_position = length;
264 :
265 : /* Read COUNT values until terminator */
266 194522 : uint32_t delta = 0U;
267 194522 : status = ccsds124_count_decode(reader, &delta);
268 :
269 1069963 : while ((status == CCSDS124_OK) && (delta != 0U)) {
270 : /* Delta represents (count of zeros + 1) */
271 875445 : if (delta > bit_position) {
272 : /* Invalid: delta exceeds remaining positions (v1.6/v1.7/v1.8) */
273 : return CCSDS124_ERROR_OVERFLOW;
274 : }
275 875441 : bit_position -= delta;
276 : /* Set the bit at this position */
277 : bitvector_set_bit(result, bit_position, 1);
278 :
279 : /* Read next delta */
280 875441 : status = ccsds124_count_decode(reader, &delta);
281 : }
282 :
283 : return status;
284 : }
285 :
286 :
287 186739 : int ccsds124_bit_insert(bitreader_t *reader, bitvector_t *data, const bitvector_t *mask) {
288 : int status = CCSDS124_ERROR_INVALID_ARG;
289 :
290 186739 : if ((reader == NULL) || (data == NULL) || (mask == NULL)) {
291 : return status;
292 : }
293 :
294 186736 : if (data->length != mask->length) {
295 : return status;
296 : }
297 :
298 186735 : size_t hamming = bitvector_hamming_weight(mask);
299 :
300 186735 : if (hamming == 0U) {
301 : /* No bits to insert */
302 : return CCSDS124_OK;
303 : }
304 :
305 : /* Check sufficient bits remain before reading */
306 186029 : if (bitreader_remaining(reader) < hamming) {
307 : return CCSDS124_ERROR_UNDERFLOW;
308 : }
309 :
310 : /* Collect positions of '1' bits in mask using word-level processing */
311 : size_t positions[CCSDS124_MAX_PACKET_LENGTH];
312 186029 : size_t pos_count = bitvector_get_set_positions(mask, positions, hamming);
313 :
314 : /* Insert bits in reverse order (matching BE extraction) */
315 41049350 : for (size_t i = pos_count; i > 0U; i--) {
316 : int bit = bitreader_read_bit(reader);
317 : if (bit < 0) {
318 : return CCSDS124_ERROR_UNDERFLOW;
319 : }
320 40863321 : bitvector_set_bit(data, positions[i - 1U], bit);
321 : }
322 :
323 : status = CCSDS124_OK;
324 : return status;
325 : }
326 :
327 : /** @} */ /* End of Decoding Functions */
328 :
329 : /**
330 : * @name Decompressor Initialization
331 : * @{
332 : */
333 :
334 :
335 178 : int ccsds124_decompressor_init(
336 : ccsds124_decompressor_t *decomp,
337 : size_t F,
338 : const bitvector_t *initial_mask,
339 : uint8_t robustness
340 : ) {
341 178 : if (decomp == NULL) {
342 : return CCSDS124_ERROR_INVALID_ARG;
343 : }
344 :
345 177 : if ((F == 0U) || (F > (size_t)CCSDS124_MAX_PACKET_LENGTH)) {
346 : return CCSDS124_ERROR_INVALID_ARG;
347 : }
348 :
349 173 : if (robustness > (uint8_t)CCSDS124_MAX_ROBUSTNESS) {
350 : return CCSDS124_ERROR_INVALID_ARG;
351 : }
352 :
353 : /* Store configuration */
354 171 : decomp->F = F;
355 171 : decomp->robustness = robustness;
356 :
357 : /* Initialize bit vectors */
358 171 : (void)bitvector_init(&decomp->mask, F);
359 171 : (void)bitvector_init(&decomp->initial_mask, F);
360 171 : (void)bitvector_init(&decomp->prev_output, F);
361 171 : (void)bitvector_init(&decomp->Xt, F);
362 :
363 : /* Set initial mask */
364 171 : if (initial_mask != NULL) {
365 1 : bitvector_copy(&decomp->initial_mask, initial_mask);
366 1 : bitvector_copy(&decomp->mask, initial_mask);
367 : } else {
368 170 : bitvector_zero(&decomp->initial_mask);
369 170 : bitvector_zero(&decomp->mask);
370 : }
371 :
372 : /* Initialize diagnostics */
373 171 : decomp->mask_inconsistent = 0U;
374 171 : decomp->count_f_mismatch = 0U;
375 :
376 : /* Initialize accuracy guarantee tracking */
377 171 : decomp->mask_synced = 0U;
378 171 : decomp->received_status_count = 0U;
379 171 : decomp->received_status_index = 0U;
380 171 : (void)memset(decomp->received_status_ring, 0, sizeof(decomp->received_status_ring));
381 :
382 : /* Reset state */
383 171 : ccsds124_decompressor_reset(decomp);
384 :
385 171 : return CCSDS124_OK;
386 : }
387 :
388 :
389 234 : void ccsds124_decompressor_reset(ccsds124_decompressor_t *decomp) {
390 234 : if (decomp != NULL) {
391 234 : decomp->t = 0U;
392 234 : bitvector_copy(&decomp->mask, &decomp->initial_mask);
393 234 : bitvector_zero(&decomp->prev_output);
394 234 : bitvector_zero(&decomp->Xt);
395 :
396 : /* Reset diagnostics */
397 234 : decomp->mask_inconsistent = 0U;
398 234 : decomp->count_f_mismatch = 0U;
399 :
400 : /* Reset accuracy guarantee tracking */
401 234 : decomp->mask_synced = 0U;
402 234 : decomp->received_status_count = 0U;
403 234 : decomp->received_status_index = 0U;
404 234 : (void)memset(decomp->received_status_ring, 0, sizeof(decomp->received_status_ring));
405 : }
406 234 : }
407 :
408 :
409 56 : int ccsds124_decompressor_notify_packet_loss(
410 : ccsds124_decompressor_t *decomp,
411 : uint32_t lost_count
412 : ) {
413 56 : if (decomp == NULL) {
414 : return CCSDS124_ERROR_INVALID_ARG;
415 : }
416 :
417 56 : if (lost_count == 0U) {
418 : return CCSDS124_OK; /* No loss, nothing to do */
419 : }
420 :
421 : /*
422 : * Advance the time index to account for lost packets.
423 : * This is critical for synchronization with the compressor's time index.
424 : *
425 : * Note: After packet loss, the next packet should ideally have:
426 : * - rt=1 (uncompressed) for full recovery, OR
427 : * - ft=1 (full mask) for mask recovery
428 : *
429 : * If the loss count <= R (minimum robustness), the next packet's Xt
430 : * window will contain all mask changes, allowing mask synchronization.
431 : * However, prev_output will be stale unless the next packet is uncompressed.
432 : */
433 56 : decomp->t += lost_count;
434 :
435 : /*
436 : * Mark prev_output as potentially invalid.
437 : * The next packet's data should be uncompressed (rt=1) for reliable recovery,
438 : * or we accept that prediction-based decompression may fail.
439 : *
440 : * Per CCSDS 124.0-B-1 Section 3.3.2: "The uncompressed flag, rt, which
441 : * shall be rt = 1 if t <= Rt" - this ensures the first R+1 packets are
442 : * always uncompressed, providing natural synchronization points.
443 : */
444 :
445 : /* Packet loss breaks mask synchronization */
446 56 : decomp->mask_synced = 0U;
447 :
448 : /* Record 0x02 (lost) status entries in the ring buffer */
449 254 : for (uint32_t i = 0U; i < lost_count; i++) {
450 198 : decomp->received_status_ring[decomp->received_status_index] = 0x02U;
451 198 : decomp->received_status_index = (decomp->received_status_index + 1U) % CCSDS124_MAX_VT_HISTORY;
452 198 : if (decomp->received_status_count < CCSDS124_MAX_VT_HISTORY) {
453 194 : decomp->received_status_count++;
454 : }
455 : }
456 :
457 : return CCSDS124_OK;
458 : }
459 :
460 : /** @} */ /* End of Decompressor Initialization */
461 :
462 : /**
463 : * @name Packet Decompression
464 : * @{
465 : */
466 :
467 : /**
468 : * @brief Internal flags extracted during decompression.
469 : *
470 : * Used by ccsds124_decompress_packet_checked() to make accuracy
471 : * guarantee decisions without re-parsing the bitstream.
472 : */
473 : typedef struct {
474 : uint8_t Vt; /**< Effective robustness (0-15) */
475 : uint8_t ft; /**< Send mask flag (0 or 1) */
476 : uint8_t rt; /**< Reference/uncompressed flag (0 or 1) */
477 : } ccsds124_decompress_flags_t;
478 :
479 : /**
480 : * @brief Internal decompression with optional flag extraction.
481 : *
482 : * Core decompression logic shared by both ccsds124_decompress_packet()
483 : * and ccsds124_decompress_packet_checked().
484 : *
485 : * @param[in,out] decomp Decompressor state
486 : * @param[in,out] reader Bit reader
487 : * @param[out] output Decompressed output
488 : * @param[out] flags Optional extracted flags (NULL to skip)
489 : * @return CCSDS124_OK or negative error code
490 : */
491 189432 : static int ccsds124_decompress_packet_internal(
492 : ccsds124_decompressor_t *decomp,
493 : bitreader_t *reader,
494 : bitvector_t *output,
495 : ccsds124_decompress_flags_t *flags
496 : ) {
497 189432 : if ((decomp == NULL) || (reader == NULL) || (output == NULL)) {
498 : return CCSDS124_ERROR_INVALID_ARG;
499 : }
500 :
501 189429 : (void)bitvector_init(output, decomp->F);
502 :
503 : /* Copy previous output as prediction base */
504 189429 : bitvector_copy(output, &decomp->prev_output);
505 :
506 : /* Clear positive changes tracker */
507 189429 : bitvector_zero(&decomp->Xt);
508 :
509 : /* ====================================================================
510 : * Parse hₜ: Mask change information
511 : * hₜ = RLE(Xₜ) ∥ BIT₄(Vₜ) ∥ eₜ ∥ kₜ ∥ cₜ ∥ ḋₜ
512 : * ==================================================================== */
513 :
514 : /* Decode RLE(Xₜ) - mask changes */
515 : bitvector_t Xt;
516 189429 : int status = ccsds124_rle_decode(reader, &Xt, decomp->F);
517 189429 : if (status != CCSDS124_OK) {
518 : return status;
519 : }
520 :
521 : /* Read BIT₄(Vₜ) - effective robustness */
522 189423 : if (bitreader_remaining(reader) < 4U) {
523 : return CCSDS124_ERROR_UNDERFLOW;
524 : }
525 189423 : uint32_t vt_raw = bitreader_read_bits(reader, 4U);
526 189423 : uint8_t Vt = (uint8_t)(vt_raw & 0x0FU);
527 :
528 : /* Process eₜ, kₜ, cₜ if Vₜ > 0 and there are changes */
529 : int ct = 0;
530 189423 : size_t change_count = bitvector_hamming_weight(&Xt);
531 :
532 189423 : if ((Vt > 0U) && (change_count > 0U)) {
533 : /* Read eₜ */
534 : int et = bitreader_read_bit(reader);
535 : if (et < 0) {
536 0 : return CCSDS124_ERROR_UNDERFLOW;
537 : }
538 :
539 : /* Pre-extract positions of set bits in Xt (word-level, much faster than bit-by-bit) */
540 : size_t change_positions[CCSDS124_MAX_PACKET_LENGTH];
541 105206 : size_t num_changes = bitvector_get_set_positions(&Xt, change_positions, change_count);
542 :
543 105206 : if (et == 1) {
544 : /* Read kₜ - determines positive/negative updates */
545 : /* kₜ has one bit per change in Xt */
546 : uint8_t kt_bits[CCSDS124_MAX_PACKET_LENGTH];
547 :
548 : /* Read kt bits using pre-extracted positions */
549 24231 : if (bitreader_remaining(reader) < num_changes) {
550 0 : return CCSDS124_ERROR_UNDERFLOW;
551 : }
552 441286 : for (size_t idx = 0U; idx < num_changes; idx++) {
553 : int bit_val = bitreader_read_bit(reader);
554 : if (bit_val < 0) {
555 : return CCSDS124_ERROR_UNDERFLOW;
556 : }
557 487199 : kt_bits[idx] = (bit_val > 0) ? 1U : 0U;
558 : }
559 :
560 : /* Apply mask updates using pre-extracted positions */
561 441286 : for (size_t idx = 0U; idx < num_changes; idx++) {
562 417055 : size_t pos = change_positions[idx];
563 : /* kt=1 means positive update (mask becomes 0) */
564 : /* kt=0 means negative update (mask becomes 1) */
565 417055 : if (kt_bits[idx] != 0U) {
566 : bitvector_set_bit(&decomp->mask, pos, 0);
567 : bitvector_set_bit(&decomp->Xt, pos, 1); /* Track positive change */
568 : } else {
569 : bitvector_set_bit(&decomp->mask, pos, 1);
570 : }
571 : }
572 :
573 : /* Read cₜ */
574 : ct = bitreader_read_bit(reader);
575 : if (ct < 0) {
576 : return CCSDS124_ERROR_UNDERFLOW;
577 : }
578 : } else {
579 : /* et = 0: all updates are negative (mask bits become 1) */
580 392222 : for (size_t idx = 0U; idx < num_changes; idx++) {
581 311247 : bitvector_set_bit(&decomp->mask, change_positions[idx], 1);
582 : }
583 : }
584 84217 : } else if ((Vt == 0U) && (change_count > 0U)) {
585 : /* Vt = 0: toggle mask bits at change positions */
586 : /* Pre-extract positions of set bits in Xt */
587 : size_t change_positions[CCSDS124_MAX_PACKET_LENGTH];
588 27 : size_t num_changes = bitvector_get_set_positions(&Xt, change_positions, change_count);
589 :
590 198 : for (size_t idx = 0U; idx < num_changes; idx++) {
591 171 : size_t pos = change_positions[idx];
592 : int current_val = bitvector_get_bit(&decomp->mask, pos);
593 : int toggled = 0;
594 : if (current_val == 0) {
595 : toggled = 1;
596 : }
597 171 : bitvector_set_bit(&decomp->mask, pos, toggled);
598 : }
599 : } else {
600 : /* No changes to apply (change_count == 0) */
601 : }
602 :
603 : /* Read ḋₜ */
604 : int dt = bitreader_read_bit(reader);
605 : if (dt < 0) {
606 : return CCSDS124_ERROR_UNDERFLOW;
607 : }
608 :
609 : /* ====================================================================
610 : * Parse qₜ: Optional full mask
611 : * ==================================================================== */
612 :
613 : int ft = 0;
614 : int rt = 0;
615 :
616 : /* Reset diagnostics flags */
617 189423 : decomp->mask_inconsistent = 0U;
618 189423 : decomp->count_f_mismatch = 0U;
619 :
620 : /* dt=1 means both ft=0 and rt=0 (optimization per CCSDS Eq. 13) */
621 : /* dt=0 means we need to read ft and rt from the stream */
622 :
623 189423 : if (dt == 0) {
624 : /* Read ft flag */
625 : ft = bitreader_read_bit(reader);
626 : if (ft < 0) {
627 : return CCSDS124_ERROR_UNDERFLOW;
628 : }
629 :
630 5343 : if (ft == 1) {
631 : /* Save delta-updated mask before full mask replacement (v1.11) */
632 : bitvector_t delta_mask;
633 5085 : bitvector_copy(&delta_mask, &decomp->mask);
634 :
635 : /* Full mask follows: decode RLE(M XOR (M<<)) */
636 : bitvector_t mask_diff;
637 5085 : status = ccsds124_rle_decode(reader, &mask_diff, decomp->F);
638 5085 : if (status != CCSDS124_OK) {
639 0 : return status;
640 : }
641 :
642 : /* Reverse the horizontal XOR to get the actual mask.
643 : * HXOR encoding: HXOR[i] = M[i] XOR M[i+1], with HXOR[F-1] = M[F-1]
644 : * Reversal: start from LSB (position F-1) and work towards MSB (position 0)
645 : * M[F-1] = HXOR[F-1] (just copy)
646 : * M[i] = HXOR[i] XOR M[i+1] for i < F-1
647 : */
648 :
649 : /* Copy LSB bit directly (position F-1 in bitvector) */
650 5085 : int current = bitvector_get_bit(&mask_diff, decomp->F - 1U);
651 5085 : bitvector_set_bit(&decomp->mask, decomp->F - 1U, current);
652 :
653 : /* Process remaining bits from F-2 down to 0 */
654 2474496 : for (size_t i = decomp->F - 1U; i > 0U; i--) {
655 2469411 : size_t pos = i - 1U;
656 : int hxor_bit = bitvector_get_bit(&mask_diff, pos);
657 : /* M[pos] = HXOR[pos] XOR M[pos+1] = HXOR[pos] XOR current */
658 2469411 : current = hxor_bit ^ current;
659 2469411 : bitvector_set_bit(&decomp->mask, pos, current);
660 : }
661 :
662 : /* Check delta/full mask consistency (v1.11) */
663 5085 : if (bitvector_equals(&delta_mask, &decomp->mask) == 0) {
664 3 : decomp->mask_inconsistent = 1U;
665 : }
666 :
667 : }
668 :
669 : /* Read rt flag */
670 : rt = bitreader_read_bit(reader);
671 : if (rt < 0) {
672 : return CCSDS124_ERROR_UNDERFLOW;
673 : }
674 : }
675 :
676 : /* Populate flags if requested */
677 189423 : if (flags != NULL) {
678 33 : flags->Vt = Vt;
679 33 : flags->ft = (ft != 0) ? 1U : 0U;
680 33 : flags->rt = (rt != 0) ? 1U : 0U;
681 : }
682 :
683 189413 : if (rt == 1) {
684 : /* Full packet follows: COUNT(F) ∥ Iₜ */
685 2688 : uint32_t packet_length = 0U;
686 2688 : status = ccsds124_count_decode(reader, &packet_length);
687 2688 : if (status != CCSDS124_OK) {
688 0 : return status;
689 : }
690 :
691 : /* Check COUNT(F) against expected packet length (diagnostic flag).
692 : * A mismatch indicates corruption but we still attempt decompression
693 : * so the harness can decide whether to accept or reject. */
694 2688 : if (packet_length != (uint32_t)decomp->F) {
695 0 : decomp->count_f_mismatch = 1U;
696 : }
697 :
698 : /* Read full packet */
699 2688 : if (bitreader_remaining(reader) < decomp->F) {
700 : return CCSDS124_ERROR_UNDERFLOW;
701 : }
702 1250960 : for (size_t i = 0U; i < decomp->F; i++) {
703 : int bit = bitreader_read_bit(reader);
704 : if (bit < 0) {
705 : return CCSDS124_ERROR_UNDERFLOW;
706 : }
707 1248272 : bitvector_set_bit(output, i, bit);
708 : }
709 : } else {
710 : /* Compressed: extract unpredictable bits */
711 : bitvector_t extraction_mask;
712 186735 : (void)bitvector_init(&extraction_mask, decomp->F);
713 :
714 186735 : if ((ct == 1) && (Vt > 0U)) {
715 : /* BE(Iₜ, (Xₜ OR Mₜ)) */
716 28 : bitvector_or(&extraction_mask, &decomp->mask, &decomp->Xt);
717 : } else {
718 : /* BE(Iₜ, Mₜ) */
719 186707 : bitvector_copy(&extraction_mask, &decomp->mask);
720 : }
721 :
722 : /* Insert unpredictable bits */
723 186735 : status = ccsds124_bit_insert(reader, output, &extraction_mask);
724 186735 : if (status != CCSDS124_OK) {
725 0 : return status;
726 : }
727 : }
728 :
729 : /* ====================================================================
730 : * Update state for next cycle
731 : * ==================================================================== */
732 :
733 189423 : bitvector_copy(&decomp->prev_output, output);
734 189423 : decomp->t++;
735 :
736 189423 : return CCSDS124_OK;
737 : }
738 :
739 :
740 1972 : int ccsds124_decompress_packet(
741 : ccsds124_decompressor_t *decomp,
742 : bitreader_t *reader,
743 : bitvector_t *output
744 : ) {
745 189395 : return ccsds124_decompress_packet_internal(decomp, reader, output, NULL);
746 : }
747 :
748 :
749 41 : int ccsds124_decompress_packet_checked(
750 : ccsds124_decompressor_t *decomp,
751 : const uint8_t *data,
752 : size_t num_bits,
753 : bitvector_t *output,
754 : ccsds124_decompress_result_t *result
755 : ) {
756 41 : if ((decomp == NULL) || (data == NULL) || (output == NULL)) {
757 : return CCSDS124_ERROR_INVALID_ARG;
758 : }
759 :
760 38 : if (num_bits == 0U) {
761 : return CCSDS124_ERROR_INVALID_ARG;
762 : }
763 :
764 : /* Save decompressor state before attempting decompression.
765 : * Per cross-validation v1.9: restore state if packet is invalid
766 : * to avoid propagating errors to subsequent packets. */
767 : ccsds124_decompressor_t saved_decomp;
768 : (void)memcpy(&saved_decomp, decomp, sizeof(*decomp));
769 :
770 : /* Create bit reader and decompress with flag extraction */
771 : bitreader_t reader;
772 : bitreader_init(&reader, data, num_bits);
773 :
774 : ccsds124_decompress_flags_t flags;
775 37 : int rc = ccsds124_decompress_packet_internal(decomp, &reader, output, &flags);
776 :
777 : /* Validate: only padding bits should remain (at most 7) (v1.10).
778 : * Reference packets (rt=1) are exempt: they are self-delimiting via
779 : * COUNT(F), and per the cross-validation rules a Received Packet
780 : * Length larger than the bits actually needed means the remainder is
781 : * simply ignored. */
782 38 : if ((rc == CCSDS124_OK) && (flags.rt == 0U) &&
783 : (bitreader_remaining(&reader) >= 8U)) {
784 : rc = CCSDS124_ERROR_OVERFLOW;
785 : }
786 :
787 : if (rc != CCSDS124_OK) {
788 : /* Decompression failed: restore state */
789 : (void)memcpy(decomp, &saved_decomp, sizeof(*decomp));
790 5 : decomp->mask_synced = 0U;
791 :
792 : /* Record 0x01 in status ring */
793 5 : decomp->received_status_ring[decomp->received_status_index] = 0x01U;
794 5 : decomp->received_status_index = (decomp->received_status_index + 1U) % CCSDS124_MAX_VT_HISTORY;
795 5 : if (decomp->received_status_count < CCSDS124_MAX_VT_HISTORY) {
796 5 : decomp->received_status_count++;
797 : }
798 :
799 5 : if (result != NULL) {
800 4 : result->status = 0x01U;
801 4 : result->Vt = 0U;
802 4 : result->ft = 0U;
803 4 : result->rt = 0U;
804 : }
805 5 : return rc;
806 : }
807 :
808 : /* Decompression succeeded — evaluate accuracy guarantee decision tree */
809 32 : uint8_t mask_inconsistent_detected = ((decomp->mask_synced != 0U) &&
810 15 : (decomp->mask_inconsistent != 0U)) ? 1U : 0U;
811 32 : uint8_t count_f_mismatch_detected = (decomp->count_f_mismatch != 0U) ? 1U : 0U;
812 :
813 : uint8_t guaranteed;
814 32 : if (mask_inconsistent_detected != 0U) {
815 : guaranteed = 0U;
816 32 : } else if (count_f_mismatch_detected != 0U) {
817 : guaranteed = 0U;
818 32 : } else if (flags.rt == 1U) {
819 : /* Reference packet: guaranteed if mask is synced or ft=1 resynchronizes */
820 23 : if ((decomp->mask_synced != 0U) || (flags.ft == 1U)) {
821 : guaranteed = 1U;
822 : } else {
823 : guaranteed = 0U;
824 : }
825 : } else {
826 : /* Non-reference packet: check preceding Vt received packets.
827 : * Skip lost packets (0x02) in the window. */
828 : guaranteed = 1U;
829 9 : if (flags.Vt > 0U) {
830 : size_t checked = 0U;
831 : /* Walk backwards through the ring buffer (before current entry) */
832 8 : size_t ring_walk = decomp->received_status_count;
833 8 : size_t idx = (decomp->received_status_index + CCSDS124_MAX_VT_HISTORY - 1U) % CCSDS124_MAX_VT_HISTORY;
834 :
835 27 : while ((checked < (size_t)flags.Vt) && (ring_walk > 0U)) {
836 19 : uint8_t st = decomp->received_status_ring[idx];
837 19 : if (st == 0x02U) {
838 : /* Skip lost packets */
839 1 : idx = (idx + CCSDS124_MAX_VT_HISTORY - 1U) % CCSDS124_MAX_VT_HISTORY;
840 1 : ring_walk--;
841 1 : continue;
842 : }
843 18 : if (st != 0x00U) {
844 : guaranteed = 0U;
845 : break;
846 : }
847 18 : checked++;
848 18 : idx = (idx + CCSDS124_MAX_VT_HISTORY - 1U) % CCSDS124_MAX_VT_HISTORY;
849 18 : ring_walk--;
850 : }
851 : /* If we checked fewer than Vt received packets because history
852 : * is too short, trust the guarantee (early packets are reliable). */
853 : }
854 : }
855 :
856 : /* Apply state decisions based on guarantee result */
857 : uint8_t out_status;
858 : int ret;
859 :
860 8 : if (guaranteed != 0U) {
861 : out_status = 0x00U;
862 : ret = CCSDS124_OK;
863 :
864 : /* ft=1 resynchronizes the mask */
865 28 : if (flags.ft == 1U) {
866 14 : decomp->mask_synced = 1U;
867 : }
868 4 : } else if (mask_inconsistent_detected != 0U) {
869 : /* Mask inconsistency while synced: restore state, clear sync */
870 : (void)memcpy(decomp, &saved_decomp, sizeof(*decomp));
871 0 : decomp->mask_synced = 0U;
872 : out_status = 0x01U;
873 : ret = CCSDS124_STATUS_UNGUARANTEED;
874 4 : } else if (count_f_mismatch_detected != 0U) {
875 : /* COUNT(F) mismatch: keep state (ft=1 still syncs mask) */
876 : out_status = 0x01U;
877 : ret = CCSDS124_STATUS_UNGUARANTEED;
878 0 : if (flags.ft == 1U) {
879 0 : decomp->mask_synced = 1U;
880 : }
881 : } else {
882 : /* Unguaranteed for other reasons: restore state, clear sync */
883 : (void)memcpy(decomp, &saved_decomp, sizeof(*decomp));
884 4 : decomp->mask_synced = 0U;
885 : out_status = 0x01U;
886 : ret = CCSDS124_STATUS_UNGUARANTEED;
887 : }
888 :
889 : /* Record status in ring buffer */
890 32 : decomp->received_status_ring[decomp->received_status_index] = out_status;
891 32 : decomp->received_status_index = (decomp->received_status_index + 1U) % CCSDS124_MAX_VT_HISTORY;
892 32 : if (decomp->received_status_count < CCSDS124_MAX_VT_HISTORY) {
893 32 : decomp->received_status_count++;
894 : }
895 :
896 : /* Populate result struct if requested */
897 32 : if (result != NULL) {
898 24 : result->status = out_status;
899 24 : result->Vt = flags.Vt;
900 24 : result->ft = flags.ft;
901 24 : result->rt = flags.rt;
902 : }
903 :
904 : return ret;
905 : }
906 :
907 :
908 : /**
909 : * @brief Skip COUNT values in an RLE sequence until the terminator.
910 : *
911 : * Reads and discards COUNT values from the reader until a terminator
912 : * (COUNT value 0) is found. Returns the hamming weight (number of
913 : * non-terminator values decoded) via output parameter.
914 : *
915 : * @param[in,out] reader Bit reader
916 : * @param[out] hamming_weight Number of non-terminator COUNTs decoded
917 : * @return CCSDS124_OK on success, error code on decode failure
918 : */
919 : /**
920 : * @brief Skip an RLE-encoded sequence, tracking hamming weight and span.
921 : *
922 : * The span is the sum of all RLE deltas, which equals one plus the highest
923 : * '1' position encoded. An RLE sequence for a vector of length F must have
924 : * span <= F — a larger span means the h or q vector is inconsistent with F
925 : * (cross-validation rule v1.6).
926 : *
927 : * @param[in,out] reader Bit reader
928 : * @param[out] hamming_weight Number of non-terminator COUNTs decoded
929 : * @param[out] span Sum of decoded deltas (1 + highest position)
930 : * @return CCSDS124_OK on success, error code on decode failure
931 : */
932 23 : static int skip_rle_sequence_span(bitreader_t *reader, uint32_t *hamming_weight,
933 : uint64_t *span) {
934 : uint32_t hw = 0U;
935 : uint64_t total = 0U;
936 23 : uint32_t count_val = 0U;
937 23 : int rc = ccsds124_count_decode(reader, &count_val);
938 :
939 46 : while ((rc == CCSDS124_OK) && (count_val != 0U)) {
940 23 : hw++;
941 23 : total += (uint64_t)count_val;
942 23 : rc = ccsds124_count_decode(reader, &count_val);
943 : }
944 :
945 23 : if (rc == CCSDS124_OK) {
946 22 : *hamming_weight = hw;
947 22 : *span = total;
948 : }
949 :
950 23 : return rc;
951 : }
952 :
953 :
954 17 : int ccsds124_discover_packet_length(
955 : const uint8_t *data,
956 : size_t num_bits,
957 : uint32_t *packet_length
958 : ) {
959 17 : if ((data == NULL) || (packet_length == NULL)) {
960 : return CCSDS124_ERROR_INVALID_ARG;
961 : }
962 :
963 15 : if (num_bits == 0U) {
964 : return CCSDS124_ERROR_INVALID_ARG;
965 : }
966 :
967 : /* Default: not discoverable from this packet */
968 14 : *packet_length = 0U;
969 :
970 : bitreader_t reader;
971 : bitreader_init(&reader, data, num_bits);
972 :
973 : /* 1. Skip RLE(Xt) — self-delimiting, doesn't need F */
974 14 : uint32_t H_Xt = 0U;
975 14 : uint64_t Xt_span = 0U;
976 14 : if (skip_rle_sequence_span(&reader, &H_Xt, &Xt_span) != CCSDS124_OK) {
977 : return CCSDS124_OK; /* Parse error — not discoverable */
978 : }
979 :
980 : /* 2. BIT4(Vt) — 4 bits */
981 13 : if (bitreader_remaining(&reader) < 4U) {
982 : return CCSDS124_OK;
983 : }
984 13 : uint32_t Vt = bitreader_read_bits(&reader, 4U);
985 :
986 : /* 3. If H(Xt) > 0 and Vt > 0: read et, then kt+ct only if et==1 */
987 13 : if ((H_Xt > 0U) && (Vt > 0U)) {
988 : int et = bitreader_read_bit(&reader);
989 : if (et < 0) {
990 : return CCSDS124_OK;
991 : }
992 4 : if (et == 1) {
993 : /* kt: H(Xt) bits — skip them */
994 0 : if (bitreader_remaining(&reader) < (size_t)H_Xt) {
995 : return CCSDS124_OK;
996 : }
997 0 : for (uint32_t i = 0U; i < H_Xt; i++) {
998 : (void)bitreader_read_bit(&reader);
999 : }
1000 : /* ct: 1 bit */
1001 : if (bitreader_read_bit(&reader) < 0) {
1002 : return CCSDS124_OK;
1003 : }
1004 : }
1005 : /* et==0: all changes are negative — no kt or ct in bitstream */
1006 : }
1007 : /* If H(Xt) > 0 and Vt == 0: no et/kt/ct (toggle mode) */
1008 :
1009 : /* 4. dt — 1 bit */
1010 : int dt = bitreader_read_bit(&reader);
1011 : if (dt < 0) {
1012 : return CCSDS124_OK;
1013 : }
1014 :
1015 13 : if (dt == 1) {
1016 : /* dt=1 means ft=0 and rt=0 — can't discover F */
1017 : return CCSDS124_OK;
1018 : }
1019 :
1020 : /* 5. dt=0: read ft flag */
1021 : int ft = bitreader_read_bit(&reader);
1022 : if (ft < 0) {
1023 : return CCSDS124_OK;
1024 : }
1025 :
1026 11 : uint64_t mask_span = 0U;
1027 11 : if (ft == 1) {
1028 : /* Full mask follows as RLE — skip it */
1029 : uint32_t mask_hw = 0U;
1030 9 : if (skip_rle_sequence_span(&reader, &mask_hw, &mask_span) != CCSDS124_OK) {
1031 0 : return CCSDS124_OK;
1032 : }
1033 : }
1034 :
1035 : /* Read rt flag */
1036 : int rt = bitreader_read_bit(&reader);
1037 : if (rt < 0) {
1038 : return CCSDS124_OK;
1039 : }
1040 :
1041 11 : if (rt != 1) {
1042 : /* Not a reference packet — can't discover F */
1043 : return CCSDS124_OK;
1044 : }
1045 :
1046 : /* 6. rt=1: read COUNT(F) */
1047 10 : uint32_t discovered_F = 0U;
1048 10 : int rc = ccsds124_count_decode(&reader, &discovered_F);
1049 10 : if ((rc != CCSDS124_OK) || (discovered_F == 0U)) {
1050 : return CCSDS124_OK;
1051 : }
1052 :
1053 : /* Validity (cross-validation rule v1.6): the signaled length must be
1054 : * within the standard's range (1..65535), and the packet's own h and q
1055 : * vectors must be consistent with it — an RLE span exceeding F encodes
1056 : * positions beyond the packet, making the signaled length
1057 : * untrustworthy. */
1058 10 : if ((discovered_F > 65535U) ||
1059 10 : (Xt_span > (uint64_t)discovered_F) ||
1060 10 : (mask_span > (uint64_t)discovered_F)) {
1061 : return CCSDS124_OK;
1062 : }
1063 :
1064 : /* Truncated reference packet: the bitstream ran out after COUNT(F) but
1065 : * before the full I_t. Per the cross-validation rules the signaled
1066 : * length is still to be considered ("stored as the actual packet length
1067 : * if it is valid and was not known before"), but it is weaker evidence
1068 : * than a fully-validated reference packet — report it distinctly so
1069 : * callers can prefer a strict discovery elsewhere in the stream. */
1070 10 : if (bitreader_remaining(&reader) < (size_t)discovered_F) {
1071 1 : *packet_length = discovered_F;
1072 1 : return CCSDS124_STATUS_TRUNCATED_LENGTH;
1073 : }
1074 :
1075 : /* Excess bits after I_t are ignored: reference packets are
1076 : * self-delimiting via COUNT(F), and a Received Packet Length larger
1077 : * than the bits actually needed means the remainder is ignored. */
1078 9 : *packet_length = discovered_F;
1079 9 : return CCSDS124_OK;
1080 : }
1081 :
1082 :
1083 66 : int ccsds124_decompress(
1084 : ccsds124_decompressor_t *decomp,
1085 : const uint8_t *input_data,
1086 : size_t input_size,
1087 : uint8_t *output_buffer,
1088 : size_t output_buffer_size,
1089 : size_t *output_size
1090 : ) {
1091 66 : if ((decomp == NULL) || (input_data == NULL) ||
1092 63 : (output_buffer == NULL) || (output_size == NULL)) {
1093 : return CCSDS124_ERROR_INVALID_ARG;
1094 : }
1095 :
1096 : /* Reset decompressor */
1097 61 : ccsds124_decompressor_reset(decomp);
1098 :
1099 : /* Initialize bit reader */
1100 : bitreader_t reader;
1101 61 : bitreader_init(&reader, input_data, input_size * 8U);
1102 :
1103 : /* Output packet size in bytes */
1104 61 : size_t packet_bytes = (decomp->F + 7U) / 8U;
1105 : size_t total_output = 0U;
1106 :
1107 : /* Decompress packets until input exhausted */
1108 61 : while (bitreader_remaining(&reader) > 0U) {
1109 : bitvector_t output;
1110 : int status = ccsds124_decompress_packet(decomp, &reader, &output);
1111 187423 : if (status != CCSDS124_OK) {
1112 5 : return status;
1113 : }
1114 :
1115 : /* Check output buffer space */
1116 187421 : if ((total_output + packet_bytes) > output_buffer_size) {
1117 : return CCSDS124_ERROR_OVERFLOW;
1118 : }
1119 :
1120 : /* Copy to output buffer */
1121 187418 : (void)bitvector_to_bytes(&output, &output_buffer[total_output], packet_bytes);
1122 : total_output += packet_bytes;
1123 :
1124 : /* Align to byte boundary for next packet */
1125 : bitreader_align_byte(&reader);
1126 : }
1127 :
1128 56 : *output_size = total_output;
1129 56 : return CCSDS124_OK;
1130 : }
1131 :
1132 : /** @} */ /* End of Packet Decompression */
|
