sundp
/
STM32CubeWL
mirror of https://github.com/STMicroelectronics/STM32CubeWL.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
STM32CubeWL/Middlewares/Third_Party/LoRaWAN/LmHandler/Packages/FragDecoder.c

630 lines
18 KiB
C
Raw Blame History

/*!
* \file FragDecoder.c
*
* \brief Implements the LoRa-Alliance fragmentation decoder
* Specification: https://lora-alliance.org/sites/default/files/2018-09/fragmented_data_block_transport_v1.0.0.pdf
*
* \copyright Revised BSD License, see section \ref LICENSE.
*
* \code
* ______ _
* / _____) _ | |
* ( (____ _____ ____ _| |_ _____ ____| |__
* \____ \| ___ | (_ _) ___ |/ ___) _ \
* _____) ) ____| | | || |_| ____( (___| | | |
* (______/|_____)_|_|_| \__)_____)\____)_| |_|
* (C)2013-2018 Semtech
*
* \endcode
*
* \author Fabien Holin ( Semtech )
* \author Miguel Luis ( Semtech )
*/
/**
******************************************************************************
*
* Portions COPYRIGHT 2020 STMicroelectronics
*
* @file FragDecoder.c
* @author MCD Application Team
* @brief Fragmentation Decoder definition
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include <stdbool.h>
#include "mw_log_conf.h" /* needed for MW_LOG */
#include "FragDecoder.h"
#include "sfu_fwimg_regions.h"
/* Private typedef -----------------------------------------------------------*/
typedef struct
{
FragDecoderCallbacks_t *Callbacks;
uint16_t FragNb;
uint8_t FragSize;
uint32_t M2BLine;
uint8_t MatrixM2B[((FRAG_MAX_REDUNDANCY >> 3) + 1) * FRAG_MAX_REDUNDANCY];
uint16_t FragNbMissingIndex[FRAG_MAX_NB];
uint8_t S[(FRAG_MAX_REDUNDANCY >> 3) + 1];
FragDecoderStatus_t Status;
} FragDecoder_t;
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/*!
* \brief Sets a row from source into file destination
*
* \param [in] src Source buffer pointer
* \param [in] row Destination index of the row to be copied
* \param [in] size Source number of bytes to be copied
*/
static void SetRow(uint8_t *src, uint16_t row, uint16_t size);
/*!
* \brief Gets a row from source and stores it into file destination
*
* \param [in] src Source buffer pointer
* \param [in] row Source index of the row to be copied
* \param [in] size Source number of bytes to be copied
*/
static void GetRow(uint8_t *src, uint16_t row, uint16_t size);
/*!
* \brief Gets the parity value from a given row of the parity matrix
*
* \param [in] index The index of the row to be computed
* \param [in] matrixRow Pointer to the parity matrix (parity bit array)
*
* \retval parity Parity value at the given index
*/
static uint8_t GetParity(uint16_t index, uint8_t *matrixRow);
/*!
* \brief Sets the parity value on the given row of the parity matrix
*
* \param [in] index The index of the row to be computed
* \param [IN/OUT] matrixRow Pointer to the parity matrix.
* \param [in] parity The parity value to be set in the parity matrix
*/
static void SetParity(uint16_t index, uint8_t *matrixRow, uint8_t parity);
/*!
* \brief Check if the provided value is a power of 2
*
* \param [in] x Value to be tested
*
* \retval status Return true if frame is a power of two
*/
static bool IsPowerOfTwo(uint32_t x);
/*!
* \brief XOrs two data lines
*
* \param [in] line1 1st Data line to be XORed
* \param [in] line2 2nd Data line to be XORed
* \param [in] size Number of elements in line1
*
* \param [out] result XOR( line1, line2 ) result stored in line1
*/
static void XorDataLine(uint8_t *line1, uint8_t *line2, int32_t size);
/*!
* \brief XORs two parity lines
*
* \param [in] line1 1st Parity line to be XORed
* \param [in] line2 2nd Parity line to be XORed
* \param [in] size Number of elements in line1
*
* \param [out] result XOR( line1, line2 ) result stored in line1
*/
static void XorParityLine(uint8_t *line1, uint8_t *line2, int32_t size);
/*!
* \brief Generates a pseudo random number : PRBS23
*
* \param [in] value The input of the PRBS23 generator
*
* \retval nextValue Returns the next pseudo random number
*/
static int32_t FragPrbs23(int32_t value);
/*!
* \brief Gets and fills the parity matrix
*
* \param [in] n Fragment N
* \param [in] m Fragment number
* \param [out] matrixRow Parity matrix
*/
static void FragGetParityMatrixRow(int32_t n, int32_t m, uint8_t *matrixRow);
/*!
* \brief Finds the index of the first one in a bit array
*
* \param [in] bitArray Pointer to the bit array
* \param [in] size Bit array size
* \retval index The index of the first 1 in the bit array
*/
static uint16_t BitArrayFindFirstOne(uint8_t *bitArray, uint16_t size);
/*!
* \brief Checks if the provided bit array only contains zeros
*
* \param [in] bitArray Pointer to the bit array
* \param [in] size Bit array size
* \retval isAllZeros [0: Contains ones, 1: Contains all zeros]
*/
static uint8_t BitArrayIsAllZeros(uint8_t *bitArray, uint16_t size);
/*!
* \brief Finds & marks missing fragments
*
* \param [in] counter Current fragment counter
* \param [out] FragDecoder.FragNbMissingIndex[] array is updated in place
*/
static void FragFindMissingFrags(uint16_t counter);
/*!
* \brief Finds the index (frag counter) of the x th missing frag
*
* \param [in] x x th missing frag
*
* \retval counter The counter value associated to the x th missing frag
*/
static uint16_t FragFindMissingIndex(uint16_t x);
/*!
* \brief Extacts a row from the binary matrix and expands it to a bitArray
*
* \param [in] bitArray Pointer to the bit array
* \param [in] rowIndex Matrix row index
* \param [in] bitsInRow Number of bits in one row
*/
static void FragExtractLineFromBinaryMatrix(uint8_t *bitArray, uint16_t rowIndex, uint16_t bitsInRow);
/*!
* \brief Collapses and Pushs a row of a bit array to the matrix
*
* \param [in] bitArray Pointer to the bit array
* \param [in] rowIndex Matrix row index
* \param [in] bitsInRow Number of bits in one row
*/
static void FragPushLineToBinaryMatrix(uint8_t *bitArray, uint16_t rowIndex, uint16_t bitsInRow);
/* Private variables ---------------------------------------------------------*/
static FragDecoder_t FragDecoder;
/* Exported functions ---------------------------------------------------------*/
void FragDecoderInit(uint16_t fragNb, uint8_t fragSize, FragDecoderCallbacks_t *callbacks)
{
#if (INTEROP_TEST_MODE == 1)
uint8_t init_buffer[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
#endif /* INTEROP_TEST_MODE == 1 */
FragDecoder.Callbacks = callbacks;
FragDecoder.FragNb = fragNb; /* FragNb = FRAG_MAX_SIZE */
FragDecoder.FragSize = fragSize; /* number of byte on a row */
FragDecoder.Status.FragNbLastRx = 0;
FragDecoder.Status.FragNbLost = 0;
FragDecoder.M2BLine = 0;
/* Initialize missing fragments index array */
for (uint16_t i = 0; i < FRAG_MAX_NB; i++)
{
FragDecoder.FragNbMissingIndex[i] = 1;
}
/* Initialize parity matrix */
for (uint32_t i = 0; i < ((FRAG_MAX_REDUNDANCY >> 3) + 1); i++)
{
FragDecoder.S[i] = 0;
}
for (uint32_t i = 0; i < (((FRAG_MAX_REDUNDANCY >> 3) + 1) * FRAG_MAX_REDUNDANCY); i++)
{
FragDecoder.MatrixM2B[i] = 0xFF;
}
/* Initialize final uncoded data buffer ( FRAG_MAX_NB * FRAG_MAX_SIZE ) */
#if (INTEROP_TEST_MODE == 1)
FragDecoder.Callbacks->FragDecoderErase(0, fragNb * fragSize);
#else /* INTEROP_TEST_MODE == 0 */
FragDecoder.Callbacks->FragDecoderErase(0, SlotEndAdd[SLOT_DWL_1] - SlotStartAdd[SLOT_DWL_1] + 1U);
#endif /* INTEROP_TEST_MODE */
FragDecoder.Status.FragNbLost = 0;
FragDecoder.Status.FragNbLastRx = 0;
}
uint32_t FragDecoderGetMaxFileSize(void)
{
return FRAG_MAX_NB * FRAG_MAX_SIZE;
}
int32_t FragDecoderProcess(uint16_t fragCounter, uint8_t *rawData)
{
uint16_t firstOneInRow = 0;
int32_t first = 0;
int32_t noInfo = 0;
uint8_t matrixRow[(FRAG_MAX_NB >> 3) + 1];
uint8_t matrixDataTemp[FRAG_MAX_SIZE];
uint8_t dataTempVector[(FRAG_MAX_REDUNDANCY >> 3) + 1];
uint8_t dataTempVector2[(FRAG_MAX_REDUNDANCY >> 3) + 1];
UTIL_MEM_set_8(matrixRow, 0, (FRAG_MAX_NB >> 3) + 1);
UTIL_MEM_set_8(matrixDataTemp, 0, FRAG_MAX_SIZE);
UTIL_MEM_set_8(dataTempVector, 0, (FRAG_MAX_REDUNDANCY >> 3) + 1);
UTIL_MEM_set_8(dataTempVector2, 0, (FRAG_MAX_REDUNDANCY >> 3) + 1);
FragDecoder.Status.FragNbRx = fragCounter;
if (fragCounter < FragDecoder.Status.FragNbLastRx)
{
return FRAG_SESSION_ONGOING; /* Drop frame out of order */
}
/* The M (FragNb) first packets aren't encoded or in other words they are */
/* encoded with the unitary matrix */
if (fragCounter < (FragDecoder.FragNb + 1))
{
/* The M first frame are not encoded store them */
SetRow(rawData, fragCounter - 1, FragDecoder.FragSize);
FragDecoder.FragNbMissingIndex[fragCounter - 1] = 0;
/* Update the FragDecoder.FragNbMissingIndex with the losing frame */
FragFindMissingFrags(fragCounter);
}
else
{
if (FragDecoder.Status.FragNbLost > FRAG_MAX_REDUNDANCY)
{
FragDecoder.Status.MatrixError = 1;
return FRAG_SESSION_FINISHED;
}
/* At this point we receive encoded frames and the number of losing frames */
/* is well known: FragDecoder.FragNbLost - 1; */
/* In case of the end of true data is missing */
FragFindMissingFrags(fragCounter);
if (FragDecoder.Status.FragNbLost == 0)
{
/* the case : all the M(FragNb) first rows have been transmitted with no error */
return FragDecoder.Status.FragNbLost;
}
/* fragCounter - FragDecoder.FragNb */
FragGetParityMatrixRow(fragCounter - FragDecoder.FragNb, FragDecoder.FragNb, matrixRow);
for (int32_t i = 0; i < FragDecoder.FragNb; i++)
{
if (GetParity(i, matrixRow) == 1)
{
if (FragDecoder.FragNbMissingIndex[i] == 0)
{
/* XOR with already receive frag */
SetParity(i, matrixRow, 0);
GetRow(matrixDataTemp, i, FragDecoder.FragSize);
XorDataLine(rawData, matrixDataTemp, FragDecoder.FragSize);
}
else
{
/* Fill the "little" boolean matrix m2b */
SetParity(FragDecoder.FragNbMissingIndex[i] - 1, dataTempVector, 1);
if (first == 0)
{
first = 1;
}
}
}
}
firstOneInRow = BitArrayFindFirstOne(dataTempVector, FragDecoder.Status.FragNbLost);
if (first > 0)
{
int32_t li;
int32_t lj;
/* Manage a new line in MatrixM2B */
while (GetParity(firstOneInRow, FragDecoder.S) == 1)
{
/* Row already diagonalized exist & ( FragDecoder.MatrixM2B[firstOneInRow][0] ) */
FragExtractLineFromBinaryMatrix(dataTempVector2, firstOneInRow, FragDecoder.Status.FragNbLost);
XorParityLine(dataTempVector, dataTempVector2, FragDecoder.Status.FragNbLost);
/* Have to store it in the mi th position of the missing frag */
li = FragFindMissingIndex(firstOneInRow);
GetRow(matrixDataTemp, li, FragDecoder.FragSize);
XorDataLine(rawData, matrixDataTemp, FragDecoder.FragSize);
if (BitArrayIsAllZeros(dataTempVector, FragDecoder.Status.FragNbLost))
{
noInfo = 1;
break;
}
firstOneInRow = BitArrayFindFirstOne(dataTempVector, FragDecoder.Status.FragNbLost);
}
if (noInfo == 0)
{
FragPushLineToBinaryMatrix(dataTempVector, firstOneInRow, FragDecoder.Status.FragNbLost);
li = FragFindMissingIndex(firstOneInRow);
SetRow(rawData, li, FragDecoder.FragSize);
SetParity(firstOneInRow, FragDecoder.S, 1);
FragDecoder.M2BLine++;
}
if (FragDecoder.M2BLine == FragDecoder.Status.FragNbLost)
{
/* Then last step diagonalized */
if (FragDecoder.Status.FragNbLost > 1)
{
int32_t i;
int32_t j;
for (i = (FragDecoder.Status.FragNbLost - 2); i >= 0 ; i--)
{
li = FragFindMissingIndex(i);
GetRow(matrixDataTemp, li, FragDecoder.FragSize);
for (j = (FragDecoder.Status.FragNbLost - 1); j > i; j--)
{
FragExtractLineFromBinaryMatrix(dataTempVector2, i, FragDecoder.Status.FragNbLost);
FragExtractLineFromBinaryMatrix(dataTempVector, j, FragDecoder.Status.FragNbLost);
if (GetParity(j, dataTempVector2) == 1)
{
XorParityLine(dataTempVector2, dataTempVector, FragDecoder.Status.FragNbLost);
lj = FragFindMissingIndex(j);
GetRow(rawData, lj, FragDecoder.FragSize);
XorDataLine(matrixDataTemp, rawData, FragDecoder.FragSize);
}
}
SetRow(matrixDataTemp, li, FragDecoder.FragSize);
}
return FragDecoder.Status.FragNbLost;
}
else
{
/* If not ( FragDecoder.FragNbLost > 1 ) */
return FragDecoder.Status.FragNbLost;
}
}
}
}
return FRAG_SESSION_ONGOING;
}
FragDecoderStatus_t FragDecoderGetStatus(void)
{
return FragDecoder.Status;
}
/* Private functions ---------------------------------------------------------*/
static void SetRow(uint8_t *src, uint16_t row, uint16_t size)
{
if ((FragDecoder.Callbacks != NULL) && (FragDecoder.Callbacks->FragDecoderWrite != NULL))
{
FragDecoder.Callbacks->FragDecoderWrite(row * size, src, size);
}
}
static void GetRow(uint8_t *dst, uint16_t row, uint16_t size)
{
if ((FragDecoder.Callbacks != NULL) && (FragDecoder.Callbacks->FragDecoderRead != NULL))
{
FragDecoder.Callbacks->FragDecoderRead(row * size, dst, size);
}
}
static uint8_t GetParity(uint16_t index, uint8_t *matrixRow)
{
uint8_t parity;
parity = matrixRow[index >> 3];
parity = (parity >> (7 - (index % 8))) & 0x01;
return parity;
}
static void SetParity(uint16_t index, uint8_t *matrixRow, uint8_t parity)
{
uint8_t mask = 0xFF - (1 << (7 - (index % 8)));
parity = parity << (7 - (index % 8));
matrixRow[index >> 3] = (matrixRow[index >> 3] & mask) + parity;
}
static bool IsPowerOfTwo(uint32_t x)
{
uint8_t sumBit = 0;
for (uint8_t i = 0; i < 32; i++)
{
sumBit += (x & (1 << i)) >> i;
}
if (sumBit == 1)
{
return true;
}
return false;
}
static void XorDataLine(uint8_t *line1, uint8_t *line2, int32_t size)
{
for (int32_t i = 0; i < size; i++)
{
line1[i] = line1[i] ^ line2[i];
}
}
static void XorParityLine(uint8_t *line1, uint8_t *line2, int32_t size)
{
for (int32_t i = 0; i < size; i++)
{
SetParity(i, line1, (GetParity(i, line1) ^ GetParity(i, line2)));
}
}
static int32_t FragPrbs23(int32_t value)
{
int32_t b0 = value & 0x01;
int32_t b1 = (value & 0x20) >> 5;
return (value >> 1) + ((b0 ^ b1) << 22);;
}
static void FragGetParityMatrixRow(int32_t n, int32_t m, uint8_t *matrixRow)
{
int32_t mTemp;
int32_t x;
int32_t nbCoeff = 0;
int32_t r;
if (IsPowerOfTwo(m) != false)
{
mTemp = 1;
}
else
{
mTemp = 0;
}
x = 1 + (1001 * n);
for (uint8_t i = 0; i < ((m >> 3) + 1); i++)
{
matrixRow[i] = 0;
}
while (nbCoeff < (m >> 1))
{
r = 1 << 16;
while (r >= m)
{
x = FragPrbs23(x);
r = x % (m + mTemp);
}
SetParity(r, matrixRow, 1);
nbCoeff += 1;
}
}
static uint16_t BitArrayFindFirstOne(uint8_t *bitArray, uint16_t size)
{
for (uint16_t i = 0; i < size; i++)
{
if (GetParity(i, bitArray) == 1)
{
return i;
}
}
return 0;
}
static uint8_t BitArrayIsAllZeros(uint8_t *bitArray, uint16_t size)
{
for (uint16_t i = 0; i < size; i++)
{
if (GetParity(i, bitArray) == 1)
{
return 0;
}
}
return 1;
}
static void FragFindMissingFrags(uint16_t counter)
{
int32_t i;
for (i = FragDecoder.Status.FragNbLastRx; i < (counter - 1); i++)
{
if (i < FragDecoder.FragNb)
{
FragDecoder.Status.FragNbLost++;
FragDecoder.FragNbMissingIndex[i] = FragDecoder.Status.FragNbLost;
}
}
if (i < FragDecoder.FragNb)
{
FragDecoder.Status.FragNbLastRx = counter;
}
else
{
FragDecoder.Status.FragNbLastRx = FragDecoder.FragNb + 1;
}
MW_LOG(TS_ON, VLEVEL_H, "RECEIVED : %5d / %5d Fragments\r\n", FragDecoder.Status.FragNbRx, FragDecoder.FragNb);
MW_LOG(TS_ON, VLEVEL_H, " %5d / %5d Bytes\r\n", FragDecoder.Status.FragNbRx * FragDecoder.FragSize,
FragDecoder.FragNb * FragDecoder.FragSize);
MW_LOG(TS_ON, VLEVEL_H, "LOST : %7d Fragments\r\n\r\n", FragDecoder.Status.FragNbLost);
}
static uint16_t FragFindMissingIndex(uint16_t x)
{
for (uint16_t i = 0; i < FragDecoder.FragNb; i++)
{
if (FragDecoder.FragNbMissingIndex[i] == (x + 1))
{
return i;
}
}
return 0;
}
static void FragExtractLineFromBinaryMatrix(uint8_t *bitArray, uint16_t rowIndex, uint16_t bitsInRow)
{
uint32_t findByte = 0;
uint32_t findBitInByte = 0;
if (rowIndex > 0)
{
findByte = (rowIndex * bitsInRow - ((rowIndex * (rowIndex - 1)) >> 1)) >> 3;
findBitInByte = (rowIndex * bitsInRow - ((rowIndex * (rowIndex - 1)) >> 1)) % 8;
}
if (rowIndex > 0)
{
for (uint16_t i = 0; i < rowIndex; i++)
{
SetParity(i, bitArray, 0);
}
}
for (uint16_t i = rowIndex; i < bitsInRow; i++)
{
SetParity(i,
bitArray,
(FragDecoder.MatrixM2B[findByte] >> (7 - findBitInByte)) & 0x01);
findBitInByte++;
if (findBitInByte == 8)
{
findBitInByte = 0;
findByte++;
}
}
}
static void FragPushLineToBinaryMatrix(uint8_t *bitArray, uint16_t rowIndex, uint16_t bitsInRow)
{
uint32_t findByte = 0;
uint32_t findBitInByte = 0;
if (rowIndex > 0)
{
findByte = (rowIndex * bitsInRow - ((rowIndex * (rowIndex - 1)) >> 1)) >> 3;
findBitInByte = (rowIndex * bitsInRow - ((rowIndex * (rowIndex - 1)) >> 1)) % 8;
}
for (uint16_t i = rowIndex; i < bitsInRow; i++)
{
if (GetParity(i, bitArray) == 0)
{
FragDecoder.MatrixM2B[findByte] = FragDecoder.MatrixM2B[findByte] & (0xFF - (1 << (7 - findBitInByte)));
}
findBitInByte++;
if (findBitInByte == 8)
{
findBitInByte = 0;
findByte++;
}
}
}
Reference in New Issue View Git Blame Copy Permalink