git.sesse.net Git - ultimatescore/blob - client/bodet.cpp

   1 // Bodet BT-6000 decoder.
   2
   3 #include <stdio.h>
   4 #include <string.h>
   5 #include <unistd.h>
   6
   7 #include <string>
   8 #include <vector>
   9
  10 #include <arpa/inet.h>
  11 #include <netinet/in.h>
  12 #include <netinet/tcp.h>
  13 #include <stdlib.h>
  14 #include <sys/types.h>
  15 #include <sys/socket.h>
  16
  17 using namespace std;
  18
  19 int sock;
  20 sockaddr_in6 saddr6;
  21
  22 void process(const string &buf)
  23 {
  24         unsigned char checksum = 0;
  25         for (size_t i = 1; i <= buf.size() - 2; ++i) {
  26                 checksum ^= buf[i];
  27         }
  28         checksum &= 0x7f;
  29         if (checksum < 0x20) {
  30                 checksum += 0x20;
  31         }
  32         if (checksum != buf.back()) {
  33         //      fprintf(stderr, "discarding message with broken checksum: [%s] [%x vs. %x]\n", buf.c_str(), checksum, buf.back());
  34         } else {
  35                 string realmsg = buf.substr(3, buf.size() - 5);
  36                 sendto(sock, realmsg.data(), realmsg.size(), 0, (sockaddr *)&saddr6, sizeof(saddr6));
  37         }
  38 }
  39
  40 int main(int argc, char **argv)
  41 {
  42         sock = socket(PF_INET6, SOCK_DGRAM, IPPROTO_UDP);
  43         if (sock == -1) {
  44                 perror("socket");
  45                 exit(1);
  46         }
  47
  48         int one = 1;
  49         if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) == -1) {
  50                 perror("setsockopt");
  51                 exit(1);
  52         }
  53
  54         memset(&saddr6, 0, sizeof(saddr6));
  55         saddr6.sin6_family = AF_INET6;
  56         if (argc >= 2) {
  57                 if (inet_pton(AF_INET6, argv[1], &saddr6.sin6_addr) != 1) {
  58                         fprintf(stderr, "Invalid address '%s'\n", argv[1]);
  59                         exit(1);
  60                 }
  61         } else {
  62                 inet_pton(AF_INET6, "::1", &saddr6.sin6_addr);
  63         }
  64
  65         int port = 6000;
  66         if (argc >= 3) {
  67                 port = atoi(argv[2]);
  68         }
  69         saddr6.sin6_port = htons(port);
  70
  71         // TODO: open serial port
  72
  73         string buf;
  74
  75         for ( ;; ) {
  76                 char ch;
  77                 int ret = read(0, &ch, 1);
  78                 if (ret == -1) {
  79                         perror("read");
  80                         exit(1);
  81                 }
  82                 if (ret == 0) {
  83                         fprintf(stderr, "short read\n");
  84                         exit(1);
  85                 }
  86
  87                 if (ch == 1) {  // SOH
  88                         buf = ch;
  89                         continue;
  90                 }
  91                 if (buf.size() == 1) {
  92                         // Address
  93                         buf.push_back(ch);
  94                         continue;
  95                 }
  96                 if (ch == 2) {  // STX
  97                         if (buf.size() == 2) {
  98                                 buf.push_back(ch);
  99                         } else {
 100                                 buf.clear();  // STX out-of-order
 101                         }
 102                         continue;
 103                 }
 104
 105                 if (!buf.empty() && buf.back() == 3) {  // Last was ETX, so this is LTC.
 106                         buf.push_back(ch);
 107                         process(buf);
 108                         buf.clear();
 109                         continue;
 110                 }
 111
 112                 if (ch == 3) {  // ETX
 113                         if (buf.size() >= 4) {
 114                                 buf.push_back(ch);
 115                         } else {
 116                                 buf.clear();  // ETX out-of-order
 117                         }
 118                         continue;
 119                 }
 120                 buf.push_back(ch);
 121         }
 122 }