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_printf(fd, "vQ\r");
  80
  81         while(ser_getline_block(fd, buf, sizeof buf) && buf[0] != 'v');
  82
  83         printf("DBG: \"%s\"\n", buf);
  84         if(buf[0] != 'v' || !strstr(buf, "MAGELLAN")) {
  85                 fprintf(stderr, "unknown device: \"%s\"\n", buf + 1);
  86                 ser_close(fd);
  87                 return -1;
  88         }
  89         return fd;
  90 }
  91
  92 static int detect(void)
  93 {
  94         int fd;
  95         if((fd = opendev(get_port())) >= 0) {
  96                 ser_close(fd);
  97                 return 1;
  98         }
  99         return 0;
 100 }
 101
 102 static int start(void)
 103 {
 104         if(devfd >= 0) {
 105                 fprintf(stderr, "magellan start: already started\n");
 106                 return -1;
 107         }
 108
 109         if((devfd = opendev(get_port())) < 0) {
 110                 return -1;
 111         }
 112         ser_printf(devfd, "m3\r");      /* start motion packets */
 113
 114         return devfd;
 115 }
 116
 117 static void stop(void)
 118 {
 119         if(devfd < 0) {
 120                 fprintf(stderr, "magellan stop: not started\n");
 121                 return;
 122         }
 123
 124         ser_close(devfd);
 125         devfd = -1;
 126 }
 127
 128 static int pending(void)
 129 {
 130         if(devfd < 0) return 0;
 131         return ser_pending(devfd);
 132 }
 133
 134 static int getevent(device_event *ev)
 135 {
 136         proc_packets();
 137
 138         if(!QEMPTY(evq)) {
 139                 *ev = evq[evq_ridx];
 140                 evq_ridx = QNEXT(evq_ridx);
 141                 return 1;
 142         }
 143         return 0;
 144 }
 145
 146 static int proc_packets(void)
 147 {
 148         int i, count = 0;
 149         device_event *ev;
 150         char buf[128];
 151         unsigned int st, delta, prev;
 152
 153         if(devfd < 0) return -1;
 154
 155         while(ser_getline(devfd, buf, sizeof buf)) {
 156
 157                 switch(buf[0]) {
 158                 case 'd':
 159                         ev = evq + evq_widx;
 160                         if(parse_motion(ev->motion.motion, buf + 1) == -1) {
 161                                 break;
 162                         }
 163                         ev->type = DEV_EV_MOTION;
 164                         enqueue_event(ev);
 165                         ++count;
 166                         break;
 167
 168                 case 'k':
 169                         if(parse_keystate(&st, buf + 1) == -1) {
 170                                 break;
 171                         }
 172
 173                         delta = st ^ bnstate;
 174                         prev = bnstate;
 175                         bnstate = st;
 176
 177                         for(i=0; i<32; i++) {
 178                                 if(delta & 1) {
 179                                         ev = evq + evq_widx;
 180                                         ev->type = DEV_EV_BUTTON;
 181                                         ev->button.id = i;
 182                                         ev->button.pressed = st & 1;
 183                                         ev->button.state = prev ^ (1 << i);
 184                                         enqueue_event(ev);
 185                                         ++count;
 186                                 }
 187                                 st >>= 1;
 188                         }
 189                         break;
 190                 }
 191         }
 192         return count;
 193 }
 194
 195 static void enqueue_event(device_event *ev)
 196 {
 197         if(ev != evq + evq_widx) {
 198                 evq[evq_widx] = *ev;
 199         }
 200
 201         evq_widx = QNEXT(evq_widx);
 202         if(evq_widx == evq_ridx) {
 203                 /* overflowed, drop the oldest event */
 204                 evq_ridx = QNEXT(evq_ridx);
 205         }
 206 }
 207
 208 static int parse_motion(int *motv, const char *data)
 209 {
 210         int i;
 211         long val;
 212
 213         for(i=0; i<6; i++) {
 214                 if(!data[0] || !data[1] || !data[2] || !data[3]) {
 215                         return -1;
 216                 }
 217                 val = ((((long)data[0] & 0xf) << 12) |
 218                         (((long)data[1] & 0xf) << 8) |
 219                         (((long)data[2] & 0xf) << 4) |
 220                         ((long)data[3] & 0xf)) - 32768;
 221                 data += 4;
 222                 *motv++ = (int)val;
 223         }
 224         return 0;
 225 }
 226
 227 static int parse_keystate(unsigned int *stptr, const char *data)
 228 {
 229         int i, bit = 0;
 230         unsigned int st = 0;
 231
 232         for(i=0; i<3; i++) {
 233                 if(!data) return -1;
 234                 st |= ((unsigned int)*data++ & 0xf) << bit;
 235                 bit += 4;
 236         }
 237         *stptr = st;
 238         return 0;
 239 }