CC1352P: DN507 FEC Decoding 解码结果与编码（DN504 FEC Implementation）不一致

/***** Includes *****/
/* Standard C Libraries */
#include <stdlib.h>

/* TI Drivers */
#include <ti/drivers/rf/RF.h>
#include <ti/drivers/GPIO.h>

/* Driverlib Header files */
#include DeviceFamily_constructPath(driverlib/rf_prop_mailbox.h)

/* Board Header files */
#include "ti_drivers_config.h"

/* Application Header files */
#include "RFQueue.h"
#include <ti_radio_config.h>

/***** Defines *****/

/* Packet RX Configuration */
#define MAX_LENGTH             12 //(Number of bytes received when transmitting payload of 5 bytes)
#define DATA_ENTRY_HEADER_SIZE 8  /* Constant header size of a Generic Data Entry */
#define NUM_DATA_ENTRIES       2  /* NOTE: Only two data entries supported at the moment */
#define NUM_APPENDED_BYTES     0



/***** Prototypes *****/
static void callback(RF_Handle h, RF_CmdHandle ch, RF_EventMask e);
static uint16_t culCalcCRC(uint8_t crcData, uint16_t crcReg);

/***** Variable declarations *****/
static RF_Object rfObject;
static RF_Handle rfHandle;

/* Buffer which contains all Data Entries for receiving data.
 * Pragmas are needed to make sure this buffer is 4 byte aligned (requirement from the RF Core) */
#if defined(__TI_COMPILER_VERSION__)
#pragma DATA_ALIGN (rxDataEntryBuffer, 4);
static uint8_t
rxDataEntryBuffer[RF_QUEUE_DATA_ENTRY_BUFFER_SIZE(NUM_DATA_ENTRIES,
                                                  MAX_LENGTH,
                                                  NUM_APPENDED_BYTES)];
#elif defined(__IAR_SYSTEMS_ICC__)
#pragma data_alignment = 4
static uint8_t
rxDataEntryBuffer[RF_QUEUE_DATA_ENTRY_BUFFER_SIZE(NUM_DATA_ENTRIES,
                                                  MAX_LENGTH,
                                                  NUM_APPENDED_BYTES)];
#elif defined(__GNUC__)
static uint8_t
rxDataEntryBuffer[RF_QUEUE_DATA_ENTRY_BUFFER_SIZE(NUM_DATA_ENTRIES,
                                                  MAX_LENGTH,
                                                  NUM_APPENDED_BYTES)]
                                                  __attribute__((aligned(4)));
#else
#error This compiler is not supported.
#endif

/* Receive dataQueue for RF Core to fill in data */
static dataQueue_t dataQueue;
static rfc_dataEntryGeneral_t* currentDataEntry;
static uint8_t* packetDataPointer;

uint16_t fecDecode(uint8_t *pDecData, uint8_t* pInData, uint16_t RemBytes);
static uint8_t hammWeight(uint8_t a);
static uint8_t min(uint8_t a, uint8_t b);
static uint16_t calcCRC(uint8_t crcData, uint16_t crcReg);
uint8_t hammWeight(uint8_t a);
uint8_t min(uint8_t a, uint8_t b);

static uint8_t packet[MAX_LENGTH];

// The payload + CRC are 31 bytes. This way the complete packet to be received will fit in the RXFIFO
uint8_t rxBuffer[4]; // Buffer used to hold data read from the RXFIFO (4 bytes are read at a time)
uint8_t rxPacket[5]; // Data + CRC after being interleaved and decoded

uint8_t aTrellisSourceStateLut[8][2] =
{
     {0, 4}, // State {0,4} -> State 0
     {0, 4}, // State {0,4} -> State 1
     {1, 5}, // State {1,5} -> State 2
     {1, 5}, // State {1,5} -> State 3
     {2, 6}, // State {2,6} -> State 4
     {2, 6}, // State {2,6} -> State 5
     {3, 7}, // State {3,7} -> State 6
     {3, 7}, // State {3,7} -> State 7
};

uint8_t aTrellisTransitionOutput[8][2] =
{
     {0, 3}, // State {0,4} -> State 0 produces {"00", "11"}
     {3, 0}, // State {0,4} -> State 1 produces {"11", "00"}
     {1, 2}, // State {1,5} -> State 2 produces {"01", "10"}
     {2, 1}, // State {1,5} -> State 3 produces {"10", "01"}
     {3, 0}, // State {2,6} -> State 4 produces {"11", "00"}
     {0, 3}, // State {2,6} -> State 5 produces {"00", "11"}
     {2, 1}, // State {3,7} -> State 6 produces {"10", "01"}
     {1, 2}, // State {3,7} -> State 7 produces {"01", "10"}
};
// Look-up input bit at encoder when:
// Destination state
uint8_t aTrellisTransitionInput[8] =
{
     0,
     1,
     0,
     1,
     0,
     1,
     0,
     1,
};

