#include <stdio.h>
#include <stdlib.h>
#include <string.h>
+#include <ctype.h>
+#include <time.h>
#include <errno.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
+#include <sys/select.h>
+#include <sys/time.h>
+#include <sys/ioctl.h>
+
+#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;
+
+ short mot[6];
+ unsigned int keystate;
+
+ struct termios saved_term;
+ int saved_mstat;
+
+ int (*parse)(struct sball*, int, char*, int);
};
-static int stty_sball(int fd);
-static int stty_mag(int fd);
+static int stty_sball(struct sball *sb);
+static int stty_mag(struct sball *sb);
+static void stty_save(struct sball *sb);
+static void stty_restore(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 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;
+ int fd, sz;
+ char buf[128];
struct sball *sb = 0;
- if((fd = open(dev, O_RDWR | | O_NOCTTY | O_NONBLOCK)) == -1) {
+ if((fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK)) == -1) {
fprintf(stderr, "sball_open: failed to open device: %s: %s\n", dev, strerror(errno));
return 0;
}
goto err;
}
sb->fd = fd;
+ sb->flags = 0;
+ sb->len = 0;
+
+ stty_save(sb);
+
+ if(stty_sball(sb) == -1) {
+ goto err;
+ }
+ write(fd, "\r@RESET\r", 8);
+
+ if((sz = read_timeout(fd, buf, sizeof buf - 1, 2000000)) > 0 && strstr(buf, "\r@1")) {
+ /* we got a response, so it's a spaceball */
+ make_printable(buf, sz);
+ printf("Spaceball detected: %s\n", buf);
+
+ sb->nbuttons = guess_num_buttons(buf);
+ printf("%d buttons\n", sb->nbuttons);
- if(stty_sball(fd) == -1) {
+ /* set binary mode and enable automatic data packet sending */
+ write(fd, "\rCB\rMSSV\r", 9);
+
+ sb->parse = sball_parsepkt;
+ return sb;
+ }
+
+ /* try as a magellan spacemouse */
+ if(stty_mag(sb) == -1) {
goto err;
}
+ write(fd, "vQ\r", 3);
+
+ if((sz = read_timeout(fd, buf, sizeof buf - 1, 250000)) > 0 && buf[0] == 'v') {
+ make_printable(buf, sz);
+ printf("Magellan SpaceMouse detected:\n%s\n", buf);
- /* set detect receiver function pointer */
+ sb->nbuttons = guess_num_buttons(buf);
+ printf("%d buttons\n", sb->nbuttons);
- return sb;
+ /* set 3D mode, not-dominant-axis, pass through motion and button packets */
+ write(fd, "m3\r", 3);
+
+ sb->parse = mag_parsepkt;
+ return sb;
+ }
err:
+ stty_restore(sb);
close(fd);
free(sb);
return 0;
void sball_close(struct sball *sb)
{
if(!sb) return;
+
+ stty_restore(sb);
close(sb->fd);
}
+int sball_fd(struct sball *sb)
+{
+ return sb->fd;
+}
-/* Labtec spaceball: 9600 8n1 XON/XOFF */
-static int stty_sball(int fd)
+int sball_read(struct sball *sb)
{
- struct termios term;
+ int sz;
- if(tcgetattr(fd, &term) == -1) {
- perror("sball_open: tcgetattr");
- return -1;
+ while((sz = read(sb->fd, sb->buf + sb->len, INP_BUF_SZ - sb->len - 1)) > 0) {
+ sb->len += sz;
+ 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) {
+ 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(struct sball *sb)
+{
+ int mstat;
+ struct termios term;
+
+ term = sb->saved_term;
term.c_oflag = 0;
- term.c_lflag = 0;
+ term.c_lflag = ICANON;
term.c_cc[VMIN] = 0;
term.c_cc[VTIME] = 0;
+ term.c_cc[VEOF] = 0;
+ term.c_cc[VEOL] = '\r';
+ term.c_cc[VEOL2] = 0;
+ term.c_cc[VERASE] = 0;
+ term.c_cc[VKILL] = 0;
term.c_cflag = CLOCAL | CREAD | CS8 | HUPCL;
- term.c_iflag = IGNBRK | IGNPAR | IXON | IXOFF;
+ term.c_iflag = IGNBRK | IGNPAR;
- csetispeed(&term, B9600);
- csetospeed(&term, B9600);
+ cfsetispeed(&term, B9600);
+ cfsetospeed(&term, B9600);
- if(tcsetattr(fd, TCSANOW, &term) == -1) {
+ if(tcsetattr(sb->fd, TCSAFLUSH, &term) == -1) {
perror("sball_open: tcsetattr");
return -1;
}
+ tcflush(sb->fd, TCIOFLUSH);
+ mstat = sb->saved_mstat | TIOCM_DTR | TIOCM_RTS;
+ ioctl(sb->fd, TIOCMGET, &mstat);
return 0;
}
/* Logicad magellan spacemouse: 9600 8n2 CTS/RTS */
-static int stty_mag(int fd)
+static int stty_mag(struct sball *sb)
{
+ int mstat;
struct termios term;
- if(tcgetattr(fd, &term) == -1) {
- perror("sball_open: tcgetattr");
- return -1;
- }
-
+ term = sb->saved_term;
term.c_oflag = 0;
- term.c_lflag = 0;
+ term.c_lflag = ICANON;
term.c_cc[VMIN] = 0;
term.c_cc[VTIME] = 0;
+ term.c_cc[VEOF] = 0;
+ term.c_cc[VEOL] = '\r';
+ term.c_cc[VEOL2] = 0;
+ term.c_cc[VERASE] = 0;
+ term.c_cc[VKILL] = 0;
term.c_cflag = CLOCAL | CREAD | CS8 | CSTOPB | HUPCL | CRTSCTS;
- term.c_iflag = IGNBRK | IGNPAR |;
+ term.c_iflag = IGNBRK | IGNPAR;
- csetispeed(&term, B9600);
- csetospeed(&term, B9600);
+ cfsetispeed(&term, B9600);
+ cfsetospeed(&term, B9600);
- if(tcsetattr(fd, TCSANOW, &term) == -1) {
+ if(tcsetattr(sb->fd, TCSAFLUSH, &term) == -1) {
perror("sball_open: tcsetattr");
return -1;
}
+ tcflush(sb->fd, TCIOFLUSH);
+ mstat = sb->saved_mstat | TIOCM_DTR | TIOCM_RTS;
+ ioctl(sb->fd, TIOCMGET, &mstat);
return 0;
}
+
+static void stty_save(struct sball *sb)
+{
+ tcgetattr(sb->fd, &sb->saved_term);
+ ioctl(sb->fd, TIOCMGET, &sb->saved_mstat);
+}
+
+static void stty_restore(struct sball *sb)
+{
+ tcsetattr(sb->fd, TCSAFLUSH, &sb->saved_term);
+ tcflush(sb->fd, TCIOFLUSH);
+ ioctl(sb->fd, TIOCMSET, &sb->saved_mstat);
+}
+
+
+static int proc_input(struct sball *sb)
+{
+ 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;
+ sb->parse(sb, *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(struct sball *sb, int id, char *data, int len)
+{
+ int i;
+
+ /*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++) {
+#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;
+ }
+ print_state(sb);
+ break;
+
+ case 'k':
+ if(len != 3) {
+ fprintf(stderr, "magellan: invalid keyboard pakcet, expected 3 bytes, got: %d\n", len);
+ return -1;
+ }
+ sb->keystate = (data[0] & 0xf) | ((data[1] & 0xf) << 4) | (((unsigned int)data[2] & 0xf) << 8);
+ print_state(sb);
+ 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 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;
+ char *wr = buf;
+
+ for(i=0; i<len; i++) {
+ c = *buf++;
+ if(c == '\r') {
+ *wr++ = '\n';
+ while(*buf == '\n' || *buf == '\r') buf++;
+ } else {
+ *wr++ = c;
+ }
+ }
+ *wr = 0;
+}
+
+static int read_timeout(int fd, char *buf, int bufsz, long tm_usec)
+{
+ int res;
+ long usec, sz = 0;
+ struct timeval tv0, tv;
+ fd_set rdset;
+
+ if(!buf || bufsz <= 0) return -1;
+
+ usec = tm_usec;
+ gettimeofday(&tv0, 0);
+
+ while(sz < bufsz && usec > 0) {
+ tv.tv_sec = usec / 1000000;
+ tv.tv_usec = usec % 1000000;
+
+ FD_ZERO(&rdset);
+ FD_SET(fd, &rdset);
+ if((res = select(fd + 1, &rdset, 0, 0, &tv)) > 0 && FD_ISSET(fd, &rdset)) {
+ sz += read(fd, buf + sz, bufsz - sz);
+ buf[sz] = 0;
+ tm_usec = usec = 128000; /* wait 128ms for the rest of the message to appear */
+ gettimeofday(&tv0, 0);
+ continue;
+ }
+ if(res == -1 && (errno == EWOULDBLOCK || errno == EAGAIN)) {
+ break;
+ }
+ gettimeofday(&tv, 0);
+ usec = tm_usec - ((tv.tv_sec - tv0.tv_sec) * 1000000 + (tv.tv_usec - tv0.tv_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);
+}
projects / sball / blobdiff