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