#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>
#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 (*parse)(int, char*, int);
};
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_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);
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);
- sb->decode = decode_sball;
+ /* set SS mode */
+ write(fd, "MSS\r", 4);
+
+ sb->parse = sball_parsepkt;
return sb;
}
}
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);
- sb->decode = decode_mag;
+ /* set 3D mode, not-dominant-axis, pass through motion and button packets */
+ write(fd, "m3\r", 3);
+
+ sb->parse = mag_parsepkt;
return 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;
+ 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;
+}
/* Labtec spaceball: 9600 8n1 XON/XOFF */
static int stty_sball(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);
}
-static int decode_sball(struct sball *sb, char *line)
+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(*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 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)
{
- return -1;
+ 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);
+ print_keystate(keystate);
+ 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 decode_mag(struct sball *sb, char *line)
+static int sball_parsepkt(int id, char *data, int len)
{
- return -1;
+ 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)
c = *buf++;
if(c == '\r') {
*wr++ = '\n';
- if(*buf == '\n') buf++;
+ while(*buf == '\n' || *buf == '\r') buf++;
} else {
*wr++ = c;
}