X-Git-Url: http://git.mutantstargoat.com/user/nuclear/?a=blobdiff_plain;f=src%2Fsball.c;h=4b0304050d09aeab890f5d080d3555b8c26f2fd3;hb=9c67b30c9aacfdd2bf783ed25848a152d73ea8b3;hp=e51ee3ec6e6813bf625e35f0bcef1474b8c7136c;hpb=3c1dc0f43eade1f8e59f74ffcc41153ed0dff15f;p=sball

diff --git a/src/sball.c b/src/sball.c
index e51ee3e..4b03040 100644
--- 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>
@@ -11,26 +13,41 @@
 
 #define INP_BUF_SZ	256
 
+enum {
+	SB4000	= 1,
+	FLIPXY	= 2
+};
+
 struct sball {
 	int fd;
+	unsigned int flags;
+	int nbuttons;
 
 	char buf[INP_BUF_SZ];
 	int len;
 
-	int (*proc_input)(struct sball *sb);
+	short mot[6];
+	unsigned int keystate;
+
+	int (*parse)(struct sball*, 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(struct sball *sb, int id, char *data, int len);
+static int sball_parsepkt(struct sball *sb, int id, char *data, int len);
 
-static int mag_parsepkt(int id, char *data, int len);
+static int guess_num_buttons(const char *verstr);
 
 static void make_printable(char *buf, int len);
 static int read_timeout(int fd, char *buf, int bufsz, long tm_usec);
 
+static void print_state(struct sball *sb);
+
+
 struct sball *sball_open(const char *dev)
 {
 	int fd, sz;
@@ -47,18 +64,25 @@ struct sball *sball_open(const char *dev)
 		goto err;
 	}
 	sb->fd = fd;
+	sb->flags = 0;
+	sb->len = 0;
 
 	if(stty_sball(fd) == -1) {
 		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;
+		sb->nbuttons = guess_num_buttons(buf);
+		printf("%d buttons\n", sb->nbuttons);
+
+		/* set binary mode and enable automatic data packet sending */
+		write(fd, "\rCB\rMSS\r", 8);
+
+		sb->parse = sball_parsepkt;
 		return sb;
 	}
 
@@ -68,15 +92,17 @@ 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);
+		printf("Magellan SpaceMouse detected:\n%s\n", buf);
+
+		sb->nbuttons = guess_num_buttons(buf);
+		printf("%d buttons\n", sb->nbuttons);
 
 		/* 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,20 +129,35 @@ 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;
 	}
 
 	return 0;
 }
 
+int sball_axis(struct sball *sb, int axis)
+{
+	return sb->mot[axis];
+}
+
+unsigned int sball_buttons(struct sball *sb)
+{
+	return sb->keystate;
+}
+
+int sball_num_buttons(struct sball *sb)
+{
+	return sb->nbuttons;
+}
+
 /* Labtec spaceball: 9600 8n1 XON/XOFF */
 static int stty_sball(int fd)
 {
@@ -176,12 +217,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 +228,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(sb, *start, start + 1, bptr - start - 1);
 			start = ++bptr;
 		} else {
 			bptr++;
@@ -207,10 +243,9 @@ static int proc_mag(struct sball *sb)
 	return 0;
 }
 
-static int mag_parsepkt(int id, char *data, int len)
+static int mag_parsepkt(struct sball *sb, int id, char *data, int len)
 {
-	int i, mot[6];
-	unsigned int keystate;
+	int i;
 
 	/*printf("magellan packet: %c - %s (%d bytes)\n", (char)id, data, len);*/
 
@@ -221,11 +256,16 @@ static int mag_parsepkt(int id, char *data, int len)
 			return -1;
 		}
 		for(i=0; i<6; i++) {
-			mot[i] = ((((int)data[0] & 0xf) << 12) | (((int)data[1] & 0xf) << 8) |
+#ifdef SBALL_BIG_ENDIAN
+			sb->mot[i] = ((((int)data[3] & 0xf) << 12) | (((int)data[2] & 0xf) << 8) |
+					(((int)data[1] & 0xf) << 4) | (data[0] & 0xf)) - 0x8000;
+#else
+			sb->mot[i] = ((((int)data[0] & 0xf) << 12) | (((int)data[1] & 0xf) << 8) |
 					(((int)data[2] & 0xf) << 4) | (data[3] & 0xf)) - 0x8000;
+#endif
 			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]);
+		print_state(sb);
 		break;
 
 	case 'k':
@@ -233,14 +273,8 @@ static int mag_parsepkt(int id, char *data, int len)
 			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');
+		sb->keystate = (data[0] & 0xf) | ((data[1] & 0xf) << 4) | (((unsigned int)data[2] & 0xf) << 8);
+		print_state(sb);
 		break;
 
 	case 'e':
@@ -259,6 +293,182 @@ static int mag_parsepkt(int id, char *data, int len)
 	return 0;
 }
 
