git.mutantstargoat.com Git - smouse/blob - src/dev_smag.c

   1 #include <stdio.h>
   2 #include <string.h>
   3 #include <unistd.h>
   4 #include <fcntl.h>
   5 #include "device.h"
   6 #include "serial.h"
   7
   8 static int init(void);
   9 static void destroy(void);
  10 static int detect(void);
  11 static int start(void);
  12 static void stop(void);
  13 static int pending(void);
  14 static int getevent(device_event *ev);
  15
  16 static int opendev(const char *dev);
  17 static int proc_packets(void);
  18 static void enqueue_event(device_event *ev);
  19 static int parse_motion(int *motv, const char *data);
  20 static int parse_keystate(unsigned int *st, const char *data);
  21
  22
  23 static int devfd = -1;
  24 static int bnstate;
  25
  26 static device_event evq[16];
  27 static int evq_widx, evq_ridx;
  28 #define QNEXT(x)        (((x) + 1) & 0xf)
  29 #define QEMPTY(b)       (b##_ridx == b##_widx)
  30
  31
  32 struct device *device_magellan(void)
  33 {
  34         static struct device dev = {
  35                 "magellan",
  36                 0,
  37                 init,
  38                 destroy,
  39                 detect,
  40                 start,
  41                 stop,
  42                 pending,
  43                 getevent
  44         };
  45         return &dev;
  46 }
  47
  48
  49 static int init(void)
  50 {
  51         return 0;
  52 }
  53
  54 static void destroy(void)
  55 {
  56 }
  57
  58 static int opendev(const char *dev)
  59 {
  60         int fd, timeout;
  61         char buf[128];
  62
  63         if((fd = ser_open(dev, 9600, SER_8N2 | SER_HWFLOW)) == -1) {
  64                 return -1;
  65         }
  66
  67         /* try for about 5 sec */
  68         timeout = 5000;
  69         do {
  70                 ser_printf(fd, "z\r");
  71         } while(!ser_wait(fd, 250) && (timeout -= 250) > 0);
  72
  73         if(timeout <= 0) {
  74                 fprintf(stderr, "magellan open(%s): device does not respond\n", dev);
  75                 ser_close(fd);
  76                 return -1;      /* failed to get a response */
  77         }
  78
  79         ser_block(fd);
  80
  81         ser_printf(fd, "vQ\r");
  82         do {
  83                 ser_getline(fd, buf, sizeof buf);
  84         } while(buf[0] != 'v');
  85
  86         if(buf[0] != 'v' || !strstr(buf, "MAGELLAN")) {
  87                 fprintf(stderr, "unknown device: \"%s\"\n", buf + 1);
  88                 ser_close(fd);
  89                 return -1;
  90         }
  91
  92         ser_nonblock(fd);
  93         return fd;
  94 }
  95
  96 static int detect(void)
  97 {
  98         int fd;
  99         if((fd = opendev(get_port())) >= 0) {
 100                 ser_close(fd);
 101                 return 1;
 102         }
 103         return 0;
 104 }
 105
 106 static int start(void)
 107 {
 108         if(devfd >= 0) {
 109                 fprintf(stderr, "magellan start: already started\n");
 110                 return -1;
 111         }
 112
 113         if((devfd = opendev(get_port())) < 0) {
 114                 return -1;
 115         }
 116         ser_printf(devfd, "m3\r");      /* start motion packets */
 117
 118         return devfd;
 119 }
 120
 121 static void stop(void)
 122 {
 123         if(devfd < 0) {
 124                 fprintf(stderr, "magellan stop: not started\n");
 125                 return;
 126         }
 127
 128         ser_close(devfd);
 129         devfd = -1;
 130 }
 131
 132 static int pending(void)
 133 {
 134         if(devfd < 0) return 0;
 135         return ser_pending(devfd);
 136 }
 137
 138 static int getevent(device_event *ev)
 139 {
 140         proc_packets();
 141
 142         if(!QEMPTY(evq)) {
 143                 *ev = evq[evq_ridx];
 144                 evq_ridx = QNEXT(evq_ridx);
 145                 return 1;
 146         }
 147         return 0;
 148 }
 149
 150 static int proc_packets(void)
 151 {
 152         int i, count = 0;
 153         device_event *ev;
 154         char buf[128];
 155         unsigned int st, delta, prev;
 156
 157         if(devfd < 0) return -1;
 158
 159         while(ser_getline(devfd, buf, sizeof buf)) {
 160
 161                 switch(buf[0]) {
 162                 case 'd':
 163                         ev = evq + evq_widx;
 164                         if(parse_motion(ev->motion.motion, buf + 1) == -1) {
 165                                 break;
 166                         }
 167                         ev->type = DEV_EV_MOTION;
 168                         enqueue_event(ev);
 169                         ++count;
 170                         break;
 171
 172                 case 'k':
 173                         if(parse_keystate(&st, buf + 1) == -1) {
 174                                 break;
 175                         }
 176
 177                         delta = st ^ bnstate;
 178                         prev = bnstate;
 179                         bnstate = st;
 180
 181                         for(i=0; i<32; i++) {
 182                                 if(delta & 1) {
 183                                         ev = evq + evq_widx;
 184                                         ev->type = DEV_EV_BUTTON;
 185                                         ev->button.id = i;
 186                                         ev->button.pressed = st & 1;
 187                                         ev->button.state = prev ^ (1 << i);
 188                                         enqueue_event(ev);
 189                                         ++count;
 190                                 }
 191                                 st >>= 1;
 192                                 delta >>= 1;
 193                         }
 194                         break;
 195                 }
 196         }
 197         return count;
 198 }
 199
 200 static void enqueue_event(device_event *ev)
 201 {
 202         if(ev != evq + evq_widx) {
 203                 evq[evq_widx] = *ev;
 204         }
 205
 206         evq_widx = QNEXT(evq_widx);
 207         if(evq_widx == evq_ridx) {
 208                 /* overflowed, drop the oldest event */
 209                 evq_ridx = QNEXT(evq_ridx);
 210         }
 211 }
 212
 213 static int parse_motion(int *motv, const char *data)
 214 {
 215         int i;
 216         long val;
 217
 218         for(i=0; i<6; i++) {
 219                 if(!data[0] || !data[1] || !data[2] || !data[3]) {
 220                         return -1;
 221                 }
 222                 val = ((((long)data[0] & 0xf) << 12) |
 223                         (((long)data[1] & 0xf) << 8) |
 224                         (((long)data[2] & 0xf) << 4) |
 225                         ((long)data[3] & 0xf)) - 32768;
 226                 data += 4;
 227                 *motv++ = (int)val;
 228         }
 229         return 0;
 230 }
 231
 232 static int parse_keystate(unsigned int *stptr, const char *data)
 233 {
 234         int i, bit = 0;
 235         unsigned int st = 0;
 236
 237         for(i=0; i<3; i++) {
 238                 if(!data) return -1;
 239                 st |= ((unsigned int)*data++ & 0xf) << bit;
 240                 bit += 4;
 241         }
 242         *stptr = st;
 243         return 0;
 244 }