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

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