From 1cb67f8f8d7d8e5f2e38cedbae4e7f62e9043e0c Mon Sep 17 00:00:00 2001
From: Sean Cross <sean@xobs.io>
Date: Thu, 28 Feb 2019 10:45:37 +0800
Subject: [PATCH] crc5-test: add simple program to generate SOF frames

This can be used to tell if we're properly decoding packets.

Signed-off-by: Sean Cross <sean@xobs.io>
---
 crc5-test.c | 160 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 160 insertions(+)
 create mode 100644 crc5-test.c

diff --git a/crc5-test.c b/crc5-test.c
new file mode 100644
index 0000000..9d57cfb
--- /dev/null
+++ b/crc5-test.c
@@ -0,0 +1,160 @@
+#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(uint16_t pkt) {
+    uint8_t pkt_flipped[2] = {
+        reverse_byte(pkt >> 8),
+        reverse_byte(pkt),
+    };
+    uint32_t data = (pkt_flipped[1] >> 5) | (pkt_flipped[0] << 3);
+    uint32_t data_flipped;
+    uint8_t crc;
+    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,
+
+    };
+
+    unsigned int i;
+
+    for (i = 0; i < ARRAY_SIZE(check_bytes); i++)
+        do_crc5(check_bytes[i]);
+
+    return 0;
+}