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