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