00001 #include <sys/types.h>
00002 #include <sys/stat.h>
00003 #include <termios.h>
00004 #include <unistd.h>
00005 #include <stdint.h>
00006 #include <errno.h>
00007 #include <fcntl.h>
00008 #include <stdlib.h>
00009 #include <string.h>
00010 #include <sys/time.h>
00011 #include <stdio.h>
00012 #ifdef __CYGWIN__
00013 #include <windows.h>
00014 #include <io.h>
00015 #endif
00016
00017
00018
00019
00020 #define DEBUG 1
00021
00022 #include "serialsource.h"
00023 #include "OscopeMsg.h"
00024
00025 typedef struct {
00026 uint8_t type;
00027 uint16_t dest_id;
00028 uint8_t handler;
00029 uint8_t group_id;
00030 uint8_t length;
00031 }tos_header;
00032
00033 typedef int bool;
00034
00035 enum {
00036 #ifndef __CYGWIN__
00037 FALSE = 0,
00038 TRUE = 1,
00039 #endif
00040 BUFSIZE = 256,
00041 MTU = 256,
00042 ACK_TIMEOUT = 1000000,
00043 SYNC_BYTE = 0x7e,
00044 ESCAPE_BYTE = 0x7d,
00045
00046 P_ACK = 64,
00047 P_PACKET_ACK = 65,
00048 P_PACKET_NO_ACK = 66,
00049 P_UNKNOWN = 255
00050 };
00051
00052 struct packet_list
00053 {
00054 uint8_t *packet;
00055 int len;
00056 struct packet_list *next;
00057 };
00058
00059 struct serial_source {
00060 int fd;
00061 bool non_blocking;
00062 void (*message)(serial_source_msg problem);
00063
00064
00065 struct {
00066 uint8_t buffer[BUFSIZE];
00067 int bufpos, bufused;
00068 uint8_t packet[MTU];
00069 bool in_sync, escaped;
00070 int count;
00071 struct packet_list *queue[256];
00072 } recv;
00073 struct {
00074 uint8_t seqno;
00075 uint8_t *escaped;
00076 int escapeptr;
00077 uint16_t crc;
00078 } send;
00079 };
00080
00081 static tcflag_t parse_baudrate(int requested)
00082 {
00083 int baudrate;
00084
00085 switch (requested)
00086 {
00087 #ifdef B50
00088 case 50: baudrate = B50; break;
00089 #endif
00090 #ifdef B75
00091 case 75: baudrate = B75; break;
00092 #endif
00093 #ifdef B110
00094 case 110: baudrate = B110; break;
00095 #endif
00096 #ifdef B134
00097 case 134: baudrate = B134; break;
00098 #endif
00099 #ifdef B150
00100 case 150: baudrate = B150; break;
00101 #endif
00102 #ifdef B200
00103 case 200: baudrate = B200; break;
00104 #endif
00105 #ifdef B300
00106 case 300: baudrate = B300; break;
00107 #endif
00108 #ifdef B600
00109 case 600: baudrate = B600; break;
00110 #endif
00111 #ifdef B1200
00112 case 1200: baudrate = B1200; break;
00113 #endif
00114 #ifdef B1800
00115 case 1800: baudrate = B1800; break;
00116 #endif
00117 #ifdef B2400
00118 case 2400: baudrate = B2400; break;
00119 #endif
00120 #ifdef B4800
00121 case 4800: baudrate = B4800; break;
00122 #endif
00123 #ifdef B9600
00124 case 9600: baudrate = B9600; break;
00125 #endif
00126 #ifdef B19200
00127 case 19200: baudrate = B19200; break;
00128 #endif
00129 #ifdef B38400
00130 case 38400: baudrate = B38400; break;
00131 #endif
00132 #ifdef B57600
00133 case 57600: baudrate = B57600; break;
00134 #endif
00135 #ifdef B115200
00136 case 115200: baudrate = B115200; break;
00137 #endif
00138 #ifdef B230400
00139 case 230400: baudrate = B230400; break;
00140 #endif
00141 #ifdef B460800
00142 case 460800: baudrate = B460800; break;
00143 #endif
00144 #ifdef B500000
00145 case 500000: baudrate = B500000; break;
00146 #endif
00147 #ifdef B576000
00148 case 576000: baudrate = B576000; break;
00149 #endif
00150 #ifdef B921600
00151 case 921600: baudrate = B921600; break;
00152 #endif
00153 #ifdef B1000000
00154 case 1000000: baudrate = B1000000; break;
00155 #endif
00156 #ifdef B1152000
00157 case 1152000: baudrate = B1152000; break;
00158 #endif
00159 #ifdef B1500000
00160 case 1500000: baudrate = B1500000; break;
00161 #endif
00162 #ifdef B2000000
00163 case 2000000: baudrate = B2000000; break;
00164 #endif
00165 #ifdef B2500000
00166 case 2500000: baudrate = B2500000; break;
00167 #endif
00168 #ifdef B3000000
00169 case 3000000: baudrate = B3000000; break;
00170 #endif
00171 #ifdef B3500000
00172 case 3500000: baudrate = B3500000; break;
00173 #endif
00174 #ifdef B4000000
00175 case 4000000: baudrate = B4000000; break;
00176 #endif
00177 default:
00178 baudrate = 0;
00179 }
00180 return baudrate;
00181 }
00182
00183 #ifdef DEBUG
00184 static void dump(const char *msg, const uint8_t *packet, int len)
00185 {
00186 int i;
00187
00188 printf("%s (%d bytes)", msg, len);
00189 for (i = 0; i < len; i++)
00190 printf(" %02x", packet[i]);
00191 putchar('\n');
00192 }
00193 #endif
00194
00195 static void message(serial_source src, serial_source_msg msg)
00196 {
00197 if (src->message)
00198 src->message(msg);
00199 }
00200
00201
00202
00203 static int buggyread(serial_source src, void *buffer, int n)
00204 {
00205 fd_set fds;
00206 int cnt;
00207
00208 if (src->non_blocking)
00209 {
00210 cnt = read(src->fd, buffer, n);
00211 if (cnt == 0)
00212 {
00213 cnt = -1;
00214 errno = EAGAIN;
00215 }
00216 return cnt;
00217 }
00218 else
00219 for (;;)
00220 {
00221 FD_ZERO(&fds);
00222 FD_SET(src->fd, &fds);
00223 cnt = select(src->fd + 1, &fds, NULL, NULL, NULL);
00224 if (cnt < 0)
00225 return -1;
00226
00227 cnt = read(src->fd, buffer, n);
00228 if (cnt != 0)
00229 return cnt;
00230 }
00231 }
00232
00233 serial_source open_serial_source(const char *device, int baud_rate,
00234 int non_blocking,
00235 void (*message)(serial_source_msg problem))
00236
00237
00238
00239
00240
00241
00242 {
00243 struct termios newtio;
00244 int fd;
00245 tcflag_t baudflag = parse_baudrate(baud_rate);
00246
00247 if (!baudflag)
00248 return NULL;
00249
00250 fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK);
00251 if (fd < 0)
00252 return NULL;
00253
00254 #ifdef __CYGWIN__
00255
00256
00257 HANDLE handle = (HANDLE)get_osfhandle(fd);
00258 DCB dcb;
00259 if (!(GetCommState(handle, &dcb) && SetCommState(handle, &dcb)))
00260 {
00261 close(fd);
00262 return NULL;
00263 }
00264 #endif
00265
00266 memset(&newtio, 0, sizeof(newtio));
00267 newtio.c_cflag = CS8 | CLOCAL | CREAD;
00268 newtio.c_iflag = IGNPAR | IGNBRK;
00269 cfsetispeed(&newtio, baudflag);
00270 cfsetospeed(&newtio, baudflag);
00271
00272
00273 newtio.c_oflag = 0;
00274 if (tcflush(fd, TCIFLUSH) >= 0 &&
00275 tcsetattr(fd, TCSANOW, &newtio) >= 0)
00276 {
00277 serial_source src = malloc(sizeof *src);
00278
00279 if (src)
00280 {
00281 memset(src, 0, sizeof src);
00282 src->fd = fd;
00283 src->non_blocking = non_blocking;
00284 src->message = message;
00285 src->send.seqno = 37;
00286
00287 return src;
00288 }
00289 }
00290 close(fd);
00291
00292 return NULL;
00293 }
00294
00295 int serial_source_fd(serial_source src)
00296
00297
00298
00299 {
00300 return src->fd;
00301 }
00302
00303 int close_serial_source(serial_source src)
00304
00305
00306
00307
00308 {
00309 int ok = close(src->fd);
00310
00311 free(src);
00312
00313 return ok;
00314 }
00315
00316 static int source_wait(serial_source src, struct timeval *deadline)
00317
00318
00319
00320
00321 {
00322 struct timeval tv;
00323 fd_set fds;
00324 int cnt;
00325
00326 if (src->recv.bufpos < src->recv.bufused)
00327 return 0;
00328
00329 for (;;)
00330 {
00331 printf("SOURCE WAITING\n");
00332 if (deadline)
00333 {
00334 gettimeofday(&tv, NULL);
00335 tv.tv_sec = deadline->tv_sec - tv.tv_sec;
00336 tv.tv_usec = deadline->tv_usec - tv.tv_usec;
00337 if (tv.tv_usec < 0)
00338 {
00339 tv.tv_usec += 1000000;
00340 tv.tv_sec--;
00341 }
00342 if (tv.tv_sec < 0)
00343 return -1;
00344 }
00345
00346 FD_ZERO(&fds);
00347 FD_SET(src->fd, &fds);
00348 cnt = select(src->fd + 1, &fds, NULL, NULL, deadline ? &tv : NULL);
00349 if (cnt < 0)
00350 {
00351 if (errno == EINTR)
00352 continue;
00353 message(src, msg_unix_error);
00354 return -1;
00355 }
00356 if (cnt == 0)
00357 return -1;
00358 return 0;
00359 }
00360 }
00361
00362 static int source_write(serial_source src, const void *buffer, int count)
00363 {
00364 int actual = 0;
00365
00366 if (fcntl(src->fd, F_SETFL, 0) < 0)
00367 {
00368 message(src, msg_unix_error);
00369 return -1;
00370 }
00371 while (count > 0)
00372 {
00373 int n = write(src->fd, buffer, count);
00374
00375 if (n < 0 && errno == EINTR)
00376 continue;
00377 if (n < 0)
00378 {
00379 message(src, msg_unix_error);
00380 actual = -1;
00381 break;
00382 }
00383
00384 count -= n;
00385 actual += n;
00386 buffer += n;
00387 }
00388 if (fcntl(src->fd, F_SETFL, O_NONBLOCK) < 0)
00389 {
00390 message(src, msg_unix_error);
00391
00392 }
00393 return actual;
00394 }
00395
00396 static void push_protocol_packet(serial_source src,
00397 uint8_t type, uint8_t *packet, uint8_t len)
00398 {
00399
00400 struct packet_list *entry = malloc(sizeof *packet), **last;
00401
00402 if (!entry)
00403 {
00404 message(src, msg_no_memory);
00405 free(packet);
00406 return;
00407 }
00408
00409 entry->packet = packet;
00410 entry->len = len;
00411 entry->next = NULL;
00412
00413 last = &src->recv.queue[type];
00414 while (*last)
00415 last = &(*last)->next;
00416 *last = entry;
00417 }
00418
00419 static struct packet_list *pop_protocol_packet(serial_source src, uint8_t type)
00420 {
00421 struct packet_list *entry = src->recv.queue[type];
00422
00423 if (entry)
00424 src->recv.queue[type] = entry->next;
00425
00426 return entry;
00427 }
00428
00429 static bool packet_available(serial_source src, uint8_t type)
00430 {
00431 return src->recv.queue[type] != NULL;
00432 }
00433
00434 int serial_source_empty(serial_source src)
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444 {
00445 return src->recv.bufpos >= src->recv.bufused &&
00446 !packet_available(src, P_PACKET_NO_ACK);
00447 }
00448
00449
00450 static uint16_t crc_byte(uint16_t crc, uint8_t b)
00451 {
00452 uint8_t i;
00453
00454 crc = crc ^ b << 8;
00455 i = 8;
00456 do
00457 if (crc & 0x8000)
00458 crc = crc << 1 ^ 0x1021;
00459 else
00460 crc = crc << 1;
00461 while (--i);
00462
00463 return crc;
00464 }
00465
00466 static uint16_t crc_packet(uint8_t *data, int len)
00467 {
00468 uint16_t crc = 0;
00469
00470 while (len-- > 0)
00471 crc = crc_byte(crc, *data++);
00472
00473 return crc;
00474 }
00475
00476 static int read_byte(serial_source src)
00477
00478
00479
00480 {
00481 if (src->recv.bufpos >= src->recv.bufused)
00482 {
00483 for (;;)
00484 {
00485 int n = buggyread(src, src->recv.buffer, sizeof src->recv.buffer);
00486 if (n == 0)
00487 {
00488 message(src, msg_closed);
00489 return -1;
00490 }
00491 if (n > 0)
00492 {
00493 #ifdef DEBUG
00494 dump("raw", src->recv.buffer, n);
00495 #endif
00496 src->recv.bufpos = 0;
00497 src->recv.bufused = n;
00498 break;
00499 }
00500 if (errno == EAGAIN)
00501 return -1;
00502 if (errno != EINTR)
00503 message(src, msg_unix_error);
00504 }
00505 }
00506 #ifdef DEBUG
00507
00508 #endif
00509 return src->recv.buffer[src->recv.bufpos++];
00510 }
00511
00512 static void process_packet(serial_source src, uint8_t *packet, int len);
00513 static int write_framed_packet(serial_source src,
00514 uint8_t packet_type, uint8_t first_byte,
00515 const uint8_t *packet, int count);
00516
00517 static void read_and_process(serial_source src)
00518
00519
00520 {
00521 uint8_t *packet = src->recv.packet;
00522
00523 for (;;)
00524 {
00525 int byte = read_byte(src);
00526
00527 if (byte < 0)
00528 return;
00529
00530 if (!src->recv.in_sync)
00531 {
00532 if (byte == SYNC_BYTE)
00533 {
00534 #ifdef DEBUG
00535 printf("out of sync: received SYNC_BYTE\n");
00536 #endif
00537 src->recv.in_sync = TRUE;
00538 message(src, msg_sync);
00539 src->recv.count = 0;
00540 src->recv.escaped = FALSE;
00541 }
00542 continue;
00543 }
00544 if (src->recv.count >= MTU)
00545 {
00546 #ifdef DEBUG
00547 printf("MTU exceeded\n");
00548 #endif
00549 message(src, msg_too_long);
00550 src->recv.in_sync = FALSE;
00551 continue;
00552 }
00553 if (src->recv.escaped)
00554 {
00555 if (byte == SYNC_BYTE)
00556 {
00557
00558 message(src, msg_bad_sync);
00559 src->recv.in_sync = FALSE;
00560 continue;
00561 }
00562 byte ^= 0x20;
00563 src->recv.escaped = FALSE;
00564 }
00565 else if (byte == ESCAPE_BYTE)
00566 {
00567 #ifdef DEBUG
00568 printf("sync'ed: ESCAPE_BYTE\n");
00569 #endif
00570 src->recv.escaped = TRUE;
00571 continue;
00572 }
00573 else if (byte == SYNC_BYTE)
00574 {
00575 int count = src->recv.count;
00576 uint8_t *received;
00577 uint16_t read_crc, computed_crc;
00578
00579 #ifdef DEBUG
00580 printf("sync'ed: SYNC_BYTE received\n");
00581 #endif
00582
00583 src->recv.count = 0;
00584
00585 if (count < 4)
00586
00587 continue;
00588
00589 received = malloc(count - 2);
00590 if (!received)
00591 {
00592 message(src, msg_no_memory);
00593 continue;
00594 }
00595 memcpy(received, packet, count - 2);
00596
00597 read_crc = packet[count - 2] | packet[count - 1] << 8;
00598 computed_crc = crc_packet(received, count - 2);
00599
00600
00601
00602
00603
00604
00605
00606 #ifdef DEBUG
00607
00608 printf(" crc %x comp %x\n", read_crc, computed_crc);
00609 #endif
00610 if (read_crc == computed_crc)
00611 {
00612 process_packet(src, received, count - 2);
00613 return;
00614 }
00615 else
00616 {
00617 message(src, msg_bad_crc);
00618
00619
00620 continue;
00621 }
00622 }
00623 packet[src->recv.count++] = byte;
00624 }
00625 }
00626
00627 static void process_packet(serial_source src, uint8_t *packet, int len)
00628 {
00629 int packet_type = packet[0], offset = 1;
00630 tos_header * header = (tos_header *) packet;
00631 OscopeMsg * msg;
00632 uint8_t* buf;
00633 tos_header * buf_header;
00634 OscopeAck * ack;
00635 #ifdef DEBUG
00636 dump("process_packet", packet, len);
00637 #endif
00638
00639
00640 if (header->handler == AM_OSCOPEMSG)
00641 {
00642 msg = (OscopeMsg *) packet + sizeof(tos_header);
00643
00644 buf = malloc(sizeof( tos_header) + sizeof( OscopeAck));
00645 buf_header = (tos_header *) buf;
00646 ack = (OscopeAck *) (buf + sizeof( tos_header));
00647 buf_header->dest_id = ack->sourceMoteID;
00648 buf_header->handler = AM_OSCOPEACK;
00649 buf_header->group_id = header->group_id;
00650 buf_header->length = sizeof( tos_header) + sizeof( OscopeAck);
00651
00652 ack->sourceMoteID = msg->sourceMoteID;
00653 ack->lastSampleNumber = msg->lastSampleNumber;
00654 ack->channel = msg->channel;
00655
00656 write_serial_packet(src, buf, buf_header->length);
00657 }
00658
00659 if (packet_type == P_PACKET_ACK)
00660 {
00661
00662 write_framed_packet(src, P_ACK, packet[1], NULL, 0);
00663
00664 packet_type = P_PACKET_NO_ACK;
00665 offset = 2;
00666 }
00667
00668
00669
00670 memmove(packet, packet + offset, len - offset);
00671 push_protocol_packet(src, packet_type, packet, len - offset);
00672 }
00673
00674 void *read_serial_packet(serial_source src, int *len)
00675
00676
00677
00678
00679
00680
00681 {
00682 for (;;)
00683 {
00684 struct packet_list *entry;
00685
00686 read_and_process(src);
00687 entry = pop_protocol_packet(src, P_PACKET_NO_ACK);
00688 if (entry)
00689 {
00690 uint8_t *packet = entry->packet;
00691
00692 *len = entry->len;
00693 free(entry);
00694
00695 return packet;
00696 }
00697 if (src->non_blocking && serial_source_empty(src))
00698 return NULL;
00699 source_wait(src, NULL);
00700 }
00701 }
00702
00703
00704
00705 static void escape_add(serial_source src, uint8_t b)
00706 {
00707 src->send.escaped[src->send.escapeptr++] = b;
00708 }
00709
00710 static int init_escaper(serial_source src, int count)
00711 {
00712 src->send.escaped = malloc(count * 2 + 2);
00713 if (!src->send.escaped)
00714 {
00715 message(src, msg_no_memory);
00716 return -1;
00717 }
00718 src->send.escapeptr = 0;
00719 src->send.crc = 0;
00720
00721 escape_add(src, SYNC_BYTE);
00722
00723 return 0;
00724 }
00725
00726 static void terminate_escaper(serial_source src)
00727 {
00728 escape_add(src, SYNC_BYTE);
00729 }
00730
00731 static void escape_byte(serial_source src, uint8_t b)
00732 {
00733 src->send.crc = crc_byte(src->send.crc, b);
00734 if (b == SYNC_BYTE || b == ESCAPE_BYTE)
00735 {
00736 escape_add(src, ESCAPE_BYTE);
00737 escape_add(src, b ^ 0x20);
00738 }
00739 else
00740 escape_add(src, b);
00741 }
00742
00743 static void free_escaper(serial_source src)
00744 {
00745 free(src->send.escaped);
00746 }
00747
00748
00749
00750 static int write_framed_packet(serial_source src,
00751 uint8_t packet_type, uint8_t first_byte,
00752 const uint8_t *packet, int count)
00753 {
00754 int i, crc;
00755
00756 #ifdef DEBUG
00757 printf("writing %02x %02x", packet_type, first_byte);
00758 dump("writing", packet, count);
00759 #endif
00760
00761 if (init_escaper(src, count + 4) < 0)
00762 return -1;
00763
00764 escape_byte(src, packet_type);
00765 escape_byte(src, first_byte);
00766 for (i = 0; i < count; i++)
00767 escape_byte(src, packet[i]);
00768
00769 crc = src->send.crc;
00770 escape_byte(src, crc & 0xff);
00771 escape_byte(src, crc >> 8);
00772
00773 terminate_escaper(src);
00774
00775 #ifdef DEBUG
00776 dump("encoded", src->send.escaped, src->send.escapeptr);
00777 #endif
00778
00779 if (source_write(src, src->send.escaped, src->send.escapeptr) < 0)
00780 {
00781 free_escaper(src);
00782 return -1;
00783 }
00784 free_escaper(src);
00785 return 0;
00786 }
00787
00788 static void add_timeval(struct timeval *tv, long us)
00789
00790 {
00791 tv->tv_sec += us / 1000000;
00792 tv->tv_usec += us % 1000000;
00793 if (tv->tv_usec > 1000000)
00794 {
00795 tv->tv_usec -= 1000000;
00796 tv->tv_sec++;
00797 }
00798 }
00799
00800 int write_serial_packet(serial_source src, const void *packet, int len)
00801
00802
00803
00804
00805 {
00806 struct timeval deadline;
00807
00808 #ifdef DEBUG
00809 dump("write", packet, len);
00810 #endif
00811
00812 src->send.seqno++;
00813 if (write_framed_packet(src, P_PACKET_ACK, src->send.seqno, packet, len) < 0)
00814 return -1;
00815
00816 gettimeofday(&deadline, NULL);
00817 add_timeval(&deadline, ACK_TIMEOUT);
00818 for (;;)
00819 {
00820 struct packet_list *entry;
00821
00822 read_and_process(src);
00823 entry = pop_protocol_packet(src, P_ACK);
00824 if (entry)
00825 {
00826 uint8_t acked = entry->packet[0];
00827
00828 free(entry->packet);
00829 free(entry);
00830 if (acked == src->send.seqno)
00831 return 0;
00832 }
00833 else if (source_wait(src, &deadline) < 0)
00834 return 1;
00835 }
00836 }