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         saddr6.sin6_port = htons(6000);
  65
  66         // TODO: open serial port
  67
  68         string buf;
  69
  70         for ( ;; ) {
  71                 char ch;
  72                 int ret = read(0, &ch, 1);
  73                 if (ret == -1) {
  74                         perror("read");
  75                         exit(1);
  76                 }
  77                 if (ret == 0) {
  78                         fprintf(stderr, "short read\n");
  79                         exit(1);
  80                 }
  81
  82                 if (ch == 1) {  // SOH
  83                         buf = ch;
  84                         continue;
  85                 }
  86                 if (buf.size() == 1) {
  87                         // Address
  88                         buf.push_back(ch);
  89                         continue;
  90                 }
  91                 if (ch == 2) {  // STX
  92                         if (buf.size() == 2) {
  93                                 buf.push_back(ch);
  94                         } else {
  95                                 buf.clear();  // STX out-of-order
  96                         }
  97                         continue;
  98                 }
  99
 100                 if (!buf.empty() && buf.back() == 3) {  // Last was ETX, so this is LTC.
 101                         buf.push_back(ch);
 102                         process(buf);
 103                         buf.clear();
 104                         continue;
 105                 }
 106
 107                 if (ch == 3) {  // ETX
 108                         if (buf.size() >= 4) {
 109                                 buf.push_back(ch);
 110                         } else {
 111                                 buf.clear();  // ETX out-of-order
 112                         }
 113                         continue;
 114                 }
 115                 buf.push_back(ch);
 116         }
 117 }