foboot/hw/crc5-test.c

#include <stdio.h>
#include <stdint.h>

const uint8_t crc5Table4[] =
        {
            0x00, 0x0E, 0x1C, 0x12, 0x11, 0x1F, 0x0D, 0x03,
            0x0B, 0x05, 0x17, 0x19, 0x1A, 0x14, 0x06, 0x08};
const uint8_t crc5Table0[] =
        {
            0x00, 0x16, 0x05, 0x13, 0x0A, 0x1C, 0x0F, 0x19,
            0x14, 0x02, 0x11, 0x07, 0x1E, 0x08, 0x1B, 0x0D};
//---------------
int crc5Check(const uint8_t *data)
//---------------
{
    uint8_t b = data[0] ^ 0x1F;
    uint8_t crc = crc5Table4[b & 0x0F] ^ crc5Table0[(b >> 4) & 0x0F];
    b = data[1] ^ crc;
    return (crc5Table4[b & 0x0F] ^ crc5Table0[(b >> 4) & 0x0F]) == 0x06;
}
//  crc5Check

int do_check(uint16_t pkt) {
    uint8_t data[2] = {
        pkt >> 8,
        pkt,
    };
    return crc5Check(data);
}

#define INT_SIZE 32
unsigned CRC5(unsigned dwInput, int iBitcnt)
{
    const uint32_t poly5 = (0x05 << (INT_SIZE-5));
    uint32_t crc5  = (0x1f << (INT_SIZE-5));
    uint32_t udata = (dwInput << (INT_SIZE-iBitcnt));

    if ( (iBitcnt<1) || (iBitcnt>INT_SIZE) )    // Validate iBitcnt
        return 0xffffffff;

    while (iBitcnt--)
    {
        if ( (udata ^ crc5) & (0x1<<(INT_SIZE-1)) ) // bit4 != bit4?
        {
            crc5 <<= 1;
            crc5 ^= poly5;
        }
        else
            crc5 <<= 1;

        udata <<= 1;
    }

    // Shift back into position
    crc5 >>= (INT_SIZE-5);

    // Invert contents to generate crc field
    crc5 ^= 0x1f;

    return crc5;
} //CRC5()

static uint32_t reverse_sof(uint32_t data) {
    int i;
    uint32_t data_flipped = 0;
    for (i = 0; i < 11; i++)
        if (data & (1 << i))
            data_flipped |= 1 << (10 - i);

    return data_flipped;
}

static uint8_t reverse_byte(uint8_t data) {
    int i;
    uint8_t data_flipped = 0;
    for (i = 0; i < 8; i++)
        if (data & (1 << i))
            data_flipped |= 1 << (7 - i);

    return data_flipped;
}

static uint8_t reverse_crc5(uint8_t data) {
    int i;
    uint8_t data_flipped = 0;
    for (i = 0; i < 5; i++)
        if (data & (1 << i))
            data_flipped |= 1 << (4 - i);

    return data_flipped;
}

static uint16_t make_token(uint16_t data) {
    uint16_t val = 0;

    data = reverse_sof(data);
    val = data << 5;
    val |= CRC5(data, 11);

    return (reverse_byte(val >> 8) << 8) | reverse_byte(val);
}

int do_crc5(uint8_t bfr[2]) {
    uint8_t pkt_flipped[2] = {
        reverse_byte(bfr[0]),
        reverse_byte(bfr[1]),
    };
    uint32_t data = (pkt_flipped[1] >> 5) | (pkt_flipped[0] << 3);
    uint32_t data_flipped;
    uint8_t crc;
    uint16_t pkt;
    ((uint8_t *)&pkt)[0] = bfr[1];
    ((uint8_t *)&pkt)[1] = bfr[0];
    uint8_t found_crc = (pkt >> 3) & 0x1f;

    data_flipped = reverse_sof(data);

    crc = CRC5(data, 11);
    crc = reverse_crc5(crc);

    uint16_t reconstructed = make_token(data_flipped);
    uint16_t wire = (reverse_byte(pkt >> 8) << 8) | reverse_byte(pkt);

    printf("Packet: 0x%04x  FCRC: %02x  Data: 0x%04x  "
    "Flipped: 0x%04x  CRC5: 0x%02x  Pass? %d  Reconstructed: 0x%04x  Wire: %04x\n",
        pkt, found_crc, data, data_flipped, crc, do_check(pkt),
        reconstructed,
        wire
        );

    return crc;
}

#define ARRAY_SIZE(x) (sizeof(x) / sizeof(*x))
int main(int argc, char **argv)
{
    // uint32_t check_bytes[] = {
    //     /*
    //     0xff3c,
    //     0x12c5,
    //     0xe17e,
    //     0x19f5,
    //     0x0225,
    //     0x0165,
    //     0x009d,
    //     0x102f,
    //     make_token(1013),
    //     make_token(1429),
    //     make_token(100),
    //     */
    //     0x82bc,
    //     make_token(0x0483),//0x5fde,
    //     0x843c,

    // };
    uint8_t check_bytes[][2] = {
        {0x82, 0xbc},
        {0x83, 0x44},
        {0x84, 0x3c},
    };

    unsigned int i;

    for (i = 0; i < ARRAY_SIZE(check_bytes); i++)
        do_crc5(check_bytes[i]);

    return 0;
}