git.mutantstargoat.com Git - sball/blob - src/sball.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <string.h>
   4 #include <ctype.h>
   5 #include <time.h>
   6 #include <errno.h>
   7 #include <alloca.h>
   8 #include <unistd.h>
   9 #include <fcntl.h>
  10 #include <termios.h>
  11 #include <sys/select.h>
  12 #include <sys/time.h>
  13
  14 #define INP_BUF_SZ      256
  15
  16 struct sball {
  17         int fd;
  18
  19         char buf[INP_BUF_SZ];
  20         int len;
  21
  22         int (*parse)(int, char*, int);
  23 };
  24
  25 static int stty_sball(int fd);
  26 static int stty_mag(int fd);
  27
  28 static int proc_input(struct sball *sb);
  29
  30 static int mag_parsepkt(int id, char *data, int len);
  31 static int sball_parsepkt(int id, char *data, int len);
  32
  33 static void make_printable(char *buf, int len);
  34 static int read_timeout(int fd, char *buf, int bufsz, long tm_usec);
  35
  36 struct sball *sball_open(const char *dev)
  37 {
  38         int fd, sz;
  39         char buf[128];
  40         struct sball *sb = 0;
  41
  42         if((fd = open(dev, O_RDWR | O_NOCTTY)) == -1) {
  43                 fprintf(stderr, "sball_open: failed to open device: %s: %s\n", dev, strerror(errno));
  44                 return 0;
  45         }
  46
  47         if(!(sb = malloc(sizeof *sb))) {
  48                 fprintf(stderr, "sball_open: failed to allocate sball object\n");
  49                 goto err;
  50         }
  51         sb->fd = fd;
  52
  53         if(stty_sball(fd) == -1) {
  54                 goto err;
  55         }
  56
  57         if((sz = read_timeout(fd, buf, sizeof buf - 1, 2000000)) > 0 && memcmp(buf, "\r@1", 3) == 0) {
  58                 /* we got a response, so it's a spaceball */
  59                 make_printable(buf, sz);
  60                 printf("Spaceball detected: %s\n", buf);
  61
  62                 /* set SS mode */
  63                 write(fd, "MSS\r", 4);
  64
  65                 sb->parse = sball_parsepkt;
  66                 return sb;
  67         }
  68
  69         /* try as a magellan spacemouse */
  70         if(stty_mag(fd) == -1) {
  71                 goto err;
  72         }
  73         write(fd, "vQ\r", 3);
  74
  75         if((sz = read_timeout(fd, buf, sizeof buf - 1, 250000)) > 0 && buf[0] == 'v') {
  76                 make_printable(buf, sz);
  77                 printf("Magellan SpaceMouse detected: %s\n", buf);
  78
  79                 /* set 3D mode, not-dominant-axis, pass through motion and button packets */
  80                 write(fd, "m3\r", 3);
  81
  82                 sb->parse = mag_parsepkt;
  83                 return sb;
  84         }
  85
  86 err:
  87         close(fd);
  88         free(sb);
  89         return 0;
  90 }
  91
  92 void sball_close(struct sball *sb)
  93 {
  94         if(!sb) return;
  95         close(sb->fd);
  96 }
  97
  98 int sball_fd(struct sball *sb)
  99 {
 100         return sb->fd;
 101 }
 102
 103 int sball_read(struct sball *sb)
 104 {
 105         int sz;
 106
 107         while((sz = read(sb->fd, sb->buf + sb->len,  INP_BUF_SZ - sb->len - 1)) > 0) {
 108                 sb->len += sz;
 109                 proc_input(sb);
 110         }
 111
 112         /* if we fill the input buffer, make a last attempt to parse it, and discard
 113          * it so we can receive more
 114          */
 115         if(sb->len >= INP_BUF_SZ) {
 116                 proc_input(sb);
 117                 sb->len = 0;
 118         }
 119
 120         return 0;
 121 }
 122
 123 /* Labtec spaceball: 9600 8n1 XON/XOFF */
 124 static int stty_sball(int fd)
 125 {
 126         struct termios term;
 127
 128         if(tcgetattr(fd, &term) == -1) {
 129                 perror("sball_open: tcgetattr");
 130                 return -1;
 131         }
 132
 133         term.c_oflag = 0;
 134         term.c_lflag = 0;
 135         term.c_cc[VMIN] = 0;
 136         term.c_cc[VTIME] = 1;
 137
 138         term.c_cflag = CLOCAL | CREAD | CS8 | HUPCL;
 139         term.c_iflag = IGNBRK | IGNPAR | IXON | IXOFF;
 140
 141         cfsetispeed(&term, B9600);
 142         cfsetospeed(&term, B9600);
 143
 144         if(tcsetattr(fd, TCSANOW, &term) == -1) {
 145                 perror("sball_open: tcsetattr");
 146                 return -1;
 147         }
 148
 149         return 0;
 150 }
 151
 152 /* Logicad magellan spacemouse: 9600 8n2 CTS/RTS */
 153 static int stty_mag(int fd)
 154 {
 155         struct termios term;
 156
 157         if(tcgetattr(fd, &term) == -1) {
 158                 perror("sball_open: tcgetattr");
 159                 return -1;
 160         }
 161
 162         term.c_oflag = 0;
 163         term.c_lflag = 0;
 164         term.c_cc[VMIN] = 0;
 165         term.c_cc[VTIME] = 1;
 166
 167         term.c_cflag = CLOCAL | CREAD | CS8 | CSTOPB | HUPCL | CRTSCTS;
 168         term.c_iflag = IGNBRK | IGNPAR;
 169
 170         cfsetispeed(&term, B9600);
 171         cfsetospeed(&term, B9600);
 172
 173         if(tcsetattr(fd, TCSANOW, &term) == -1) {
 174                 perror("sball_open: tcsetattr");
 175                 return -1;
 176         }
 177
 178         return 0;
 179 }
 180
 181
 182 static int proc_input(struct sball *sb)
 183 {
 184         int sz;
 185         char *bptr = sb->buf;
 186         char *start = sb->buf;
 187         char *end = sb->buf + sb->len;
 188
 189         /* see if we have a CR in the buffer */
 190         while(bptr < end) {
 191                 if(*bptr == '\r') {
 192                         *bptr = 0;
 193                         sb->parse(*start, start + 1, bptr - start - 1);
 194                         start = ++bptr;
 195                 } else {
 196                         bptr++;
 197                 }
 198         }
 199
 200         sz = start - sb->buf;
 201         if(sz > 0) {
 202                 memmove(sb->buf, start, sz);
 203                 sb->len -= sz;
 204         }
 205         return 0;
 206 }
 207
 208 static void print_keystate(unsigned int keystate)
 209 {
 210         int i;
 211
 212         printf("keystate: ");
 213         for(i=0; i<12; i++) {
 214                 int b = 11 - i;
 215                 int hex = b < 10 ? b + '0' : b - 10 + 'a';
 216                 putchar(keystate & (1 << b) ? hex : '-');
 217         }
 218         putchar('\n');
 219 }
 220
 221 static int mag_parsepkt(int id, char *data, int len)
 222 {
 223         int i, mot[6];
 224         unsigned int keystate;
 225
 226         /*printf("magellan packet: %c - %s (%d bytes)\n", (char)id, data, len);*/
 227
 228         switch(id) {
 229         case 'd':
 230                 if(len != 24) {
 231                         fprintf(stderr, "magellan: invalid data packet, expected 24 bytes, got: %d\n", len);
 232                         return -1;
 233                 }
 234                 for(i=0; i<6; i++) {
 235                         mot[i] = ((((int)data[0] & 0xf) << 12) | (((int)data[1] & 0xf) << 8) |
 236                                         (((int)data[2] & 0xf) << 4) | (data[3] & 0xf)) - 0x8000;
 237                         data += 4;
 238                 }
 239                 printf("motion: T %+4d %+4d %+4d  R %+4d %+4d %+4d\n", mot[0], mot[1], mot[2], mot[3], mot[4], mot[5]);
 240                 break;
 241
 242         case 'k':
 243                 if(len != 3) {
 244                         fprintf(stderr, "magellan: invalid keyboard pakcet, expected 3 bytes, got: %d\n", len);
 245                         return -1;
 246                 }
 247                 keystate = (data[0] & 0xf) | ((data[1] & 0xf) << 4) | (((unsigned int)data[2] & 0xf) << 8);
 248                 print_keystate(keystate);
 249                 break;
 250
 251         case 'e':
 252                 if(data[0] == 1) {
 253                         fprintf(stderr, "magellan error: illegal command: %c%c\n", data[1], data[2]);
 254                 } else if(data[0] == 2) {
 255                         fprintf(stderr, "magellan error: framing error\n");
 256                 } else {
 257                         fprintf(stderr, "magellan error: unknown device error\n");
 258                 }
 259                 return -1;
 260
 261         default:
 262                 break;
 263         }
 264         return 0;
 265 }
 266
 267 static int sball_parsepkt(int id, char *data, int len)
 268 {
 269         int i;
 270         unsigned int keystate;
 271         char c, *rd, *wr;
 272         short *mot;
 273
 274         /* decode data packet, replacing escaped values with the correct ones */
 275         rd = wr = data;
 276         while(rd < data + len) {
 277                 if((c = *rd++) == '^') {
 278                         switch(*rd++) {
 279                         case 'Q':
 280                                 *wr++ = 0x11;   /* XON */
 281                                 break;
 282                         case 'S':
 283                                 *wr++ = 0x13;   /* XOFF */
 284                                 break;
 285                         case 'M':
 286                                 *wr++ = 13;             /* CR */
 287                                 break;
 288                         case '^':
 289                                 *wr++ = '^';
 290                         default:
 291                                 fprintf(stderr, "sball decode: ignoring invalid escape code: %xh\n", (unsigned int)c);
 292                         }
 293                 } else {
 294                         *wr++ = c;
 295                 }
 296         }
 297         len = wr - data;        /* update the decoded length */
 298
 299         switch(id) {
 300         case 'D':
 301                 if(len != 14) {
 302                         fprintf(stderr, "sball: invalid data packet, expected 14 bytes, got: %d\n", len);
 303                         return -1;
 304                 }
 305
 306                 mot = (short*)(data + 2);       /* skip the period */
 307                 printf("motion: T %+6d %+6d %+6d  R %+6d %+6d %+6d\n", mot[0], mot[1], mot[2], mot[3], mot[4], mot[5]);
 308                 break;
 309
 310         case 'K':
 311                 if(len != 2) {
 312                         fprintf(stderr, "sball: invalid key packet, expected 2 bytes, got: %d\n", len);
 313                         return -1;
 314                 }
 315                 keystate = (data[1] & 0x30) >> 4;
 316                 print_keystate(keystate);
 317                 break;
 318
 319         case 'E':
 320                 fprintf(stderr, "sball: error:");
 321                 for(i=0; i<len; i++) {
 322                         if(isprint(data[i])) {
 323                                 fprintf(stderr, " %c", data[i]);
 324                         } else {
 325                                 fprintf(stderr, " %02xh", (unsigned int)data[i]);
 326                         }
 327                 }
 328                 break;
 329
 330         default:
 331                 /* DEBUG */
 332                 fprintf(stderr, "sball: got '%c' packet:", (char)id);
 333                 for(i=0; i<len; i++) {
 334                         fprintf(stderr, " %02x", (unsigned int)data[i]);
 335                 }
 336                 fputc('\n', stderr);
 337         }
 338         return 0;
 339 }
 340
 341 static void make_printable(char *buf, int len)
 342 {
 343         int i, c;
 344         char *wr = buf;
 345
 346         for(i=0; i<len; i++) {
 347                 c = *buf++;
 348                 if(c == '\r') {
 349                         *wr++ = '\n';
 350                         while(*buf == '\n' || *buf == '\r') buf++;
 351                 } else {
 352                         *wr++ = c;
 353                 }
 354         }
 355         *wr = 0;
 356 }
 357
 358 static int read_timeout(int fd, char *buf, int bufsz, long tm_usec)
 359 {
 360         int res;
 361         long usec, sz = 0;
 362         struct timeval tv0, tv;
 363         fd_set rdset;
 364
 365         if(!buf || bufsz <= 0) return -1;
 366
 367         usec = tm_usec;
 368         gettimeofday(&tv0, 0);
 369
 370         while(sz < bufsz && usec > 0) {
 371                 tv.tv_sec = usec / 1000000;
 372                 tv.tv_usec = usec % 1000000;
 373
 374                 FD_ZERO(&rdset);
 375                 FD_SET(fd, &rdset);
 376                 if((res = select(fd + 1, &rdset, 0, 0, &tv)) > 0 && FD_ISSET(fd, &rdset)) {
 377                         sz += read(fd, buf + sz, bufsz - sz);
 378                         buf[sz] = 0;
 379                         tm_usec = usec = 128000;        /* wait 128ms for the rest of the message to appear */
 380                         gettimeofday(&tv0, 0);
 381                         continue;
 382                 }
 383                 if(res == -1 && (errno == EWOULDBLOCK || errno == EAGAIN)) {
 384                         break;
 385                 }
 386                 gettimeofday(&tv, 0);
 387                 usec = tm_usec - ((tv.tv_sec - tv0.tv_sec) * 1000000 + (tv.tv_usec - tv0.tv_usec));
 388         }
 389
 390         return sz > 0 ? sz : -1;
 391 }
 392