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