initial spaceball parsing implementation

diff --git a/src/sball.c b/src/sball.c
index e51ee3e..b52d896 100644 (file)
--- a/src/sball.c
+++ b/src/sball.c
@@ -1,8 +1,10 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <ctype.h>
 #include <time.h>
 #include <errno.h>
+#include <alloca.h>
 #include <unistd.h>
 #include <fcntl.h>
 #include <termios.h>
@@ -17,16 +19,16 @@ struct sball {
        char buf[INP_BUF_SZ];
        int len;
 
-       int (*proc_input)(struct sball *sb);
+       int (*parse)(int, char*, int);
 };
 
 static int stty_sball(int fd);
 static int stty_mag(int fd);
 
-static int proc_sball(struct sball *sb);
-static int proc_mag(struct sball *sb);
+static int proc_input(struct sball *sb);
 
 static int mag_parsepkt(int id, char *data, int len);
+static int sball_parsepkt(int id, char *data, int len);
 
 static void make_printable(char *buf, int len);
 static int read_timeout(int fd, char *buf, int bufsz, long tm_usec);
@@ -52,13 +54,15 @@ struct sball *sball_open(const char *dev)
                goto err;
        }
 
-       if((sz = read_timeout(fd, buf, sizeof buf - 1, 2000000)) > 0) {
+       if((sz = read_timeout(fd, buf, sizeof buf - 1, 2000000)) > 0 && memcmp(buf, "\r@1", 3) == 0) {
                /* we got a response, so it's a spaceball */
                make_printable(buf, sz);
                printf("Spaceball detected: %s\n", buf);
-               /* TODO: improve detection, verify it's a correct reset response */
 
-               sb->proc_input = proc_sball;
+               /* set SS mode */
+               write(fd, "MSS\r", 4);
+
+               sb->parse = sball_parsepkt;
                return sb;
        }
 
@@ -68,15 +72,14 @@ struct sball *sball_open(const char *dev)
        }
        write(fd, "vQ\r", 3);
 
-       if((sz = read_timeout(fd, buf, sizeof buf - 1, 250000)) > 0) {
-               /* we got a response, it's a magellan spacemouse */
+       if((sz = read_timeout(fd, buf, sizeof buf - 1, 250000)) > 0 && buf[0] == 'v') {
                make_printable(buf, sz);
                printf("Magellan SpaceMouse detected: %s\n", buf);
 
                /* set 3D mode, not-dominant-axis, pass through motion and button packets */
                write(fd, "m3\r", 3);
 
-               sb->proc_input = proc_mag;
+               sb->parse = mag_parsepkt;
                return sb;
        }
 
@@ -103,14 +106,14 @@ int sball_read(struct sball *sb)
 
        while((sz = read(sb->fd, sb->buf + sb->len,  INP_BUF_SZ - sb->len - 1)) > 0) {
                sb->len += sz;
-               sb->proc_input(sb);
+               proc_input(sb);
        }
 
        /* if we fill the input buffer, make a last attempt to parse it, and discard
         * it so we can receive more
         */
        if(sb->len >= INP_BUF_SZ) {
-               sb->proc_input(sb);
+               proc_input(sb);
                sb->len = 0;
        }
 
@@ -176,12 +179,7 @@ static int stty_mag(int fd)
 }
 
 
