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

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <string.h>
   4 #include <time.h>
   5 #include <errno.h>
   6 #include <unistd.h>
   7 #include <fcntl.h>
   8 #include <termios.h>
   9 #include <sys/select.h>
  10 #include <sys/time.h>
  11
  12 #define INP_BUF_SZ      256
  13
  14 struct sball {
  15         int fd;
  16
  17         char buf[INP_BUF_SZ];
  18         int len;
  19
  20         int (*proc_input)(struct sball *sb);
  21 };
  22
  23 static int stty_sball(int fd);
  24 static int stty_mag(int fd);
  25
  26 static int proc_sball(struct sball *sb);
  27 static int proc_mag(struct sball *sb);
  28
  29 static int mag_parsepkt(int id, char *data, int len);
  30
  31 static void make_printable(char *buf, int len);
  32 static int read_timeout(int fd, char *buf, int bufsz, long tm_usec);
  33
  34 struct sball *sball_open(const char *dev)
  35 {
  36         int fd, sz;
  37         char buf[128];
  38         struct sball *sb = 0;
  39
  40         if((fd = open(dev, O_RDWR | O_NOCTTY)) == -1) {
  41                 fprintf(stderr, "sball_open: failed to open device: %s: %s\n", dev, strerror(errno));
  42                 return 0;
  43         }
  44
  45         if(!(sb = malloc(sizeof *sb))) {
  46                 fprintf(stderr, "sball_open: failed to allocate sball object\n");
  47                 goto err;
  48         }
  49         sb->fd = fd;
  50
  51         if(stty_sball(fd) == -1) {
  52                 goto err;
  53         }
  54
  55         if((sz = read_timeout(fd, buf, sizeof buf - 1, 2000000)) > 0) {
  56                 /* we got a response, so it's a spaceball */
  57                 make_printable(buf, sz);
  58                 printf("Spaceball detected: %s\n", buf);
  59                 /* TODO: improve detection, verify it's a correct reset response */
  60
  61                 sb->proc_input = proc_sball;
  62                 return sb;
  63         }
  64
  65         /* try as a magellan spacemouse */
  66         if(stty_mag(fd) == -1) {
  67                 goto err;
  68         }
  69         write(fd, "vQ\r", 3);
  70
  71         if((sz = read_timeout(fd, buf, sizeof buf - 1, 250000)) > 0) {
  72                 /* we got a response, it's a magellan spacemouse */
  73                 make_printable(buf, sz);
  74                 printf("Magellan SpaceMouse detected: %s\n", buf);
  75
  76                 /* set 3D mode, not-dominant-axis, pass through motion and button packets */
  77                 write(fd, "m3\r", 3);
  78
  79                 sb->proc_input = proc_mag;
  80                 return sb;
  81         }
  82
  83 err:
  84         close(fd);
  85         free(sb);
  86         return 0;
  87 }
  88
  89 void sball_close(struct sball *sb)
  90 {
  91         if(!sb) return;
  92         close(sb->fd);
  93 }
  94
  95 int sball_fd(struct sball *sb)
  96 {
  97         return sb->fd;
  98 }
  99
 100 int sball_read(struct sball *sb)
 101 {
 102         int sz;
 103
 104         while((sz = read(sb->fd, sb->buf + sb->len,  INP_BUF_SZ - sb->len - 1)) > 0) {
 105                 sb->len += sz;
 106                 sb->proc_input(sb);
 107         }
 108
 109         /* if we fill the input buffer, make a last attempt to parse it, and discard
 110          * it so we can receive more
 111          */
 112         if(sb->len >= INP_BUF_SZ) {
 113                 sb->proc_input(sb);
 114                 sb->len = 0;
 115         }
 116
 117         return 0;
 118 }
 119
 120 /* Labtec spaceball: 9600 8n1 XON/XOFF */
 121 static int stty_sball(int fd)
 122 {
 123         struct termios term;
 124
 125         if(tcgetattr(fd, &term) == -1) {
 126                 perror("sball_open: tcgetattr");
 127                 return -1;
 128         }
 129
 130         term.c_oflag = 0;
 131         term.c_lflag = 0;
 132         term.c_cc[VMIN] = 0;
 133         term.c_cc[VTIME] = 1;
 134
 135         term.c_cflag = CLOCAL | CREAD | CS8 | HUPCL;
 136         term.c_iflag = IGNBRK | IGNPAR | IXON | IXOFF;
 137
 138         cfsetispeed(&term, B9600);
 139         cfsetospeed(&term, B9600);
 140
 141         if(tcsetattr(fd, TCSANOW, &term) == -1) {
 142                 perror("sball_open: tcsetattr");
 143                 return -1;
 144         }
 145
 146         return 0;
 147 }
 148
 149 /* Logicad magellan spacemouse: 9600 8n2 CTS/RTS */
 150 static int stty_mag(int fd)
 151 {
 152         struct termios term;
 153
 154         if(tcgetattr(fd, &term) == -1) {
 155                 perror("sball_open: tcgetattr");
 156                 return -1;
 157         }
 158
 159         term.c_oflag = 0;
 160         term.c_lflag = 0;
 161         term.c_cc[VMIN] = 0;
 162         term.c_cc[VTIME] = 1;
 163
 164         term.c_cflag = CLOCAL | CREAD | CS8 | CSTOPB | HUPCL | CRTSCTS;
 165         term.c_iflag = IGNBRK | IGNPAR;
 166
 167         cfsetispeed(&term, B9600);
 168         cfsetospeed(&term, B9600);
 169
 170         if(tcsetattr(fd, TCSANOW, &term) == -1) {
 171                 perror("sball_open: tcsetattr");
 172                 return -1;
 173         }
 174
 175         return 0;
 176 }
 177
 178
 179 static int proc_sball(struct sball *sb)
 180 {
 181         return -1;
 182 }
 183
 184 static int proc_mag(struct sball *sb)
 185 {
 186         int sz;
 187         char *bptr = sb->buf;
 188         char *start = sb->buf;
 189         char *end = sb->buf + sb->len;
 190
 191         /* see if we have a CR in the buffer */
 192         while(bptr < end) {
 193                 if(*bptr == '\r') {
 194                         *bptr = 0;
 195                         mag_parsepkt(*start, start + 1, bptr - start - 1);
 196                         start = ++bptr;
 197                 } else {
 198                         bptr++;
 199                 }
 200         }
 201
 202         sz = start - sb->buf;
 203         if(sz > 0) {
 204                 memmove(sb->buf, start, sz);
 205                 sb->len -= sz;
 206         }
 207         return 0;
 208 }
 209
 210 static int mag_parsepkt(int id, char *data, int len)
 211 {
 212         int i, mot[6];
 213         unsigned int keystate;
 214
 215         /*printf("magellan packet: %c - %s (%d bytes)\n", (char)id, data, len);*/
 216
 217         switch(id) {
 218         case 'd':
 219                 if(len != 24) {
 220                         fprintf(stderr, "magellan: invalid data packet, expected 24 bytes, got: %d\n", len);
 221                         return -1;
 222                 }
 223                 for(i=0; i<6; i++) {
 224                         mot[i] = ((((int)data[0] & 0xf) << 12) | (((int)data[1] & 0xf) << 8) |
 225                                         (((int)data[2] & 0xf) << 4) | (data[3] & 0xf)) - 0x8000;
 226                         data += 4;
 227                 }
 228                 printf("motion: T %+4d %+4d %+4d  R %+4d %+4d %+4d\n", mot[0], mot[1], mot[2], mot[3], mot[4], mot[5]);
 229                 break;
 230
 231         case 'k':
 232                 if(len != 3) {
 233                         fprintf(stderr, "magellan: invalid keyboard pakcet, expected 3 bytes, got: %d\n", len);
 234                         return -1;
 235                 }
 236                 keystate = (data[0] & 0xf) | ((data[1] & 0xf) << 4) | (((unsigned int)data[2] & 0xf) << 8);
 237                 printf("keystate: ");
 238                 for(i=0; i<12; i++) {
 239                         int b = 11 - i;
 240                         int hex = b < 10 ? b + '0' : b - 10 + 'a';
 241                         putchar(keystate & (1 << b) ? hex : '-');
 242                 }
 243                 putchar('\n');
 244                 break;
 245
 246         case 'e':
 247                 if(data[0] == 1) {
 248                         fprintf(stderr, "magellan error: illegal command: %c%c\n", data[1], data[2]);
 249                 } else if(data[0] == 2) {
 250                         fprintf(stderr, "magellan error: framing error\n");
 251                 } else {
 252                         fprintf(stderr, "magellan error: unknown device error\n");
 253                 }
 254                 return -1;
 255
 256         default:
 257                 break;
 258         }
 259         return 0;
 260 }
 261
 262 static void make_printable(char *buf, int len)
 263 {
 264         int i, c;
 265         char *wr = buf;
 266
 267         for(i=0; i<len; i++) {
 268                 c = *buf++;
 269                 if(c == '\r') {
 270                         *wr++ = '\n';
 271                         if(*buf == '\n') buf++;
 272                 } else {
 273                         *wr++ = c;
 274                 }
 275         }
 276         *wr = 0;
 277 }
 278
 279 static int read_timeout(int fd, char *buf, int bufsz, long tm_usec)
 280 {
 281         int res;
 282         long usec, sz = 0;
 283         struct timeval tv0, tv;
 284         fd_set rdset;
 285
 286         if(!buf || bufsz <= 0) return -1;
 287
 288         usec = tm_usec;
 289         gettimeofday(&tv0, 0);
 290
 291         while(sz < bufsz && usec > 0) {
 292                 tv.tv_sec = usec / 1000000;
 293                 tv.tv_usec = usec % 1000000;
 294
 295                 FD_ZERO(&rdset);
 296                 FD_SET(fd, &rdset);
 297                 if((res = select(fd + 1, &rdset, 0, 0, &tv)) > 0 && FD_ISSET(fd, &rdset)) {
 298                         sz += read(fd, buf + sz, bufsz - sz);
 299                         buf[sz] = 0;
 300                         tm_usec = usec = 128000;        /* wait 128ms for the rest of the message to appear */
 301                         gettimeofday(&tv0, 0);
 302                         continue;
 303                 }
 304                 if(res == -1 && (errno == EWOULDBLOCK || errno == EAGAIN)) {
 305                         break;
 306                 }
 307                 gettimeofday(&tv, 0);
 308                 usec = tm_usec - ((tv.tv_sec - tv0.tv_sec) * 1000000 + (tv.tv_usec - tv0.tv_usec));
 309         }
 310
 311         return sz > 0 ? sz : -1;
 312 }
 313