// NUMBER_OF_BYTES_AFTER_DECODING should be given the length of the payload + CRC (CRC is optional)
#define NUMBER_OF_BYTES_AFTER_DECODING 5
#define NUMBER_OF_BYTES_BEFORE_DECODING (4 * ((NUMBER_OF_BYTES_AFTER_DECODING / 2) + 1))

/***** Function definitions *****/

void *mainThread(void *arg0)
{
    RF_Params rfParams;
    RF_Params_init(&rfParams);

    GPIO_setConfig(CONFIG_GPIO_RLED, GPIO_CFG_OUT_STD | GPIO_CFG_OUT_LOW);
    GPIO_write(CONFIG_GPIO_RLED, CONFIG_GPIO_LED_OFF);

    if( RFQueue_defineQueue(&dataQueue,
                            rxDataEntryBuffer,
                            sizeof(rxDataEntryBuffer),
                            NUM_DATA_ENTRIES,
                            MAX_LENGTH + NUM_APPENDED_BYTES))
    {
        /* Failed to allocate space for all data entries */
        while(1);
    }

    /* Modify CMD_PROP_RX command for application needs */
    /* Set the Data Entity queue for received data */
    RF_cmdPropRx.pQueue = &dataQueue;
    /* Discard ignored packets from Rx queue */
    RF_cmdPropRx.rxConf.bAutoFlushIgnored = 0;
    /* Discard packets with CRC error from Rx queue */
    RF_cmdPropRx.rxConf.bAutoFlushCrcErr = 0;
    RF_cmdPropRx.rxConf.bIncludeHdr = 0;
    RF_cmdPropRx.rxConf.bAppendStatus = 0;

    RF_cmdPropRx.maxPktLen = MAX_LENGTH;

    RF_cmdPropRx.pktConf.bUseCrc = 0;   // Turn off CRC
    RF_cmdPropRx.pktConf.bVarLen = 0;   // Use fixed packet length mode (length is manually FEC encoded
    RF_cmdPropRx.syncWord = 0xD391D391; // CC1101 compatible Sync word

    /* Request access to the radio */
    rfHandle = RF_open(&rfObject, &RF_prop, (RF_RadioSetup*)&RF_cmdPropRadioDivSetup, &rfParams);

    /* Set the frequency */
    RF_postCmd(rfHandle, (RF_Op*)&RF_cmdFs, RF_PriorityNormal, NULL, 0);

    while(1)
    {
        RF_runCmd(rfHandle, (RF_Op*)&RF_cmdPropRx, RF_PriorityNormal, &callback, RF_EventRxEntryDone);

        {
            uint16_t nBytes;
            uint8_t *pDecData = rxPacket; // Destination for decoded data

            pDecData = rxPacket;

            // Perform de-interleaving and decoding (both done in the same function)
            fecDecode(NULL, NULL, 0); // The function needs to be called with a NULL pointer for

            // initialization before every packet to decode
            nBytes = NUMBER_OF_BYTES_AFTER_DECODING;
            uint8_t packetIndex = 0;
            while (nBytes > 0)
            {
                uint16_t nBytesOut;

                memcpy(rxBuffer, &packet[packetIndex], 4);
                packetIndex += 4;

                nBytesOut = fecDecode(pDecData, rxBuffer, nBytes);
                nBytes -= nBytesOut;
                pDecData += nBytesOut;
            }
        }
    }
}

void callback(RF_Handle h, RF_CmdHandle ch, RF_EventMask e)
{
    if (e & RF_EventRxEntryDone)
    {
        GPIO_toggle(CONFIG_GPIO_RLED);

        /* Get current unhandled data entry */
        currentDataEntry = RFQueue_getDataEntry();

        packetDataPointer = (uint8_t*)(&currentDataEntry->data);

        /* Copy the payload + the status byte to the packet variable */
        memcpy(packet, packetDataPointer, MAX_LENGTH);

        RFQueue_nextEntry();
    }
}

uint16_t culCalcCRC(uint8_t crcData, uint16_t crcReg) {
    uint8_t i;
    for (i = 0; i < 8; i++)
    {
        if (((crcReg & 0x8000) >> 8) ^ (crcData & 0x80))
        {
            crcReg = (crcReg << 1) ^ 0x8005;
        }
        else
        {
            crcReg = (crcReg << 1);
        }
        crcData <<= 1;
    }
    return crcReg;
}

uint8_t hammWeight(uint8_t a)
{
     a = ((a & 0xAA) >> 1) + (a & 0x55);
     a = ((a & 0xCC) >> 2) + (a & 0x33);
     a = ((a & 0xF0) >> 4) + (a & 0x0F);
     return a;
}

uint8_t min(uint8_t a, uint8_t b)
{
    return (a <= b ? a : b);
}

