magellan works

diff --git a/src/main.c b/src/main.c
index 04b0ace..c39e0ff 100644 (file)
--- a/src/main.c
+++ b/src/main.c
@@ -1,16 +1,45 @@
 #include <stdio.h>
 #include <stdlib.h>
+#include <signal.h>
+#include <errno.h>
+#include <unistd.h>
+#include <sys/select.h>
 #include "sball.h"
 
+static void sighandler(int s);
+
 static struct sball *sb;
+static int quit;
 
 int main(int argc, char **argv)
 {
+       int fd;
+       fd_set rdset;
+
+       signal(SIGINT, sighandler);
+
        if(!(sb = sball_open(argv[1] ? argv[1] : "/dev/ttyS0"))) {
                fprintf(stderr, "Failed to open spaceball at %s\n", argv[1] ? argv[1] : "/dev/ttyS0");
                return 1;
        }
+       fd = sball_fd(sb);
+
+       while(!quit) {
+               FD_ZERO(&rdset);
+               FD_SET(fd, &rdset);
+
+               if(select(fd + 1, &rdset, 0, 0, 0) > 0) {
+                       if(FD_ISSET(fd, &rdset)) {
+                               sball_read(sb);
+                       }
+               }
+       }
 
        sball_close(sb);
        return 0;
 }
+
+static void sighandler(int s)
+{
+       quit = 1;
+}

diff --git a/src/sball.c b/src/sball.c
index 2de1403..e51ee3e 100644 (file)
--- a/src/sball.c
+++ b/src/sball.c
@@ -9,17 +9,24 @@
 #include <sys/select.h>
 #include <sys/time.h>
 
+#define INP_BUF_SZ     256
+
 struct sball {
        int fd;
 
-       int (*decode)(struct sball *sb, char *line);
+       char buf[INP_BUF_SZ];
+       int len;
+
+       int (*proc_input)(struct sball *sb);
 };
 
 static int stty_sball(int fd);
 static int stty_mag(int fd);
 
-static int decode_sball(struct sball *sb, char *line);
-static int decode_mag(struct sball *sb, char *line);
+static int proc_sball(struct sball *sb);
+static int proc_mag(struct sball *sb);
+
+static int mag_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);
@@ -49,8 +56,9 @@ struct sball *sball_open(const char *dev)
                /* 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->decode = decode_sball;
+               sb->proc_input = proc_sball;
                return sb;
        }
 
@@ -65,7 +73,10 @@ struct sball *sball_open(const char *dev)
                make_printable(buf, sz);
                printf("Magellan SpaceMouse detected: %s\n", buf);
 
-               sb->decode = decode_mag;
+               /* set 3D mode, not-dominant-axis, pass through motion and button packets */
+               write(fd, "m3\r", 3);
+
+               sb->proc_input = proc_mag;
                return sb;
        }
 
@@ -81,6 +92,30 @@ void sball_close(struct sball *sb)
        close(sb->fd);
 }
 
+int sball_fd(struct sball *sb)
+{
+       return sb->fd;
+}
+
+int sball_read(struct sball *sb)
+{
+       int sz;
+
+       while((sz = read(sb->fd, sb->buf + sb->len,  INP_BUF_SZ - sb->len - 1)) > 0) {
+               sb->len += sz;
+               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);
+               sb->len = 0;
+       }
+
+       return 0;
+}
 
 /* Labtec spaceball: 9600 8n1 XON/XOFF */
 static int stty_sball(int fd)
@@ -127,7 +162,7 @@ static int stty_mag(int fd)
        term.c_cc[VTIME] = 1;
 
        term.c_cflag = CLOCAL | CREAD | CS8 | CSTOPB | HUPCL | CRTSCTS;
-       term.c_iflag = IGNBRK | IGNPAR | ICRNL;
+       term.c_iflag = IGNBRK | IGNPAR;
 
        cfsetispeed(&term, B9600);
        cfsetospeed(&term, B9600);
@@ -141,14 +176,87 @@ static int stty_mag(int fd)
 }
 
 
-static int decode_sball(struct sball *sb, char *line)
+static int proc_sball(struct sball *sb)
 {
        return -1;
 }
 
-static int decode_mag(struct sball *sb, char *line)
+static int proc_mag(struct sball *sb)
 {
-       return -1;
+       int sz;
+       char *bptr = sb->buf;
+       char *start = sb->buf;
+       char *end = sb->buf + sb->len;
+
+       /* see if we have a CR in the buffer */
+       while(bptr < end) {
+               if(*bptr == '\r') {
+                       *bptr = 0;
+                       mag_parsepkt(*start, start + 1, bptr - start - 1);
+                       start = ++bptr;
+               } else {
+                       bptr++;
+               }
+       }
+
+       sz = start - sb->buf;
+       if(sz > 0) {
+               memmove(sb->buf, start, sz);
+               sb->len -= sz;
+       }
+       return 0;
+}
+
+static int mag_parsepkt(int id, char *data, int len)
+{
+       int i, mot[6];
+       unsigned int keystate;
+
+       /*printf("magellan packet: %c - %s (%d bytes)\n", (char)id, data, len);*/
+
+       switch(id) {
+       case 'd':
+               if(len != 24) {
+                       fprintf(stderr, "magellan: invalid data packet, expected 24 bytes, got: %d\n", len);
+                       return -1;
+               }
+               for(i=0; i<6; i++) {
+                       mot[i] = ((((int)data[0] & 0xf) << 12) | (((int)data[1] & 0xf) << 8) |
+                                       (((int)data[2] & 0xf) << 4) | (data[3] & 0xf)) - 0x8000;
+                       data += 4;
+               }
+               printf("motion: T %+4d %+4d %+4d  R %+4d %+4d %+4d\n", mot[0], mot[1], mot[2], mot[3], mot[4], mot[5]);
+               break;
+
+       case 'k':
+               if(len != 3) {
+                       fprintf(stderr, "magellan: invalid keyboard pakcet, expected 3 bytes, got: %d\n", 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');
+               break;
+
+       case 'e':
+               if(data[0] == 1) {
+                       fprintf(stderr, "magellan error: illegal command: %c%c\n", data[1], data[2]);
+               } else if(data[0] == 2) {
+                       fprintf(stderr, "magellan error: framing error\n");
+               } else {
+                       fprintf(stderr, "magellan error: unknown device error\n");
+               }
+               return -1;
+
+       default:
+               break;
+       }
+       return 0;
 }
 
 static void make_printable(char *buf, int len)

diff --git a/src/sball.h b/src/sball.h
index 67ff1cf..797e132 100644 (file)
--- a/src/sball.h
+++ b/src/sball.h
@@ -5,5 +5,7 @@ struct sball;
 
 struct sball *sball_open(const char *dev);
 void sball_close(struct sball *sb);
+int sball_fd(struct sball *sb);
+int sball_read(struct sball *sb);
 
 #endif /* SBALL_H_ */