1 /*!
2 * \file FragDecoder.c
3 *
4 * \brief Implements the LoRa-Alliance fragmentation decoder
5 * Specification: https://lora-alliance.org/sites/default/files/2018-09/fragmented_data_block_transport_v1.0.0.pdf
6 *
7 * \copyright Revised BSD License, see section \ref LICENSE.
8 *
9 * \code
10 * ______ _
11 * / _____) _ | |
12 * ( (____ _____ ____ _| |_ _____ ____| |__
13 * \____ \| ___ | (_ _) ___ |/ ___) _ \
14 * _____) ) ____| | | || |_| ____( (___| | | |
15 * (______/|_____)_|_|_| \__)_____)\____)_| |_|
16 * (C)2013-2018 Semtech
17 *
18 * \endcode
19 *
20 * \author Fabien Holin ( Semtech )
21 * \author Miguel Luis ( Semtech )
22 */
23 #include <stddef.h>
24 #include <stdbool.h>
25 #include "utilities.h"
26 #include "FragDecoder.h"
27
28 #define DBG_TRACE 0
29
30 #if DBG_TRACE == 1
31 #include <stdio.h>
32 /*!
33 * Works in the same way as the printf function does.
34 */
35 #define DBG( ... ) \
36 do \
37 { \
38 printf( __VA_ARGS__ ); \
39 }while( 0 )
40 #else
41 #define DBG( fmt, ... )
42 #endif
43
44
45 /*
46 *=============================================================================
47 * Fragmentation decoder algorithm utilities
48 *=============================================================================
49 */
50
51 typedef struct
52 {
53 #if( FRAG_DECODER_FILE_HANDLING_NEW_API == 1 )
54 FragDecoderCallbacks_t *Callbacks;
55 #else
56 uint8_t *File;
57 uint32_t FileSize;
58 #endif
59 uint16_t FragNb;
60 uint8_t FragSize;
61
62 uint32_t M2BLine;
63 uint8_t MatrixM2B[( ( FRAG_MAX_REDUNDANCY >> 3 ) + 1 ) * FRAG_MAX_REDUNDANCY];
64 uint16_t FragNbMissingIndex[FRAG_MAX_NB];
65
66 uint8_t S[( FRAG_MAX_REDUNDANCY >> 3 ) + 1];
67
68 FragDecoderStatus_t Status;
69 }FragDecoder_t;
70
71 #if( FRAG_DECODER_FILE_HANDLING_NEW_API == 1 )
72 /*!
73 * \brief Sets a row from source into file destination
74 *
75 * \param [IN] src Source buffer pointer
76 * \param [IN] row Destination index of the row to be copied
77 * \param [IN] size Source number of bytes to be copied
78 */
79 static void SetRow( uint8_t *src, uint16_t row, uint16_t size );
80 #else
81 /*!
82 * \brief Sets a row from source into destination
83 *
84 * \param [IN] dst Destination buffer pointer
85 * \param [IN] src Source buffer pointer
86 * \param [IN] row Destination index of the row to be copied
87 * \param [IN] size Source number of bytes to be copied
88 */
89 static void SetRow( uint8_t *dst, uint8_t *src, uint16_t row, uint16_t size );
90 #endif
91
92 #if( FRAG_DECODER_FILE_HANDLING_NEW_API == 1 )
93 /*!
94 * \brief Gets a row from source and stores it into file destination
95 *
96 * \param [IN] src Source buffer pointer
97 * \param [IN] row Source index of the row to be copied
98 * \param [IN] size Source number of bytes to be copied
99 */
100 static void GetRow( uint8_t *src, uint16_t row, uint16_t size );
101 #else
102 /*!
103 * \brief Gets a row from source and stores it into destination
104 *
105 * \param [IN] dst Destination buffer pointer
106 * \param [IN] src Source buffer pointer
107 * \param [IN] row Source index of the row to be copied
108 * \param [IN] size Source number of bytes to be copied
109 */
110 static void GetRow( uint8_t *dst, uint8_t *src, uint16_t row, uint16_t size );
111 #endif
112
113 /*!
114 * \brief Gets the parity value from a given row of the parity matrix
115 *
116 * \param [IN] index The index of the row to be computed
117 * \param [IN] matrixRow Pointer to the parity matrix (parity bit array)
118 *
119 * \retval parity Parity value at the given index
120 */
121 static uint8_t GetParity( uint16_t index, uint8_t *matrixRow );
122
123 /*!
124 * \brief Sets the parity value on the given row of the parity matrix
125 *
126 * \param [IN] index The index of the row to be computed
127 * \param [IN/OUT] matrixRow Pointer to the parity matrix.
128 * \param [IN] parity The parity value to be set in the parity matrix
129 */
130 static void SetParity( uint16_t index, uint8_t *matrixRow, uint8_t parity );
131
132 /*!
133 * \brief Check if the provided value is a power of 2
134 *
135 * \param [IN] x Value to be tested
136 *
137 * \retval status Return true if frame is a power of two
138 */
139 static bool IsPowerOfTwo( uint32_t x );
140
141 /*!
142 * \brief XOrs two data lines
143 *
144 * \param [IN] line1 1st Data line to be XORed
145 * \param [IN] line2 2nd Data line to be XORed
146 * \param [IN] size Number of elements in line1
147 *
148 * \param [OUT] result XOR( line1, line2 ) result stored in line1
149 */
150 static void XorDataLine( uint8_t *line1, uint8_t *line2, int32_t size );
151
152 /*!
153 * \brief XORs two parity lines
154 *
155 * \param [IN] line1 1st Parity line to be XORed
156 * \param [IN] line2 2nd Parity line to be XORed
157 * \param [IN] size Number of elements in line1
158 *
159 * \param [OUT] result XOR( line1, line2 ) result stored in line1
160 */
161 static void XorParityLine( uint8_t* line1, uint8_t* line2, int32_t size );
162
163 /*!
164 * \brief Generates a pseudo random number : PRBS23
165 *
166 * \param [IN] value The input of the PRBS23 generator
167 *
168 * \retval nextValue Returns the next pseudo random number
169 */
170 static int32_t FragPrbs23( int32_t value );
171
172 /*!
173 * \brief Gets and fills the parity matrix
174 *
175 * \param [IN] n Fragment N
176 * \param [IN] m Fragment number
177 * \param [OUT] matrixRow Parity matrix
178 */
179 static void FragGetParityMatrixRow( int32_t n, int32_t m, uint8_t *matrixRow );
180
181 /*!
182 * \brief Finds the index of the first one in a bit array
183 *
184 * \param [IN] bitArray Pointer to the bit array
185 * \param [IN] size Bit array size
186 * \retval index The index of the first 1 in the bit array
187 */
188 static uint16_t BitArrayFindFirstOne( uint8_t *bitArray, uint16_t size );
189
190 /*!
191 * \brief Checks if the provided bit array only contains zeros
192 *
193 * \param [IN] bitArray Pointer to the bit array
194 * \param [IN] size Bit array size
195 * \retval isAllZeros [0: Contains ones, 1: Contains all zeros]
196 */
197 static uint8_t BitArrayIsAllZeros( uint8_t *bitArray, uint16_t size );
198
199 /*!
200 * \brief Finds & marks missing fragments
201 *
202 * \param [IN] counter Current fragment counter
203 * \param [OUT] FragDecoder.FragNbMissingIndex[] array is updated in place
204 */
205 static void FragFindMissingFrags( uint16_t counter );
206
207 /*!
208 * \brief Finds the index (frag counter) of the x th missing frag
209 *
210 * \param [IN] x x th missing frag
211 *
212 * \retval counter The counter value associated to the x th missing frag
213 */
214 static uint16_t FragFindMissingIndex( uint16_t x );
215
216 /*!
217 * \brief Extacts a row from the binary matrix and expands it to a bitArray
218 *
219 * \param [IN] bitArray Pointer to the bit array
220 * \param [IN] rowIndex Matrix row index
221 * \param [IN] bitsInRow Number of bits in one row
222 */
223 static void FragExtractLineFromBinaryMatrix( uint8_t* bitArray, uint16_t rowIndex, uint16_t bitsInRow );
224
225 /*!
226 * \brief Collapses and Pushs a row of a bit array to the matrix
227 *
228 * \param [IN] bitArray Pointer to the bit array
229 * \param [IN] rowIndex Matrix row index
230 * \param [IN] bitsInRow Number of bits in one row
231 */
232 static void FragPushLineToBinaryMatrix( uint8_t *bitArray, uint16_t rowIndex, uint16_t bitsInRow );
233
234 /*
235 *=============================================================================
236 * Fragmentation decoder algorithm
237 *=============================================================================
238 */
239
240 static FragDecoder_t FragDecoder;
241
242 #if( FRAG_DECODER_FILE_HANDLING_NEW_API == 1 )
FragDecoderInit(uint16_t fragNb,uint8_t fragSize,FragDecoderCallbacks_t * callbacks)243 void FragDecoderInit( uint16_t fragNb, uint8_t fragSize, FragDecoderCallbacks_t *callbacks )
244 #else
245 void FragDecoderInit( uint16_t fragNb, uint8_t fragSize, uint8_t *file, uint32_t fileSize )
246 #endif
247 {
248 #if( FRAG_DECODER_FILE_HANDLING_NEW_API == 1 )
249 FragDecoder.Callbacks = callbacks;
250 #else
251 FragDecoder.File = file;
252 FragDecoder.FileSize = fileSize;
253 #endif
254 FragDecoder.FragNb = fragNb; // FragNb = FRAG_MAX_SIZE
255 FragDecoder.FragSize = fragSize; // number of byte on a row
256 FragDecoder.Status.FragNbLastRx = 0;
257 FragDecoder.Status.FragNbLost = 0;
258 FragDecoder.M2BLine = 0;
259
260 // Initialize missing fragments index array
261 for( uint16_t i = 0; i < FRAG_MAX_NB; i++ )
262 {
263 FragDecoder.FragNbMissingIndex[i] = 1;
264 }
265
266 // Initialize parity matrix
267 for( uint32_t i = 0; i < ( ( FRAG_MAX_REDUNDANCY >> 3 ) + 1 ); i++ )
268 {
269 FragDecoder.S[i] = 0;
270 }
271
272 for( uint32_t i = 0; i < ( ( ( FRAG_MAX_REDUNDANCY >> 3 ) + 1 ) * FRAG_MAX_REDUNDANCY ); i++ )
273 {
274 FragDecoder.MatrixM2B[i] = 0xFF;
275 }
276
277 // Initialize final uncoded data buffer ( FRAG_MAX_NB * FRAG_MAX_SIZE )
278 for( uint32_t i = 0; i < ( fragNb * fragSize ); i++ )
279 {
280 #if( FRAG_DECODER_FILE_HANDLING_NEW_API == 1 )
281 if( ( FragDecoder.Callbacks != NULL ) && ( FragDecoder.Callbacks->FragDecoderWrite != NULL ) )
282 {
283 uint8_t buffer[1] = { 0xFF };
284 FragDecoder.Callbacks->FragDecoderWrite( i, buffer, 1 );
285 }
286 #else
287 FragDecoder.File[i] = 0xFF;
288 #endif
289 }
290 FragDecoder.Status.FragNbLost = 0;
291 FragDecoder.Status.FragNbLastRx = 0;
292 }
293
294 #if( FRAG_DECODER_FILE_HANDLING_NEW_API == 1 )
FragDecoderGetMaxFileSize(void)295 uint32_t FragDecoderGetMaxFileSize( void )
296 {
297 return FRAG_MAX_NB * FRAG_MAX_SIZE;
298 }
299 #endif
300
FragDecoderProcess(uint16_t fragCounter,uint8_t * rawData)301 int32_t FragDecoderProcess( uint16_t fragCounter, uint8_t *rawData )
302 {
303 uint16_t firstOneInRow = 0;
304 int32_t first = 0;
305 int32_t noInfo = 0;
306
307 uint8_t matrixRow[(FRAG_MAX_NB >> 3 ) + 1];
308 uint8_t matrixDataTemp[FRAG_MAX_SIZE];
309 uint8_t dataTempVector[( FRAG_MAX_REDUNDANCY >> 3 ) + 1];
310 uint8_t dataTempVector2[( FRAG_MAX_REDUNDANCY >> 3 ) + 1];
311
312 memset1( matrixRow, 0, ( FRAG_MAX_NB >> 3 ) + 1 );
313 memset1( matrixDataTemp, 0, FRAG_MAX_SIZE );
314 memset1( dataTempVector, 0, ( FRAG_MAX_REDUNDANCY >> 3 ) + 1 );
315 memset1( dataTempVector2, 0, ( FRAG_MAX_REDUNDANCY >> 3 ) + 1 );
316
317 FragDecoder.Status.FragNbRx = fragCounter;
318
319 if( fragCounter < FragDecoder.Status.FragNbLastRx )
320 {
321 return FRAG_SESSION_ONGOING; // Drop frame out of order
322 }
323
324 // The M (FragNb) first packets aren't encoded or in other words they are
325 // encoded with the unitary matrix
326 if( fragCounter < ( FragDecoder.FragNb + 1 ) )
327 {
328 // The M first frame are not encoded store them
329 #if( FRAG_DECODER_FILE_HANDLING_NEW_API == 1 )
330 SetRow( rawData, fragCounter - 1, FragDecoder.FragSize );
331 #else
332 SetRow( FragDecoder.File, rawData, fragCounter - 1, FragDecoder.FragSize );
333 #endif
334
335 FragDecoder.FragNbMissingIndex[fragCounter - 1] = 0;
336
337 // Update the FragDecoder.FragNbMissingIndex with the loosing frame
338 FragFindMissingFrags( fragCounter );
339
340 if( ( FragDecoder.Status.FragNbLost == 0 ) && ( fragCounter == FragDecoder.FragNb ) )
341 {
342 // the case : all the M(FragNb) first rows have been transmitted with no error
343 return FragDecoder.Status.FragNbLost;
344 }
345 }
346 else
347 {
348 if( FragDecoder.Status.FragNbLost > FRAG_MAX_REDUNDANCY )
349 {
350 FragDecoder.Status.MatrixError = 1;
351 return FRAG_SESSION_FINISHED;
352 }
353 // At this point we receive encoded frames and the number of loosing frames
354 // is well known: FragDecoder.FragNbLost - 1;
355
356 // In case of the end of true data is missing
357 FragFindMissingFrags( fragCounter );
358
359 // fragCounter - FragDecoder.FragNb
360 FragGetParityMatrixRow( fragCounter - FragDecoder.FragNb, FragDecoder.FragNb, matrixRow );
361
362 for( int32_t i = 0; i < FragDecoder.FragNb; i++ )
363 {
364 if( GetParity( i , matrixRow ) == 1 )
365 {
366 if( FragDecoder.FragNbMissingIndex[i] == 0 )
367 {
368 // XOR with already receive frag
369 SetParity( i, matrixRow, 0 );
370 #if( FRAG_DECODER_FILE_HANDLING_NEW_API == 1 )
371 GetRow( matrixDataTemp, i, FragDecoder.FragSize );
372 #else
373 GetRow( matrixDataTemp, FragDecoder.File, i, FragDecoder.FragSize );
374 #endif
375 XorDataLine( rawData, matrixDataTemp, FragDecoder.FragSize );
376 }
377 else
378 {
379 // Fill the "little" boolean matrix m2b
380 SetParity( FragDecoder.FragNbMissingIndex[i] - 1, dataTempVector, 1 );
381 if( first == 0 )
382 {
383 first = 1;
384 }
385 }
386 }
387 }
388
389 firstOneInRow = BitArrayFindFirstOne( dataTempVector, FragDecoder.Status.FragNbLost );
390
391 if( first > 0 )
392 {
393 int32_t li;
394 int32_t lj;
395
396 // Manage a new line in MatrixM2B
397 while( GetParity( firstOneInRow, FragDecoder.S ) == 1 )
398 {
399 // Row already diagonalized exist & ( FragDecoder.MatrixM2B[firstOneInRow][0] )
400 FragExtractLineFromBinaryMatrix( dataTempVector2, firstOneInRow, FragDecoder.Status.FragNbLost );
401 XorParityLine( dataTempVector, dataTempVector2, FragDecoder.Status.FragNbLost );
402 // Have to store it in the mi th position of the missing frag
403 li = FragFindMissingIndex( firstOneInRow );
404 #if( FRAG_DECODER_FILE_HANDLING_NEW_API == 1 )
405 GetRow( matrixDataTemp, li, FragDecoder.FragSize );
406 #else
407 GetRow( matrixDataTemp, FragDecoder.File, li, FragDecoder.FragSize );
408 #endif
409 XorDataLine( rawData, matrixDataTemp, FragDecoder.FragSize );
410 if( BitArrayIsAllZeros( dataTempVector, FragDecoder.Status.FragNbLost ) )
411 {
412 noInfo = 1;
413 break;
414 }
415 firstOneInRow = BitArrayFindFirstOne( dataTempVector, FragDecoder.Status.FragNbLost );
416 }
417
418 if( noInfo == 0 )
419 {
420 FragPushLineToBinaryMatrix( dataTempVector, firstOneInRow, FragDecoder.Status.FragNbLost );
421 li = FragFindMissingIndex( firstOneInRow );
422 #if( FRAG_DECODER_FILE_HANDLING_NEW_API == 1 )
423 SetRow( rawData, li, FragDecoder.FragSize );
424 #else
425 SetRow( FragDecoder.File, rawData, li, FragDecoder.FragSize );
426 #endif
427 SetParity( firstOneInRow, FragDecoder.S, 1 );
428 FragDecoder.M2BLine++;
429 }
430
431 if( FragDecoder.M2BLine == FragDecoder.Status.FragNbLost )
432 {
433 // Then last step diagonalized
434 if( FragDecoder.Status.FragNbLost > 1 )
435 {
436 int32_t i, j;
437
438 for( i = ( FragDecoder.Status.FragNbLost - 2 ); i >= 0 ; i-- )
439 {
440 li = FragFindMissingIndex( i );
441 #if( FRAG_DECODER_FILE_HANDLING_NEW_API == 1 )
442 GetRow( matrixDataTemp, li, FragDecoder.FragSize );
443 #else
444 GetRow( matrixDataTemp, FragDecoder.File, li, FragDecoder.FragSize );
445 #endif
446 for( j = ( FragDecoder.Status.FragNbLost - 1 ); j > i; j--)
447 {
448 FragExtractLineFromBinaryMatrix( dataTempVector2, i, FragDecoder.Status.FragNbLost );
449 FragExtractLineFromBinaryMatrix( dataTempVector, j, FragDecoder.Status.FragNbLost );
450 if( GetParity( j, dataTempVector2 ) == 1 )
451 {
452 XorParityLine( dataTempVector2, dataTempVector, FragDecoder.Status.FragNbLost );
453
454 lj = FragFindMissingIndex( j );
455
456 #if( FRAG_DECODER_FILE_HANDLING_NEW_API == 1 )
457 GetRow( rawData, lj, FragDecoder.FragSize );
458 #else
459 GetRow( rawData, FragDecoder.File, lj, FragDecoder.FragSize );
460 #endif
461 XorDataLine( matrixDataTemp , rawData , FragDecoder.FragSize );
462 }
463 }
464 #if( FRAG_DECODER_FILE_HANDLING_NEW_API == 1 )
465 SetRow( matrixDataTemp, li, FragDecoder.FragSize );
466 #else
467 SetRow( FragDecoder.File, matrixDataTemp, li, FragDecoder.FragSize );
468 #endif
469 }
470 return FragDecoder.Status.FragNbLost;
471 }
472 else
473 {
474 //If not ( FragDecoder.FragNbLost > 1 )
475 return FragDecoder.Status.FragNbLost;
476 }
477 }
478 }
479 }
480 return FRAG_SESSION_ONGOING;
481 }
482
FragDecoderGetStatus(void)483 FragDecoderStatus_t FragDecoderGetStatus( void )
484 {
485 return FragDecoder.Status;
486 }
487
488 /*
489 *=============================================================================
490 * Fragmentation decoder algorithm utilities
491 *=============================================================================
492 */
493
494 #if( FRAG_DECODER_FILE_HANDLING_NEW_API == 1 )
SetRow(uint8_t * src,uint16_t row,uint16_t size)495 static void SetRow( uint8_t *src, uint16_t row, uint16_t size )
496 {
497 if( ( FragDecoder.Callbacks != NULL ) && ( FragDecoder.Callbacks->FragDecoderWrite != NULL ) )
498 {
499 FragDecoder.Callbacks->FragDecoderWrite( row * size, src, size );
500 }
501 }
502
GetRow(uint8_t * dst,uint16_t row,uint16_t size)503 static void GetRow( uint8_t *dst, uint16_t row, uint16_t size )
504 {
505 if( ( FragDecoder.Callbacks != NULL ) && ( FragDecoder.Callbacks->FragDecoderRead != NULL ) )
506 {
507 FragDecoder.Callbacks->FragDecoderRead( row * size, dst, size );
508 }
509 }
510 #else
SetRow(uint8_t * dst,uint8_t * src,uint16_t row,uint16_t size)511 static void SetRow( uint8_t *dst, uint8_t *src, uint16_t row, uint16_t size )
512 {
513 memcpy1( &dst[row * size], src, size );
514 }
515
GetRow(uint8_t * dst,uint8_t * src,uint16_t row,uint16_t size)516 static void GetRow( uint8_t *dst, uint8_t *src, uint16_t row, uint16_t size )
517 {
518 memcpy1( dst, &src[row * size], size );
519 }
520 #endif
521
GetParity(uint16_t index,uint8_t * matrixRow)522 static uint8_t GetParity( uint16_t index, uint8_t *matrixRow )
523 {
524 uint8_t parity;
525 parity = matrixRow[index >> 3];
526 parity = ( parity >> ( 7 - ( index % 8 ) ) ) & 0x01;
527 return parity;
528 }
529
SetParity(uint16_t index,uint8_t * matrixRow,uint8_t parity)530 static void SetParity( uint16_t index, uint8_t *matrixRow, uint8_t parity )
531 {
532 uint8_t mask = 0xFF - ( 1 << ( 7 - ( index % 8 ) ) );
533 parity = parity << ( 7 - ( index % 8 ) );
534 matrixRow[index >> 3] = ( matrixRow[index >> 3] & mask ) + parity;
535 }
536
IsPowerOfTwo(uint32_t x)537 static bool IsPowerOfTwo( uint32_t x )
538 {
539 uint8_t sumBit = 0;
540
541 for( uint8_t i = 0; i < 32; i++ )
542 {
543 sumBit += ( x & ( 1 << i ) ) >> i;
544 }
545 if( sumBit == 1 )
546 {
547 return true;
548 }
549 return false;
550 }
551
XorDataLine(uint8_t * line1,uint8_t * line2,int32_t size)552 static void XorDataLine( uint8_t *line1, uint8_t *line2, int32_t size )
553 {
554 for( int32_t i = 0; i < size; i++ )
555 {
556 line1[i] = line1[i] ^ line2[i];
557 }
558 }
559
XorParityLine(uint8_t * line1,uint8_t * line2,int32_t size)560 static void XorParityLine( uint8_t* line1, uint8_t* line2, int32_t size )
561 {
562 for( int32_t i = 0; i < size; i++ )
563 {
564 SetParity( i, line1, ( GetParity( i, line1 ) ^ GetParity( i, line2 ) ) );
565 }
566 }
567
FragPrbs23(int32_t value)568 static int32_t FragPrbs23( int32_t value )
569 {
570 int32_t b0 = value & 0x01;
571 int32_t b1 = ( value & 0x20 ) >> 5;
572 return ( value >> 1 ) + ( ( b0 ^ b1 ) << 22 );
573 }
574
FragGetParityMatrixRow(int32_t n,int32_t m,uint8_t * matrixRow)575 static void FragGetParityMatrixRow( int32_t n, int32_t m, uint8_t *matrixRow )
576 {
577 int32_t mTemp;
578 int32_t x;
579 int32_t nbCoeff = 0;
580 int32_t r;
581
582 if( IsPowerOfTwo( m ) != false )
583 {
584 mTemp = 1;
585 }
586 else
587 {
588 mTemp = 0;
589 }
590
591 x = 1 + ( 1001 * n );
592 for( int32_t i = 0; i < ( ( m >> 3 ) + 1 ); i++ )
593 {
594 matrixRow[i] = 0;
595 }
596 while( nbCoeff < ( m >> 1 ) )
597 {
598 r = 1 << 16;
599 while( r >= m )
600 {
601 x = FragPrbs23( x );
602 r = x % ( m + mTemp );
603 }
604 SetParity( r, matrixRow, 1 );
605 nbCoeff += 1;
606 }
607 }
608
BitArrayFindFirstOne(uint8_t * bitArray,uint16_t size)609 static uint16_t BitArrayFindFirstOne( uint8_t *bitArray, uint16_t size )
610 {
611 for( uint16_t i = 0; i < size; i++)
612 {
613 if ( GetParity( i, bitArray ) == 1 )
614 {
615 return i;
616 }
617 }
618 return 0;
619 }
620
BitArrayIsAllZeros(uint8_t * bitArray,uint16_t size)621 static uint8_t BitArrayIsAllZeros( uint8_t *bitArray, uint16_t size )
622 {
623 for( uint16_t i = 0; i < size; i++ )
624 {
625 if( GetParity( i, bitArray ) == 1 )
626 {
627 return 0;
628 }
629 }
630 return 1;
631 }
632
633 /*!
634 * \brief Finds & marks missing fragments
635 *
636 * \param [IN] counter Current fragment counter
637 * \param [OUT] FragDecoder.FragNbMissingIndex[] array is updated in place
638 */
FragFindMissingFrags(uint16_t counter)639 static void FragFindMissingFrags( uint16_t counter )
640 {
641 int32_t i;
642 for( i = FragDecoder.Status.FragNbLastRx; i < ( counter - 1 ); i++ )
643 {
644 if( i < FragDecoder.FragNb )
645 {
646 FragDecoder.Status.FragNbLost++;
647 FragDecoder.FragNbMissingIndex[i] = FragDecoder.Status.FragNbLost;
648 }
649 }
650 if( i < FragDecoder.FragNb )
651 {
652 FragDecoder.Status.FragNbLastRx = counter;
653 }
654 else
655 {
656 FragDecoder.Status.FragNbLastRx = FragDecoder.FragNb + 1;
657 }
658 DBG( "RECEIVED : %5d / %5d Fragments\n", FragDecoder.Status.FragNbRx, FragDecoder.FragNb );
659 DBG( " %5d / %5d Bytes\n", FragDecoder.Status.FragNbRx * FragDecoder.FragSize, FragDecoder.FragNb * FragDecoder.FragSize );
660 DBG( "LOST : %7d Fragments\n\n", FragDecoder.Status.FragNbLost );
661 }
662
663 /*!
664 * \brief Finds the index (frag counter) of the x th missing frag
665 *
666 * \param [IN] x x th missing frag
667 *
668 * \retval counter The counter value associated to the x th missing frag
669 */
FragFindMissingIndex(uint16_t x)670 static uint16_t FragFindMissingIndex( uint16_t x )
671 {
672 for( uint16_t i = 0; i < FragDecoder.FragNb; i++ )
673 {
674 if( FragDecoder.FragNbMissingIndex[i] == ( x + 1 ) )
675 {
676 return i;
677 }
678 }
679 return 0;
680 }
681
682 /*!
683 * \brief Extacts a row from the binary matrix and expands it to a bitArray
684 *
685 * \param [IN] bitArray Pointer to the bit array
686 * \param [IN] rowIndex Matrix row index
687 * \param [IN] bitsInRow Number of bits in one row
688 */
FragExtractLineFromBinaryMatrix(uint8_t * bitArray,uint16_t rowIndex,uint16_t bitsInRow)689 static void FragExtractLineFromBinaryMatrix( uint8_t* bitArray, uint16_t rowIndex, uint16_t bitsInRow )
690 {
691 uint32_t findByte = 0;
692 uint32_t findBitInByte = 0;
693
694 if( rowIndex > 0 )
695 {
696 findByte = ( rowIndex * bitsInRow - ( ( rowIndex * ( rowIndex - 1 ) ) >> 1 ) ) >> 3;
697 findBitInByte = ( rowIndex * bitsInRow - ( ( rowIndex * ( rowIndex - 1 ) ) >> 1 ) ) % 8;
698 }
699 if( rowIndex > 0 )
700 {
701 for( uint16_t i = 0; i < rowIndex; i++ )
702 {
703 SetParity( i, bitArray, 0 );
704 }
705 }
706 for( uint16_t i = rowIndex; i < bitsInRow; i++ )
707 {
708 SetParity( i,
709 bitArray,
710 ( FragDecoder.MatrixM2B[findByte] >> ( 7 - findBitInByte ) ) & 0x01 );
711
712 findBitInByte++;
713 if( findBitInByte == 8 )
714 {
715 findBitInByte = 0;
716 findByte++;
717 }
718 }
719 }
720
721 /*!
722 * \brief Collapses and Pushs a row of a bit array to the matrix
723 *
724 * \param [IN] bitArray Pointer to the bit array
725 * \param [IN] rowIndex Matrix row index
726 * \param [IN] bitsInRow Number of bits in one row
727 */
FragPushLineToBinaryMatrix(uint8_t * bitArray,uint16_t rowIndex,uint16_t bitsInRow)728 static void FragPushLineToBinaryMatrix( uint8_t *bitArray, uint16_t rowIndex, uint16_t bitsInRow )
729 {
730 uint32_t findByte = 0;
731 uint32_t findBitInByte = 0;
732
733 if ( rowIndex > 0) {
734 findByte = ( rowIndex * bitsInRow - ( ( rowIndex * ( rowIndex - 1 ) ) >> 1 ) ) >> 3;
735 findBitInByte = ( rowIndex * bitsInRow - ( ( rowIndex * ( rowIndex - 1 ) ) >> 1 ) ) % 8;
736
737 }
738 for( uint16_t i = rowIndex; i < bitsInRow; i++ )
739 {
740 if( GetParity( i, bitArray ) == 0 )
741 {
742 FragDecoder.MatrixM2B[findByte] = FragDecoder.MatrixM2B[findByte] & ( 0xFF - ( 1 << ( 7 - findBitInByte ) ) );
743 }
744 findBitInByte++;
745 if( findBitInByte == 8 )
746 {
747 findBitInByte = 0;
748 findByte++;
749 }
750 }
751 }
752
