]> rtime.felk.cvut.cz Git - frescor/frsh-forb.git/blob - src/frsh/resources/item/serialsource.c
Get rid of the most of other warnings
[frescor/frsh-forb.git] / src / frsh / resources / item / serialsource.c
1 #include <sys/types.h>
2 #include <sys/stat.h>
3 #include <termios.h>
4 #include <unistd.h>
5 #include <errno.h>
6 #include <fcntl.h>
7 #include <stdlib.h>
8 #include <string.h>
9 #include <ctype.h>
10 #include <sys/time.h>
11 #include <stdio.h>
12 #ifdef __CYGWIN__
13 #include <windows.h>
14 #include <io.h>
15 #else
16 #include <stdint.h>
17 #endif
18
19 /* C implementation of the mote serial protocol. See
20    net.tinyos.packet.Packetizer for more details */
21
22 #undef DEBUG
23
24 #include "serialsource.h"
25 #include "serialprotocol.h"
26
27 typedef int bool;
28
29 enum {
30 #ifndef __CYGWIN__
31   FALSE = 0,
32   TRUE = 1,
33 #endif
34   BUFSIZE = 256,
35   MTU = 256,
36   ACK_TIMEOUT = 1000000, /* in us */
37   SYNC_BYTE = SERIAL_HDLC_FLAG_BYTE,
38   ESCAPE_BYTE = SERIAL_HDLC_CTLESC_BYTE,
39
40   P_ACK = SERIAL_SERIAL_PROTO_ACK,
41   P_PACKET_ACK = SERIAL_SERIAL_PROTO_PACKET_ACK,
42   P_PACKET_NO_ACK = SERIAL_SERIAL_PROTO_PACKET_NOACK,
43   P_UNKNOWN = SERIAL_SERIAL_PROTO_PACKET_UNKNOWN
44 };
45
46 struct packet_list
47 {
48   uint8_t *packet;
49   int len;
50   struct packet_list *next;
51 };
52
53 struct serial_source {
54   int fd;
55   bool non_blocking;
56   void (*message)(serial_source_msg problem);
57
58   /* Receive state */
59   struct {
60     uint8_t buffer[BUFSIZE];
61     int bufpos, bufused;
62     uint8_t packet[MTU];
63     bool in_sync, escaped;
64     int count;
65     struct packet_list *queue[256]; // indexed by protocol
66   } recv;
67   struct {
68     uint8_t seqno;
69     uint8_t *escaped;
70     int escapeptr;
71     uint16_t crc;
72   } send;
73 };
74
75 static tcflag_t parse_baudrate(int requested)
76 {
77   int baudrate;
78
79   switch (requested)
80     {
81 #ifdef B50
82     case 50: baudrate = B50; break;
83 #endif
84 #ifdef B75
85     case 75: baudrate = B75; break;
86 #endif
87 #ifdef B110
88     case 110: baudrate = B110; break;
89 #endif
90 #ifdef B134
91     case 134: baudrate = B134; break;
92 #endif
93 #ifdef B150
94     case 150: baudrate = B150; break;
95 #endif
96 #ifdef B200
97     case 200: baudrate = B200; break;
98 #endif
99 #ifdef B300
100     case 300: baudrate = B300; break;
101 #endif
102 #ifdef B600
103     case 600: baudrate = B600; break;
104 #endif
105 #ifdef B1200
106     case 1200: baudrate = B1200; break;
107 #endif
108 #ifdef B1800
109     case 1800: baudrate = B1800; break;
110 #endif
111 #ifdef B2400
112     case 2400: baudrate = B2400; break;
113 #endif
114 #ifdef B4800
115     case 4800: baudrate = B4800; break;
116 #endif
117 #ifdef B9600
118     case 9600: baudrate = B9600; break;
119 #endif
120 #ifdef B19200
121     case 19200: baudrate = B19200; break;
122 #endif
123 #ifdef B38400
124     case 38400: baudrate = B38400; break;
125 #endif
126 #ifdef B57600
127     case 57600: baudrate = B57600; break;
128 #endif
129 #ifdef B115200
130     case 115200: baudrate = B115200; break;
131 #endif
132 #ifdef B230400
133     case 230400: baudrate = B230400; break;
134 #endif
135 #ifdef B460800
136     case 460800: baudrate = B460800; break;
137 #endif
138 #ifdef B500000
139     case 500000: baudrate = B500000; break;
140 #endif
141 #ifdef B576000
142     case 576000: baudrate = B576000; break;
143 #endif
144 #ifdef B921600
145     case 921600: baudrate = B921600; break;
146 #endif
147 #ifdef B1000000
148     case 1000000: baudrate = B1000000; break;
149 #endif
150 #ifdef B1152000
151     case 1152000: baudrate = B1152000; break;
152 #endif
153 #ifdef B1500000
154     case 1500000: baudrate = B1500000; break;
155 #endif
156 #ifdef B2000000
157     case 2000000: baudrate = B2000000; break;
158 #endif
159 #ifdef B2500000
160     case 2500000: baudrate = B2500000; break;
161 #endif
162 #ifdef B3000000
163     case 3000000: baudrate = B3000000; break;
164 #endif
165 #ifdef B3500000
166     case 3500000: baudrate = B3500000; break;
167 #endif
168 #ifdef B4000000
169     case 4000000: baudrate = B4000000; break;
170 #endif
171     default:
172       baudrate = 0;
173     }
174   return baudrate;
175 }
176
177 #ifdef DEBUG
178 static void dump(const char *msg, unsigned char *packet, int len)
179 {
180   int i;
181
182   printf("%s", msg);
183   for (i = 0; i < len; i++)
184     printf(" %02x", packet[i]);
185   putchar('\n');
186 }
187 #endif
188
189 static void message(serial_source src, serial_source_msg msg)
190 {
191   if (src->message)
192     src->message(msg);
193 }
194
195 static int serial_read(serial_source src, int non_blocking, void *buffer, int n)
196 {
197   fd_set fds;
198   int cnt;
199
200   if (non_blocking)
201     {
202       cnt = read(src->fd, buffer, n);
203
204       /* Work around buggy usb serial driver (returns 0 when no data
205          is available). Mac OS X seems to like to do this too (at
206          least with a Keyspan 49WG).
207       */
208       if (cnt == 0)
209         {
210           cnt = -1;
211           errno = EAGAIN;
212         }
213       return cnt;
214     }
215   else
216     for (;;)
217       {
218         FD_ZERO(&fds);
219         FD_SET(src->fd, &fds);
220         cnt = select(src->fd + 1, &fds, NULL, NULL, NULL);
221         if (cnt < 0)
222           return -1;
223
224         cnt = read(src->fd, buffer, n);
225         if (cnt != 0)
226           return cnt;
227       }
228 }
229
230 serial_source open_serial_source(const char *device, int baud_rate,
231                                  int non_blocking,
232                                  void (*message)(serial_source_msg problem))
233 /* Effects: opens serial port device at specified baud_rate. If non_blocking
234      is true, read_serial_packet calls will be non-blocking (writes are
235      always blocking, for now at least)
236    Returns: descriptor for serial forwarder at host:port, or
237      NULL for failure (bad device or bad baud rate)
238  */
239 {
240   struct termios newtio;
241   int fd;
242   tcflag_t baudflag = parse_baudrate(baud_rate);
243
244   if (!baudflag)
245     return NULL;
246
247   fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK);
248   if (fd < 0)
249     return NULL;
250
251 #ifdef __CYGWIN__
252   /* For some very mysterious reason, this incantation is necessary to make
253      the serial port work under some windows machines */
254   HANDLE handle = (HANDLE)get_osfhandle(fd);
255   DCB dcb;
256   if (!(GetCommState(handle, &dcb) && SetCommState(handle, &dcb)))
257     {
258       close(fd);
259       return NULL;
260     }
261 #endif
262   /* Serial port setting */
263   memset(&newtio, 0, sizeof(newtio));
264   newtio.c_cflag = CS8 | CLOCAL | CREAD;
265   newtio.c_iflag = IGNPAR | IGNBRK;
266   cfsetispeed(&newtio, baudflag);
267   cfsetospeed(&newtio, baudflag);
268
269   /* Raw output_file */
270   newtio.c_oflag = 0;
271
272   if (tcflush(fd, TCIFLUSH) >= 0 &&
273       tcsetattr(fd, TCSANOW, &newtio) >= 0)
274     {
275       serial_source src = malloc(sizeof *src);
276
277       if (src)
278         {
279           memset(src, 0, sizeof *src);
280           src->fd = fd;
281           src->non_blocking = non_blocking;
282           src->message = message;
283           src->send.seqno = 37;
284
285           return src;
286         }
287     }
288   close(fd);
289
290   return NULL;
291 }
292
293 int serial_source_fd(serial_source src)
294 /* Returns: the file descriptor used by serial source src (useful when
295      non-blocking reads were requested)
296 */
297 {
298   return src->fd;
299 }
300
301 int close_serial_source(serial_source src)
302 /* Effects: closes serial source src
303    Returns: 0 if successful, -1 if some problem occured (but source is
304      considered closed anyway)
305  */
306 {
307   int ok = close(src->fd);
308
309   free(src);
310
311   return ok;
312 }
313
314 static int source_wait(serial_source src, struct timeval *deadline)
315 /* Effects: waits until deadline for some data on source. deadline
316      can be NULL for indefinite waiting.
317    Returns: 0 if data is available, -1 if the deadline expires
318 */
319 {
320   struct timeval tv;
321   fd_set fds;
322   int cnt;
323
324   if (src->recv.bufpos < src->recv.bufused)
325     return 0;
326
327   for (;;)
328     {
329       if (deadline)
330         {
331           gettimeofday(&tv, NULL);
332           tv.tv_sec = deadline->tv_sec - tv.tv_sec;
333           tv.tv_usec = deadline->tv_usec - tv.tv_usec;
334           if (tv.tv_usec < 0)
335             {
336               tv.tv_usec += 1000000;
337               tv.tv_sec--;
338             }
339           if (tv.tv_sec < 0)
340             return -1;
341         }
342
343       FD_ZERO(&fds);
344       FD_SET(src->fd, &fds);
345       cnt = select(src->fd + 1, &fds, NULL, NULL, deadline ? &tv : NULL);
346       if (cnt < 0)
347         {
348           if (errno == EINTR)
349             continue;
350           message(src, msg_unix_error);
351           return -1;
352         }
353       if (cnt == 0)
354         return -1;
355       return 0;
356     }
357 }
358
359 static int source_write(serial_source src, const void *buffer, int count)
360 {
361   int actual = 0;
362
363   if (fcntl(src->fd, F_SETFL, 0) < 0)
364     {
365       message(src, msg_unix_error);
366       return -1;
367     }
368   while (count > 0)
369     {
370       int n = write(src->fd, buffer, count);
371
372       if (n < 0 && errno == EINTR)
373         continue;
374       if (n < 0)
375         {
376           message(src, msg_unix_error);
377           actual = -1;
378           break;
379         }
380
381       count -= n;
382       actual += n;
383       buffer += n;
384     }
385   if (fcntl(src->fd, F_SETFL, O_NONBLOCK) < 0)
386     {
387       message(src, msg_unix_error);
388       /* We're in trouble, but there's no obvious fix. */
389     }
390   return actual;
391 }
392
393 static void push_protocol_packet(serial_source src,
394                                  uint8_t type, uint8_t *packet, uint8_t len)
395 {
396   /* I'm assuming short queues */
397   struct packet_list *entry = malloc(sizeof *entry), **last;
398
399   if (!entry)
400     {
401       message(src, msg_no_memory);
402       free(packet);
403       return;
404     }
405
406   entry->packet = packet;
407   entry->len = len;
408   entry->next = NULL;
409
410   last = &src->recv.queue[type];
411   while (*last)
412     last = &(*last)->next;
413   *last = entry;
414 }
415
416 static struct packet_list *pop_protocol_packet(serial_source src, uint8_t type)
417 {
418   struct packet_list *entry = src->recv.queue[type];
419
420   if (entry)
421     src->recv.queue[type] = entry->next;
422
423   return entry;
424 }
425
426 static bool packet_available(serial_source src, uint8_t type)
427 {
428   return src->recv.queue[type] != NULL;
429 }
430
431 int serial_source_empty(serial_source src)
432 /* Returns: true if serial source does not contain any pending data, i.e.,
433      if the result is true and there is no data available on the source's
434      file descriptor, then read_serial_packet will:
435        - return NULL if the source is non-blocking
436        - block if it is blocking
437
438     (Note: the presence of this calls allows the serial_source to do some
439     internal buffering)
440 */
441 {
442   return src->recv.bufpos >= src->recv.bufused &&
443     !packet_available(src, P_PACKET_NO_ACK);
444 }
445
446 /* Slow implementation of crc function */
447 static uint16_t crc_byte(uint16_t crc, uint8_t b)
448 {
449   uint8_t i;
450   
451   crc = crc ^ b << 8;
452   i = 8;
453   do
454     if (crc & 0x8000)
455       crc = crc << 1 ^ 0x1021;
456     else
457       crc = crc << 1;
458   while (--i);
459
460   return crc;
461 }
462
463 static uint16_t crc_packet(uint8_t *data, int len)
464 {
465   uint16_t crc = 0;
466
467   while (len-- > 0)
468     crc = crc_byte(crc, *data++);
469
470   return crc;
471 }
472
473 static int read_byte(serial_source src, int non_blocking)
474 /* Returns: next byte (>= 0), or -1 if no data available and non-blocking is true.
475 */
476 {
477   if (src->recv.bufpos >= src->recv.bufused)
478     {
479       for (;;)
480         {
481           int n = serial_read(src, non_blocking, src->recv.buffer, sizeof src->recv.buffer);
482
483           if (n == 0) /* Can't occur because of serial_read bug workaround */
484             {
485               message(src, msg_closed);
486               return -1;
487             }
488           if (n > 0)
489             {
490 #ifdef DEBUG
491               dump("raw", src->recv.buffer, n);
492 #endif
493               src->recv.bufpos = 0;
494               src->recv.bufused = n;
495               break;
496             }
497           if (errno == EAGAIN)
498             return -1;
499           if (errno != EINTR)
500             message(src, msg_unix_error);
501         }
502     }
503   //printf("in %02x\n", src->recv.buffer[src->recv.bufpos]);
504   return src->recv.buffer[src->recv.bufpos++];
505 }
506
507 static void process_packet(serial_source src, uint8_t *packet, int len);
508 static int write_framed_packet(serial_source src,
509                                uint8_t packet_type, uint8_t first_byte,
510                                const uint8_t *packet, int count);
511
512 static void read_and_process(serial_source src, int non_blocking)
513 /* Effects: reads and processes up to one packet.
514 */
515 {
516   uint8_t *packet = src->recv.packet;
517
518   for (;;)
519     {
520       int byte = read_byte(src, non_blocking);
521
522       if (byte < 0)
523         return;
524
525       if (!src->recv.in_sync)
526         {
527           if (byte == SYNC_BYTE)
528             {
529               src->recv.in_sync = TRUE;
530               message(src, msg_sync);
531               src->recv.count = 0;
532               src->recv.escaped = FALSE;
533             }
534           continue;
535         }
536       if (src->recv.count >= MTU)
537         {
538           message(src, msg_too_long);
539           src->recv.in_sync = FALSE;
540           continue;
541         }
542       if (src->recv.escaped)
543         {
544           if (byte == SYNC_BYTE)
545             {
546               /* sync byte following escape is an error, resync */
547               message(src, msg_bad_sync);
548               src->recv.in_sync = FALSE;
549               continue;
550             }
551           byte ^= 0x20;
552           src->recv.escaped = FALSE;
553         }
554       else if (byte == ESCAPE_BYTE)
555         {
556           src->recv.escaped = TRUE;
557           continue;
558         }
559       else if (byte == SYNC_BYTE)
560         {
561           int count = src->recv.count;
562           uint8_t *received;
563           uint16_t read_crc, computed_crc;
564
565           src->recv.count = 0; /* ready for next packet */
566
567           if (count < 4)
568             /* frames that are too small are ignored */
569             continue;
570
571           received = malloc(count - 2);
572           if (!received)
573             {
574               message(src, msg_no_memory);
575               continue;
576             }
577           memcpy(received, packet, count - 2);
578
579           read_crc = packet[count - 2] | packet[count - 1] << 8;
580           computed_crc = crc_packet(received, count - 2);
581
582 #ifdef DEBUG
583           dump("received", packet, count);
584           printf("  crc %x comp %x\n", read_crc, computed_crc);
585 #endif
586           if (read_crc == computed_crc) 
587             {
588               process_packet(src, received, count - 2);
589               return; /* give rest of world chance to do something */
590             }
591           else
592             {
593               message(src, msg_bad_crc);
594               free(received);
595               /* We don't lose sync here. If we did, garbage on the line
596                  at startup will cause loss of the first packet. */
597               continue;
598             }
599         }
600       packet[src->recv.count++] = byte;
601     }
602 }
603
604 static void process_packet(serial_source src, uint8_t *packet, int len)
605 {
606   int packet_type = packet[0], offset = 1;
607
608   if (packet_type == P_PACKET_ACK)
609     {
610       /* send ack */
611       write_framed_packet(src, P_ACK, packet[1], NULL, 0);
612       /* And merge with un-acked packets */
613       packet_type = P_PACKET_NO_ACK;
614       offset = 2;
615     }
616   /* packet must remain a valid pointer to pass to free. So we move the
617      data rather than pass an internal pointer */
618   memmove(packet, packet + offset, len - offset);
619   push_protocol_packet(src, packet_type, packet, len - offset);
620 }
621
622 void *read_serial_packet(serial_source src, int *len)
623 /* Effects: Read the serial source src. If a packet is available, return it.
624      If in blocking mode and no packet is available, wait for one.
625    Returns: the packet read (in newly allocated memory), with *len is
626      set to the packet length, or NULL if no packet is yet available and
627      the serial source is in non-blocking mode
628 */
629 {
630   read_and_process(src, TRUE);
631   for (;;)
632     {
633       struct packet_list *entry;
634
635       entry = pop_protocol_packet(src, P_PACKET_NO_ACK);
636       if (entry)
637         {
638           uint8_t *packet = entry->packet;
639
640           *len = entry->len;
641           free(entry);
642
643           return packet;
644         }
645       if (src->non_blocking && serial_source_empty(src))
646         return NULL;
647       source_wait(src, NULL);
648       read_and_process(src, src->non_blocking);
649     }
650 }
651
652 /* The escaper does the sync bytes+escape-like encoding+crc of packets */
653
654 static void escape_add(serial_source src, uint8_t b)
655 {
656   src->send.escaped[src->send.escapeptr++] = b;
657 }
658
659 static int init_escaper(serial_source src, int count)
660 {
661   src->send.escaped = malloc(count * 2 + 2);
662   if (!src->send.escaped)
663     {
664       message(src, msg_no_memory);
665       return -1;
666     }
667   src->send.escapeptr = 0;
668   src->send.crc = 0;
669
670   escape_add(src, SYNC_BYTE);
671
672   return 0;
673 }
674
675 static void terminate_escaper(serial_source src)
676 {
677   escape_add(src, SYNC_BYTE);
678 }
679
680 static void escape_byte(serial_source src, uint8_t b)
681 {
682   src->send.crc = crc_byte(src->send.crc, b);
683   if (b == SYNC_BYTE || b == ESCAPE_BYTE)
684     {
685       escape_add(src, ESCAPE_BYTE);
686       escape_add(src, b ^ 0x20);
687     }
688   else
689     escape_add(src, b);
690 }
691
692 static void free_escaper(serial_source src)
693 {
694   free(src->send.escaped);
695 }
696
697 // Write a packet of type 'packetType', first byte 'firstByte'
698 // and bytes 2..'count'+1 in 'packet'
699 static int write_framed_packet(serial_source src,
700                                uint8_t packet_type, uint8_t first_byte,
701                                const uint8_t *packet, int count)
702 {
703   int i, crc;
704
705 #ifdef DEBUG
706   printf("writing %02x %02x", packet_type, first_byte);
707   dump("", packet, count);
708 #endif
709
710   if (init_escaper(src, count + 4) < 0)
711     return -1;
712         
713   escape_byte(src, packet_type);
714   escape_byte(src, first_byte);
715   for (i = 0; i < count; i++)
716     escape_byte(src, packet[i]);
717
718   crc = src->send.crc;
719   escape_byte(src, crc & 0xff);
720   escape_byte(src, crc >> 8);
721   
722   terminate_escaper(src);
723
724 #ifdef DEBUG
725   dump("encoded", src->send.escaped, src->send.escapeptr);
726 #endif
727
728   if (source_write(src, src->send.escaped, src->send.escapeptr) < 0)
729     {
730       free_escaper(src);
731       return -1;
732     }
733   free_escaper(src);
734   return 0;
735 }
736
737 static void add_timeval(struct timeval *tv, long us)
738 /* Specialised for this app */
739 {
740   tv->tv_sec += us / 1000000;
741   tv->tv_usec += us % 1000000;
742   if (tv->tv_usec > 1000000)
743     {
744       tv->tv_usec -= 1000000;
745       tv->tv_sec++;
746     }
747 }
748
749 int write_serial_packet(serial_source src, const void *packet, int len)
750 /* Effects: writes len byte packet to serial source src
751    Returns: 0 if packet successfully written, 1 if successfully written
752      but not acknowledged, -1 otherwise
753 */
754 {
755   struct timeval deadline;
756
757   src->send.seqno++;
758   if (write_framed_packet(src, P_PACKET_ACK, src->send.seqno, packet, len) < 0)
759     return -1;
760
761   gettimeofday(&deadline, NULL);
762   add_timeval(&deadline, ACK_TIMEOUT);
763   for (;;) 
764     {
765       struct packet_list *entry;
766       
767       read_and_process(src, TRUE);
768       entry = pop_protocol_packet(src, P_ACK);
769       if (entry)
770         {
771           uint8_t acked = entry->packet[0];
772
773           free(entry->packet);
774           free(entry);
775           if (acked == src->send.seqno)
776             return 0;
777         }
778       else if (source_wait(src, &deadline) < 0)
779         return 1;
780     }
781 }
782
783 /* This somewhat convoluted code allows us to use a common baudrate table
784    with the Java code. This could be improved if we generated the Java
785    code from a common table.
786 */
787
788 struct pargs {
789   char *name;
790   int rate;
791 };
792
793 static void padd(struct pargs *args, const char *name, int baudrate)
794 {
795   if (!strcmp(args->name, name))
796     args->rate = baudrate;
797 }
798
799 static void init(void) { }
800
801 int platform_baud_rate(char *platform_name)
802 /* Returns: The baud rate of the specified platform, or -1 for unknown
803      platforms
804 */
805 {
806   /* The Java code looks like Platform.add(Platform.x, "name", baudrate); 
807      Fake up some C stuff which will make that work right. */
808   struct pargs args;
809   struct {
810     void (*add)(struct pargs *args, const char *name, int baudrate);
811     struct pargs *x;
812   } Platform = { padd, &args };
813
814   if (isdigit(platform_name[0]))
815     return atoi(platform_name);
816
817   args.name = platform_name;
818   args.rate = -1;
819
820 #define class
821 #define BaudRate
822 #define static
823 #define void
824 #define throws ;
825 #define Exception
826 #define package
827 #include "./BaudRate.java"
828
829   return args.rate;
830 }