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