-static int proc_sball(struct sball *sb)
-{
-       return -1;
-}
-
-static int proc_mag(struct sball *sb)
+static int proc_input(struct sball *sb)
 {
        int sz;
        char *bptr = sb->buf;
@@ -192,7 +190,7 @@ static int proc_mag(struct sball *sb)
        while(bptr < end) {
                if(*bptr == '\r') {
                        *bptr = 0;
-                       mag_parsepkt(*start, start + 1, bptr - start - 1);
+                       sb->parse(*start, start + 1, bptr - start - 1);
                        start = ++bptr;
                } else {
                        bptr++;
@@ -207,6 +205,19 @@ static int proc_mag(struct sball *sb)
        return 0;
 }
 
+static void print_keystate(unsigned int keystate)
+{
+       int i;
+
+       printf("keystate: ");
+       for(i=0; i<12; i++) {
+               int b = 11 - i;
+               int hex = b < 10 ? b + '0' : b - 10 + 'a';
+               putchar(keystate & (1 << b) ? hex : '-');
+       }
+       putchar('\n');
+}
+
 static int mag_parsepkt(int id, char *data, int len)
 {
        int i, mot[6];
@@ -234,13 +245,7 @@ static int mag_parsepkt(int id, char *data, int len)
                        return -1;
                }
                keystate = (data[0] & 0xf) | ((data[1] & 0xf) << 4) | (((unsigned int)data[2] & 0xf) << 8);
-               printf("keystate: ");
-               for(i=0; i<12; i++) {
-                       int b = 11 - i;
-                       int hex = b < 10 ? b + '0' : b - 10 + 'a';
-                       putchar(keystate & (1 << b) ? hex : '-');
-               }
-               putchar('\n');
+               print_keystate(keystate);
                break;
 
        case 'e':
@@ -259,6 +264,80 @@ static int mag_parsepkt(int id, char *data, int len)
        return 0;
 }
 
+static int sball_parsepkt(int id, char *data, int len)
+{
+       int i;
+       unsigned int keystate;
+       char c, *rd, *wr;
+       short *mot;
+
+       /* decode data packet, replacing escaped values with the correct ones */
+       rd = wr = data;
+       while(rd < data + len) {
+               if((c = *rd++) == '^') {
+                       switch(*rd++) {
+                       case 'Q':
+                               *wr++ = 0x11;   /* XON */
+                               break;
+                       case 'S':
+                               *wr++ = 0x13;   /* XOFF */
+                               break;
+                       case 'M':
+                               *wr++ = 13;             /* CR */
+                               break;
+                       case '^':
+                               *wr++ = '^';
+                       default:
+                               fprintf(stderr, "sball decode: ignoring invalid escape code: %xh\n", (unsigned int)c);
+                       }
+               } else {
+                       *wr++ = c;
+               }
+       }
+       len = wr - data;        /* update the decoded length */
+
+       switch(id) {
+       case 'D':
+               if(len != 14) {
+                       fprintf(stderr, "sball: invalid data packet, expected 14 bytes, got: %d\n", len);
+                       return -1;
+               }
+
+               mot = (short*)(data + 2);       /* skip the period */
+               printf("motion: T %+6d %+6d %+6d  R %+6d %+6d %+6d\n", mot[0], mot[1], mot[2], mot[3], mot[4], mot[5]);
+               break;
+
+       case 'K':
+               if(len != 2) {
+                       fprintf(stderr, "sball: invalid key packet, expected 2 bytes, got: %d\n", len);
+                       return -1;
+               }
+               keystate = (data[1] & 0x30) >> 4;
+               print_keystate(keystate);
+               break;
+
+       case 'E':
+               fprintf(stderr, "sball: error:");
+               for(i=0; i<len; i++) {
+                       if(isprint(data[i])) {
+                               fprintf(stderr, " %c", data[i]);
+                       } else {
+                               fprintf(stderr, " %02xh", (unsigned int)data[i]);
+                       }
+               }
+               break;
+
+       default:
+               /* DEBUG */
+               fprintf(stderr, "sball: got '%c' packet:", (char)id);
+               for(i=0; i<len; i++) {
+                       fprintf(stderr, " %02x", (unsigned int)data[i]);
+               }
+               fputc('\n', stderr);
+       }
+       return 0;
+}
+
 static void make_printable(char *buf, int len)
 {
        int i, c;
@@ -268,7 +347,7 @@ static void make_printable(char *buf, int len)
                c = *buf++;
                if(c == '\r') {
                        *wr++ = '\n';
-                       if(*buf == '\n') buf++;
+                       while(*buf == '\n' || *buf == '\r') buf++;
                } else {
                        *wr++ = c;
                }