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