test-bsb/parse.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <stdint.h>
   4
   5 typedef unsigned char byte;
   6 typedef unsigned int uint;
   7 typedef uint16_t u16;
   8
   9 uint rd(void)
  10 {
  11         uint x;
  12         if (scanf("%x", &x) == 1)
  13                 return x;
  14         else {
  15                 puts("EOF");
  16                 exit(1);
  17         }
  18 }
  19
  20 u16 crc_update(u16 crc, byte data)
  21 {
  22         crc = crc ^ (data << 8);
  23         for (uint i=0; i<8; i++) {
  24                 if (crc & 0x8000)
  25                         crc = (crc << 1) ^ 0x1021;
  26                 else
  27                         crc <<= 1;
  28         }
  29         return crc;
  30 }
  31
  32 int main(void)
  33 {
  34         byte pkt[256];
  35
  36         for (;;) {
  37                 pkt[0] = rd();
  38                 if (pkt[0] != 0xdc) {
  39                         printf("# Bad sync: %02x\n", pkt[0]);
  40                         continue;
  41                 }
  42
  43                 int len = 1;
  44                 while (len < 4 || len < pkt[3]) {
  45                         pkt[len++] = rd();
  46                 }
  47
  48                 for (int i=0; i<len; i++)
  49                         printf("%02x ", pkt[i]);
  50
  51                 u16 crc = 0;
  52                 for (int i=0; i<len; i++)
  53                         crc = crc_update(crc, pkt[i]);
  54                 if (crc) {
  55                         printf("[BAD CRC]\n");
  56                         continue;
  57                 }
  58                 len -= 2;
  59
  60                 if (len < 9) {
  61                         printf("[TOO SHORT]\n");
  62                         continue;
  63                 }
  64
  65                 printf("\n\t%02x -> %02x: ", pkt[1] ^ 0x80, pkt[2]);
  66                 switch (pkt[4]) {
  67                         case 2:
  68                                 printf("INFO %04x:%04x =", (pkt[5]<<8) | pkt[6], (pkt[7]<<8) | pkt[8]);
  69                                 for (int i=9; i<len; i++)
  70                                         printf(" %02x", pkt[i]);
  71                                 putchar('\n');
  72                                 break;
  73                         case 6:
  74                                 printf("GET %04x:%04x\n", (pkt[6]<<8) | pkt[5], (pkt[7]<<8) | pkt[8]);
  75                                 break;
  76                         case 7:
  77                                 printf("RET %04x:%04x =", (pkt[5]<<8) | pkt[6], (pkt[7]<<8) | pkt[8]);
  78                                 for (int i=9; i<len; i++)
  79                                         printf(" %02x", pkt[i]);
  80                                 putchar('\n');
  81                                 break;
  82                         default:
  83                                 printf("??? type=%02x\n", pkt[4]);
  84                 }
  85         }
  86 }