uint16_t fecDecode(uint8_t *pDecData, uint8_t* pInData, uint16_t nRemBytes)
{
    // Two sets of buffers (last, current) for each destination state for holding:
    static uint8_t nCost[2][8]; // Accumulated path cost
    static uint32_t aPath[2][8]; // Encoder input data (32b window)

    // Indices of (last, current) buffer for each iteration
    static uint8_t iLastBuf;
    static uint8_t iCurrBuf;

    // Number of bits in each path buffer
    static uint8_t nPathBits;

    // Variables used to hold # Viterbi iterations to run, # bytes output,
    // minimum cost for any destination state, bit index of input symbol
    uint8_t nIterations;
    uint16_t nOutputBytes = 0;
    uint8_t nMinCost;
    int8_t iBit = 8 - 2;

    // Initialize variables at start of packet (and return without doing any more)
    if (pDecData == NULL)
    {
        uint8_t n ;
        memset(nCost, 0, sizeof(nCost));
        for (n = 1; n < 8; n++)
        {
            nCost[0][n] = 100;
        }
        iLastBuf = 0;
        iCurrBuf = 1;
        nPathBits = 0;
        return 0;
    }

    {
        uint8_t aDeintData[4];
        int8_t iOut;
        int8_t iIn;

        // De-interleave received data (and change pInData to point to de-interleaved data)
        for (iOut = 0; iOut < 4; iOut++)
        {
            uint8_t dataByte = 0;
            for (iIn = 3; iIn >= 0; iIn--)
            {
                dataByte = (dataByte << 2) | ((pInData[iIn] >>( 2 * iOut)) & 0x03);
            }
            aDeintData[iOut] = dataByte;
        }
        pInData = aDeintData;
    }

    // Process up to 4 bytes of de-interleaved input data, processing one encoder symbol (2b) at a time
    for (nIterations = 16; nIterations > 0; nIterations--)
    {
        uint8_t iDestState;
        uint8_t symbol = ((*pInData) >> iBit) & 0x03;

        // Find minimum cost so that we can normalize costs (only last iteration used)
        nMinCost = 0xFF;

        // Get 2b input symbol (MSB first) and do one iteration of Viterbi decoding
        if ((iBit -= 2) < 0)
        {
            iBit = 6;
            pInData++; // Update pointer to the next byte of received data
        }

        // For each destination state in the trellis, calculate hamming costs for both possible paths into state and
        // select the one with lowest cost.
        for (iDestState = 0; iDestState < 8; iDestState++)
        {
            uint8_t nCost0;
            uint8_t nCost1;
            uint8_t iSrcState0;
            uint8_t iSrcState1;
            uint8_t nInputBit;

            nInputBit = aTrellisTransitionInput[iDestState];

            // Calculate cost of transition from each of the two source states (cost is Hamming difference between
            // received 2b symbol and expected symbol for transition)
            iSrcState0 = aTrellisSourceStateLut[iDestState][0];
            nCost0 = nCost[iLastBuf][iSrcState0];
            nCost0 += hammWeight(symbol ^ aTrellisTransitionOutput[iDestState][0]);

            iSrcState1 = aTrellisSourceStateLut[iDestState][1];
            nCost1 = nCost[iLastBuf][iSrcState1];
            nCost1 += hammWeight(symbol ^ aTrellisTransitionOutput[iDestState][1]);

            // Select transition that gives lowest cost in destination state, copy that source state's path and add
            // new decoded bit
            if (nCost0 <= nCost1)
            {
                nCost[iCurrBuf][iDestState] = nCost0;
                nMinCost = min(nMinCost, nCost0);
                aPath[iCurrBuf][iDestState] = (aPath[iLastBuf][iSrcState0] << 1) | nInputBit;
            }
            else
            {
                nCost[iCurrBuf][iDestState] = nCost1;
                nMinCost = min(nMinCost, nCost1);
                aPath[iCurrBuf][iDestState] = (aPath[iLastBuf][iSrcState1] << 1) | nInputBit;
            }
        }
        nPathBits++;

        // If trellis history is sufficiently long, output a byte of decoded data
        if (nPathBits == 32)
        {
            *pDecData++ = (aPath[iCurrBuf][0] >> 24) & 0xFF;
            nOutputBytes++;
            nPathBits -= 8;
            nRemBytes--;
        }

        // After having processed 3-symbol trellis terminator, flush out remaining data
        if ((nRemBytes <= 3) && (nPathBits == ((8 * nRemBytes) + 3)))
        {
            while (nPathBits >= 8)
            {
                *pDecData++ = (aPath[iCurrBuf][0] >> (nPathBits - 8)) & 0xFF;
                nOutputBytes++;
                nPathBits -= 8;
            }
            return nOutputBytes;
        }

        // Swap current and last buffers for next iteration
        iLastBuf = (iLastBuf + 1) % 2;
        iCurrBuf = (iCurrBuf + 1) % 2;
    }

    // Normalize costs so that minimum cost becomes 0
    {
        unsigned char iState;
        for (iState = 0; iState < 8; iState++)
        {
            nCost[iLastBuf][iState] -= nMinCost;
        }
    }
    return nOutputBytes;
 }