+static int sball_parsepkt(struct sball *sb, int id, char *data, int len)
+{
+	int i;
+	char c, *rd, *wr;
+
+	/* 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++ = '^';
+				break;
+			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;
+		}
+
+#ifndef SBALL_BIG_ENDIAN
+		for(i=0; i<6; i++) {
+			data += 2;
+			c = data[0];
+			data[0] = data[1];
+			data[1] = c;
+			sb->mot[i] = *(short*)data;
+		}
+#else
+		memcpy(sb->mot, data + 2, 12);
+#endif
+		print_state(sb);
+		break;
+
+	case 'K':
+		if(len != 2) {
+			fprintf(stderr, "sball: invalid key packet, expected 2 bytes, got: %d\n", len);
+			return -1;
+		}
+		if(sb->flags & SB4000) break;	/* ignore K packets from spaceball 4000 devices */
+
+		/* data[1] bits 0-3 -> buttons 0,1,2,3
+		 * data[1] bits 4,5 (3003 L/R) -> buttons 0, 1
+		 * data[0] bits 0-2 -> buttons 4,5,6
+		 * data[0] bit 4 is (2003 pick) -> button 7
+		 */
+		sb->keystate = (data[1] & 0xf) | ((data[1] >> 4) & 3) | ((data[0] & 7) << 4) |
+			((data[0] & 0x10) >> 1);
+		print_state(sb);
+		break;
+
+	case '.':
+		if(len != 2) {
+			fprintf(stderr, "sball: invalid sb4k key packet, expected 2 bytes, got: %d\n", len);
+			return -1;
+		}
+		/* spaceball 4000 key packet */
+		sb->flags |= SB4000;
+		/* update orientation flag (actually don't bother) */
+		/*
+		if(data[0] & 0x20) {
+			sb->flags |= FLIPXY;
+		} else {
+			sb->flags &= ~FLIPXY;
+		}
+		*/
+
+		/* data[1] bits 0-5 -> buttons 0,1,2,3,4,5
+		 * data[1] bit 7 -> button 6
+		 * data[0] bits 0-4 -> buttons 7,8,9,10,11
+		 */
+		sb->keystate = (data[1] & 0x3f) | ((data[1] & 0x80) >> 1) | ((data[0] & 0x1f) << 7);
+		print_state(sb);
+		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;
+
+	case 'M':	/* ignore MSS responses */
+		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 int guess_num_buttons(const char *verstr)
+{
+	int major, minor;
+	const char *s, *model;
+
+	if((s = strstr(verstr, "Firmware version"))) {	/* spaceball */
+
+		/* if we got a model number, guess based on that */
+		if((model = strchr(s, '('))) {
+			if(memcmp(model, "(Model ", 7) == 0) {
+				model += 7;
+			} else {
+				model++;
+			}
+			switch(atoi(model)) {
+			case 2003:
+				return 8;
+			case 3003:
+				return 2;
+			case 5000:
+				return 12;
+			default:
+				break;
+			}
+		}
+		/* try to guess based on firmware number */
+		if(sscanf(s + 17, "%d.%d", &major, &minor) == 2 && major == 2) {
+			if(minor == 35 || minor == 62 || minor == 63) {
+				return 2;	/* spaceball 3003/3003C */
+			}
+			if(minor == 42 || minor == 43 || minor == 45) {
+				/* 2.42 is also used by spaceball 2003C, but this should be
+				 * caught before we get here by the model number guess
+				 */
+				return 12;	/* spaceball 4000flx/5000flx-a */
+			}
+			if(minor == 2 || minor == 13 || minor == 15) {
+				return 8;	/* spaceball 1003/2003/2003c */
+			}
+		}
+	}
+
+	if(strstr(verstr, "MAGELLAN")) {
+		return 9; /* magellan spacemouse */
+	}
+
+	if(strstr(verstr, "SPACEBALL")) {
+		return 12; /* spaceball 5000 */
+	}
+
+	if(strstr(verstr, "CadMan")) {
+		return 2;
+	}
+
+	fprintf(stderr, "Can't guess number of buttons, default to 8, report this as a bug!\n");
+	return 8;
+}
+
 static void make_printable(char *buf, int len)
 {
 	int i, c;
@@ -268,7 +478,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;
 		}
@@ -311,3 +521,28 @@ static int read_timeout(int fd, char *buf, int bufsz, long tm_usec)
 	return sz > 0 ? sz : -1;
 }
 
+static void print_motion(short *mot)
+{
+	printf(" T[%+6d %+6d %+6d]  R[%+6d %+6d %+6d]", mot[0], mot[1],
+			mot[2], mot[3], mot[4], mot[5]);
+}
+
+static void print_keystate(unsigned int keystate)
+{
+	int i;
+
+	for(i=0; i<12; i++) {
+		int b = 11 - i;
+		int hex = b < 10 ? b + '0' : b - 10 + 'a';
+		putchar(keystate & (1 << b) ? hex : '-');
+	}
+}
+
+static void print_state(struct sball *sb)
+{
+	print_motion(sb->mot);
+	printf("  B[");
+	print_keystate(sb->keystate);
+	printf("]\r");
+	fflush(stdout);
+}