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 enum {
  17         SB4000  = 1,
  18         FLIPXY  = 2
  19 };
  20
  21 struct sball {
  22         int fd;
  23         unsigned int flags;
  24         int nbuttons;
  25
  26         char buf[INP_BUF_SZ];
  27         int len;
  28
  29         short mot[6];
  30         unsigned int keystate;
  31
  32         int (*parse)(struct sball*, int, char*, int);
  33 };
  34
  35 static int stty_sball(int fd);
  36 static int stty_mag(int fd);
  37
  38 static int proc_input(struct sball *sb);
  39
  40 static int mag_parsepkt(struct sball *sb, int id, char *data, int len);
  41 static int sball_parsepkt(struct sball *sb, int id, char *data, int len);
  42
  43 static int guess_num_buttons(const char *verstr);
  44
  45 static void make_printable(char *buf, int len);
  46 static int read_timeout(int fd, char *buf, int bufsz, long tm_usec);
  47
  48 static void print_state(struct sball *sb);
  49
  50
  51 struct sball *sball_open(const char *dev)
  52 {
  53         int fd, sz;
  54         char buf[128];
  55         struct sball *sb = 0;
  56
  57         if((fd = open(dev, O_RDWR | O_NOCTTY)) == -1) {
  58                 fprintf(stderr, "sball_open: failed to open device: %s: %s\n", dev, strerror(errno));
  59                 return 0;
  60         }
  61
  62         if(!(sb = malloc(sizeof *sb))) {
  63                 fprintf(stderr, "sball_open: failed to allocate sball object\n");
  64                 goto err;
  65         }
  66         sb->fd = fd;
  67         sb->flags = 0;
  68         sb->len = 0;
  69
  70         if(stty_sball(fd) == -1) {
  71                 goto err;
  72         }
  73
  74         if((sz = read_timeout(fd, buf, sizeof buf - 1, 2000000)) > 0 && memcmp(buf, "\r@1", 3) == 0) {
  75                 /* we got a response, so it's a spaceball */
  76                 make_printable(buf, sz);
  77                 printf("Spaceball detected: %s\n", buf);
  78
  79                 sb->nbuttons = guess_num_buttons(buf);
  80                 printf("%d buttons\n", sb->nbuttons);
  81
  82                 /* set binary mode and enable automatic data packet sending */
  83                 write(fd, "\rCB\rMSS\r", 8);
  84
  85                 sb->parse = sball_parsepkt;
  86                 return sb;
  87         }
  88
  89         /* try as a magellan spacemouse */
  90         if(stty_mag(fd) == -1) {
  91                 goto err;
  92         }
  93         write(fd, "vQ\r", 3);
  94
  95         if((sz = read_timeout(fd, buf, sizeof buf - 1, 250000)) > 0 && buf[0] == 'v') {
  96                 make_printable(buf, sz);
  97                 printf("Magellan SpaceMouse detected:\n%s\n", buf);
  98
  99                 sb->nbuttons = guess_num_buttons(buf);
 100                 printf("%d buttons\n", sb->nbuttons);
 101
 102                 /* set 3D mode, not-dominant-axis, pass through motion and button packets */
 103                 write(fd, "m3\r", 3);
 104
 105                 sb->parse = mag_parsepkt;
 106                 return sb;
 107         }
 108
 109 err:
 110         close(fd);
 111         free(sb);
 112         return 0;
 113 }
 114
 115 void sball_close(struct sball *sb)
 116 {
 117         if(!sb) return;
 118         close(sb->fd);
 119 }
 120
 121 int sball_fd(struct sball *sb)
 122 {
 123         return sb->fd;
 124 }
 125
 126 int sball_read(struct sball *sb)
 127 {
 128         int sz;
 129
 130         while((sz = read(sb->fd, sb->buf + sb->len,  INP_BUF_SZ - sb->len - 1)) > 0) {
 131                 sb->len += sz;
 132                 proc_input(sb);
 133         }
 134
 135         /* if we fill the input buffer, make a last attempt to parse it, and discard
 136          * it so we can receive more
 137          */
 138         if(sb->len >= INP_BUF_SZ) {
 139                 proc_input(sb);
 140                 sb->len = 0;
 141         }
 142
 143         return 0;
 144 }
 145
 146 int sball_axis(struct sball *sb, int axis)
 147 {
 148         return sb->mot[axis];
 149 }
 150
 151 unsigned int sball_buttons(struct sball *sb)
 152 {
 153         return sb->keystate;
 154 }
 155
 156 int sball_num_buttons(struct sball *sb)
 157 {
 158         return sb->nbuttons;
 159 }
 160
 161 /* Labtec spaceball: 9600 8n1 XON/XOFF */
 162 static int stty_sball(int fd)
 163 {
 164         struct termios term;
 165
 166         if(tcgetattr(fd, &term) == -1) {
 167                 perror("sball_open: tcgetattr");
 168                 return -1;
 169         }
 170
 171         term.c_oflag = 0;
 172         term.c_lflag = 0;
 173         term.c_cc[VMIN] = 0;
 174         term.c_cc[VTIME] = 1;
 175
 176         term.c_cflag = CLOCAL | CREAD | CS8 | HUPCL;
 177         term.c_iflag = IGNBRK | IGNPAR | IXON | IXOFF;
 178
 179         cfsetispeed(&term, B9600);
 180         cfsetospeed(&term, B9600);
 181
 182         if(tcsetattr(fd, TCSANOW, &term) == -1) {
 183                 perror("sball_open: tcsetattr");
 184                 return -1;
 185         }
 186
 187         return 0;
 188 }
 189
 190 /* Logicad magellan spacemouse: 9600 8n2 CTS/RTS */
 191 static int stty_mag(int fd)
 192 {
 193         struct termios term;
 194
 195         if(tcgetattr(fd, &term) == -1) {
 196                 perror("sball_open: tcgetattr");
 197                 return -1;
 198         }
 199
 200         term.c_oflag = 0;
 201         term.c_lflag = 0;
 202         term.c_cc[VMIN] = 0;
 203         term.c_cc[VTIME] = 1;
 204
 205         term.c_cflag = CLOCAL | CREAD | CS8 | CSTOPB | HUPCL | CRTSCTS;
 206         term.c_iflag = IGNBRK | IGNPAR;
 207
 208         cfsetispeed(&term, B9600);
 209         cfsetospeed(&term, B9600);
 210
 211         if(tcsetattr(fd, TCSANOW, &term) == -1) {
 212                 perror("sball_open: tcsetattr");
 213                 return -1;
 214         }
 215
 216         return 0;
 217 }
 218
 219
 220 static int proc_input(struct sball *sb)
 221 {
 222         int sz;
 223         char *bptr = sb->buf;
 224         char *start = sb->buf;
 225         char *end = sb->buf + sb->len;
 226
 227         /* see if we have a CR in the buffer */
 228         while(bptr < end) {
 229                 if(*bptr == '\r') {
 230                         *bptr = 0;
 231                         sb->parse(sb, *start, start + 1, bptr - start - 1);
 232                         start = ++bptr;
 233                 } else {
 234                         bptr++;
 235                 }
 236         }
 237
 238         sz = start - sb->buf;
 239         if(sz > 0) {
 240                 memmove(sb->buf, start, sz);
 241                 sb->len -= sz;
 242         }
 243         return 0;
 244 }
 245
 246 static int mag_parsepkt(struct sball *sb, int id, char *data, int len)
 247 {
 248         int i;
 249
 250         /*printf("magellan packet: %c - %s (%d bytes)\n", (char)id, data, len);*/
 251
 252         switch(id) {
 253         case 'd':
 254                 if(len != 24) {
 255                         fprintf(stderr, "magellan: invalid data packet, expected 24 bytes, got: %d\n", len);
 256                         return -1;
 257                 }
 258                 for(i=0; i<6; i++) {
 259 #ifdef SBALL_BIG_ENDIAN
 260                         sb->mot[i] = ((((int)data[3] & 0xf) << 12) | (((int)data[2] & 0xf) << 8) |
 261                                         (((int)data[1] & 0xf) << 4) | (data[0] & 0xf)) - 0x8000;
 262 #else
 263                         sb->mot[i] = ((((int)data[0] & 0xf) << 12) | (((int)data[1] & 0xf) << 8) |
 264                                         (((int)data[2] & 0xf) << 4) | (data[3] & 0xf)) - 0x8000;
 265 #endif
 266                         data += 4;
 267                 }
 268                 print_state(sb);
 269                 break;
 270
 271         case 'k':
 272                 if(len != 3) {
 273                         fprintf(stderr, "magellan: invalid keyboard pakcet, expected 3 bytes, got: %d\n", len);
 274                         return -1;
 275                 }
 276                 sb->keystate = (data[0] & 0xf) | ((data[1] & 0xf) << 4) | (((unsigned int)data[2] & 0xf) << 8);
 277                 print_state(sb);
 278                 break;
 279
 280         case 'e':
 281                 if(data[0] == 1) {
 282                         fprintf(stderr, "magellan error: illegal command: %c%c\n", data[1], data[2]);
 283                 } else if(data[0] == 2) {
 284                         fprintf(stderr, "magellan error: framing error\n");
 285                 } else {
 286                         fprintf(stderr, "magellan error: unknown device error\n");
 287                 }
 288                 return -1;
 289
 290         default:
 291                 break;
 292         }
 293         return 0;
 294 }
 295
 296 static int sball_parsepkt(struct sball *sb, int id, char *data, int len)
 297 {
 298         int i;
 299         char c, *rd, *wr;
 300
 301         /* decode data packet, replacing escaped values with the correct ones */
 302         rd = wr = data;
 303         while(rd < data + len) {
 304                 if((c = *rd++) == '^') {
 305                         switch(*rd++) {
 306                         case 'Q':
 307                                 *wr++ = 0x11;   /* XON */
 308                                 break;
 309                         case 'S':
 310                                 *wr++ = 0x13;   /* XOFF */
 311                                 break;
 312                         case 'M':
 313                                 *wr++ = 13;             /* CR */
 314                                 break;
 315                         case '^':
 316                                 *wr++ = '^';
 317                                 break;
 318                         default:
 319                                 fprintf(stderr, "sball decode: ignoring invalid escape code: %xh\n", (unsigned int)c);
 320                         }
 321                 } else {
 322                         *wr++ = c;
 323                 }
 324         }
 325         len = wr - data;        /* update the decoded length */
 326
 327         switch(id) {
 328         case 'D':
 329                 if(len != 14) {
 330                         fprintf(stderr, "sball: invalid data packet, expected 14 bytes, got: %d\n", len);
 331                         return -1;
 332                 }
 333
 334 #ifndef SBALL_BIG_ENDIAN
 335                 for(i=0; i<6; i++) {
 336                         data += 2;
 337                         c = data[0];
 338                         data[0] = data[1];
 339                         data[1] = c;
 340                         sb->mot[i] = *(short*)data;
 341                 }
 342 #else
 343                 memcpy(sb->mot, data + 2, 12);
 344 #endif
 345                 print_state(sb);
 346                 break;
 347
 348         case 'K':
 349                 if(len != 2) {
 350                         fprintf(stderr, "sball: invalid key packet, expected 2 bytes, got: %d\n", len);
 351                         return -1;
 352                 }
 353                 if(sb->flags & SB4000) break;   /* ignore K packets from spaceball 4000 devices */
 354
 355                 /* data[1] bits 0-3 -> buttons 0,1,2,3
 356                  * data[1] bits 4,5 (3003 L/R) -> buttons 0, 1
 357                  * data[0] bits 0-2 -> buttons 4,5,6
 358                  * data[0] bit 4 is (2003 pick) -> button 7
 359                  */
 360                 sb->keystate = (data[1] & 0xf) | ((data[1] >> 4) & 3) | ((data[0] & 7) << 4) |
 361                         ((data[0] & 0x10) >> 1);
 362                 print_state(sb);
 363                 break;
 364
 365         case '.':
 366                 if(len != 2) {
 367                         fprintf(stderr, "sball: invalid sb4k key packet, expected 2 bytes, got: %d\n", len);
 368                         return -1;
 369                 }
 370                 /* spaceball 4000 key packet */
 371                 sb->flags |= SB4000;
 372                 /* update orientation flag (actually don't bother) */
 373                 /*
 374                 if(data[0] & 0x20) {
 375                         sb->flags |= FLIPXY;
 376                 } else {
 377                         sb->flags &= ~FLIPXY;
 378                 }
 379                 */
 380
 381                 /* data[1] bits 0-5 -> buttons 0,1,2,3,4,5
 382                  * data[1] bit 7 -> button 6
 383                  * data[0] bits 0-4 -> buttons 7,8,9,10,11
 384                  */
 385                 sb->keystate = (data[1] & 0x3f) | ((data[1] & 0x80) >> 1) | ((data[0] & 0x1f) << 7);
 386                 print_state(sb);
 387                 break;
 388
 389         case 'E':
 390                 fprintf(stderr, "sball: error:");
 391                 for(i=0; i<len; i++) {
 392                         if(isprint(data[i])) {
 393                                 fprintf(stderr, " %c", data[i]);
 394                         } else {
 395                                 fprintf(stderr, " %02xh", (unsigned int)data[i]);
 396                         }
 397                 }
 398                 break;
 399
 400         case 'M':       /* ignore MSS responses */
 401                 break;
 402
 403         default:
 404                 /* DEBUG */
 405                 fprintf(stderr, "sball: got '%c' packet:", (char)id);
 406                 for(i=0; i<len; i++) {
 407                         fprintf(stderr, " %02x", (unsigned int)data[i]);
 408                 }
 409                 fputc('\n', stderr);
 410         }
 411         return 0;
 412 }
 413
 414 static int guess_num_buttons(const char *verstr)
 415 {
 416         int major, minor;
 417         const char *s, *model;
 418
 419         if((s = strstr(verstr, "Firmware version"))) {  /* spaceball */
 420
 421                 /* if we got a model number, guess based on that */
 422                 if((model = strchr(s, '('))) {
 423                         if(memcmp(model, "(Model ", 7) == 0) {
 424                                 model += 7;
 425                         } else {
 426                                 model++;
 427                         }
 428                         switch(atoi(model)) {
 429                         case 2003:
 430                                 return 8;
 431                         case 3003:
 432                                 return 2;
 433                         case 5000:
 434                                 return 12;
 435                         default:
 436                                 break;
 437                         }
 438                 }
 439                 /* try to guess based on firmware number */
 440                 if(sscanf(s + 17, "%d.%d", &major, &minor) == 2 && major == 2) {
 441                         if(minor == 35 || minor == 62 || minor == 63) {
 442                                 return 2;       /* spaceball 3003/3003C */
 443                         }
 444                         if(minor == 42 || minor == 43 || minor == 45) {
 445                                 /* 2.42 is also used by spaceball 2003C, but this should be
 446                                  * caught before we get here by the model number guess
 447                                  */
 448                                 return 12;      /* spaceball 4000flx/5000flx-a */
 449                         }
 450                         if(minor == 2 || minor == 13 || minor == 15) {
 451                                 return 8;       /* spaceball 1003/2003/2003c */
 452                         }
 453                 }
 454         }
 455
 456         if(strstr(verstr, "MAGELLAN")) {
 457                 return 9; /* magellan spacemouse */
 458         }
 459
 460         if(strstr(verstr, "SPACEBALL")) {
 461                 return 12; /* spaceball 5000 */
 462         }
 463
 464         if(strstr(verstr, "CadMan")) {
 465                 return 2;
 466         }
 467
 468         fprintf(stderr, "Can't guess number of buttons, default to 8, report this as a bug!\n");
 469         return 8;
 470 }
 471
 472 static void make_printable(char *buf, int len)
 473 {
 474         int i, c;
 475         char *wr = buf;
 476
 477         for(i=0; i<len; i++) {
 478                 c = *buf++;
 479                 if(c == '\r') {
 480                         *wr++ = '\n';
 481                         while(*buf == '\n' || *buf == '\r') buf++;
 482                 } else {
 483                         *wr++ = c;
 484                 }
 485         }
 486         *wr = 0;
 487 }
 488
 489 static int read_timeout(int fd, char *buf, int bufsz, long tm_usec)
 490 {
 491         int res;
 492         long usec, sz = 0;
 493         struct timeval tv0, tv;
 494         fd_set rdset;
 495
 496         if(!buf || bufsz <= 0) return -1;
 497
 498         usec = tm_usec;
 499         gettimeofday(&tv0, 0);
 500
 501         while(sz < bufsz && usec > 0) {
 502                 tv.tv_sec = usec / 1000000;
 503                 tv.tv_usec = usec % 1000000;
 504
 505                 FD_ZERO(&rdset);
 506                 FD_SET(fd, &rdset);
 507                 if((res = select(fd + 1, &rdset, 0, 0, &tv)) > 0 && FD_ISSET(fd, &rdset)) {
 508                         sz += read(fd, buf + sz, bufsz - sz);
 509                         buf[sz] = 0;
 510                         tm_usec = usec = 128000;        /* wait 128ms for the rest of the message to appear */
 511                         gettimeofday(&tv0, 0);
 512                         continue;
 513                 }
 514                 if(res == -1 && (errno == EWOULDBLOCK || errno == EAGAIN)) {
 515                         break;
 516                 }
 517                 gettimeofday(&tv, 0);
 518                 usec = tm_usec - ((tv.tv_sec - tv0.tv_sec) * 1000000 + (tv.tv_usec - tv0.tv_usec));
 519         }
 520
 521         return sz > 0 ? sz : -1;
 522 }
 523
 524 static void print_motion(short *mot)
 525 {
 526         printf(" T[%+6d %+6d %+6d]  R[%+6d %+6d %+6d]", mot[0], mot[1],
 527                         mot[2], mot[3], mot[4], mot[5]);
 528 }
 529
 530 static void print_keystate(unsigned int keystate)
 531 {
 532         int i;
 533
 534         for(i=0; i<12; i++) {
 535                 int b = 11 - i;
 536                 int hex = b < 10 ? b + '0' : b - 10 + 'a';
 537                 putchar(keystate & (1 << b) ? hex : '-');
 538         }
 539 }
 540
 541 static void print_state(struct sball *sb)
 542 {
 543         print_motion(sb->mot);
 544         printf("  B[");
 545         print_keystate(sb->keystate);
 546         printf("]\r");
 547         fflush(stdout);
 548 }