From d9869dd84b82cbf6cd786d857145b62e2bcf50de Mon Sep 17 00:00:00 2001 From: John Tsiombikas Date: Thu, 5 Nov 2020 15:01:57 -0500 Subject: [PATCH] initial spaceball parsing implementation --- src/sball.c | 131 +++++++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 105 insertions(+), 26 deletions(-) diff --git a/src/sball.c b/src/sball.c index e51ee3e..b52d896 100644 --- a/src/sball.c +++ b/src/sball.c @@ -1,8 +1,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -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