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