IEEE 802.15.4 subsystem
Sign in or create your account | Project List | Help
IEEE 802.15.4 subsystem Git Source Tree
Root/
1 | /* |
2 | * dirtpan/dirtpan.c - Quick and dirty IPv4 over 802.15.4 tunnel |
3 | * |
4 | * Written 2011 by Werner Almesberger |
5 | * Copyright 2011 Werner Almesberger |
6 | * |
7 | * This program is free software; you can redistribute it and/or modify |
8 | * it under the terms of the GNU General Public License as published by |
9 | * the Free Software Foundation; either version 2 of the License, or |
10 | * (at your option) any later version. |
11 | */ |
12 | |
13 | |
14 | #include <stdarg.h> |
15 | #include <stdint.h> |
16 | #include <stdlib.h> |
17 | #include <stdio.h> |
18 | #include <unistd.h> |
19 | #include <string.h> |
20 | #include <fcntl.h> |
21 | #include <signal.h> |
22 | #include <errno.h> |
23 | #include <assert.h> |
24 | #include <sys/types.h> |
25 | #include <sys/time.h> |
26 | #include <sys/wait.h> |
27 | #include <sys/select.h> |
28 | #include <sys/ioctl.h> |
29 | #include <sys/socket.h> |
30 | #include <linux/if.h> |
31 | #include <linux/if_tun.h> |
32 | |
33 | #include <ieee802154.h> |
34 | |
35 | #include "daemon.h" |
36 | |
37 | |
38 | /* |
39 | * Control byte structure: |
40 | * |
41 | * +--7--+--6--+--5--+--4--+--3--+--2--+--1--+--0--+ |
42 | * | 0 | 0 | 0 | 0 | 0 | seq | pck_type | |
43 | * +-----+-----+-----+-----+-----+-----+-----+-----+ |
44 | * ============= |
45 | * |
46 | * Bits 6 and 7 must be zero to avoid conflicts with RFC 4944 (LoWPAN) |
47 | */ |
48 | |
49 | #define SEQ 4 |
50 | #define PT_MASK 3 |
51 | |
52 | enum packet_type { |
53 | pt_first = 0, |
54 | pt_next = 1, |
55 | pt_ack = 2, |
56 | }; |
57 | |
58 | |
59 | #define TUN_DEV "/dev/net/tun" |
60 | |
61 | #define MAX_FRAG (127-11-2-1) /* MHDR, FCS, control byte */ |
62 | #define MAX_PACKET 2000 |
63 | #define MAX_TRIES 5 |
64 | #define T_REASS_MS 200 |
65 | #define T_ACK_MS 50 |
66 | |
67 | |
68 | static struct { |
69 | unsigned tx_pck; /* packets enqueued */ |
70 | unsigned lost; /* packets never acked */ |
71 | unsigned rx_pck; /* packets successfully received */ |
72 | unsigned tx_frm; /* frames sent */ |
73 | unsigned rx_frm; /* frames received */ |
74 | unsigned tx_frst; /* pt_first sent */ |
75 | unsigned rx_frst; /* pt_first received */ |
76 | unsigned tx_nxt; /* pt_next sent */ |
77 | unsigned rx_nxt; /* pt_next received */ |
78 | unsigned tx_ack; /* pt_ack sent */ |
79 | unsigned rx_ack; /* pt_ack received */ |
80 | unsigned invalid; /* invalid packet type */ |
81 | unsigned not_rx; /* pt_next but not receiving */ |
82 | unsigned rx_seq; /* wrong rx sequence */ |
83 | unsigned not_tx; /* pt_ack but not sending */ |
84 | unsigned ack_seq; /* wrong ack sequence */ |
85 | unsigned garbled; /* IPv4 length problem */ |
86 | unsigned no_ack; /* no ack response (giving up) */ |
87 | unsigned retry; /* retransmit */ |
88 | unsigned reass; /* reassembly timeout */ |
89 | } stats; |
90 | |
91 | static int tun, net; |
92 | static uint8_t rx_packet[MAX_PACKET], tx_packet[MAX_PACKET+1]; |
93 | static void *rx_pos, *tx_pos; |
94 | static int rx_left, tx_left; |
95 | static int txing = 0, rxing = 0; |
96 | static int rx_seq, tx_seq = 0; |
97 | static int retries; |
98 | static int debug = 0; |
99 | |
100 | |
101 | /* ----- Debugging --------------------------------------------------------- */ |
102 | |
103 | |
104 | static void debug_label(const char *label) |
105 | { |
106 | fprintf(stderr, "%s(%c%c)", |
107 | label, txing ? 'T' : '-', rxing ? 'R' : '-'); |
108 | } |
109 | |
110 | |
111 | static void dump(const void *buf, int size) |
112 | { |
113 | const uint8_t *p = buf; |
114 | |
115 | while (size--) { |
116 | fprintf(stderr, "%s%02x", p == buf ? "" : " ", *p); |
117 | p++; |
118 | } |
119 | } |
120 | |
121 | |
122 | static void debug_ip(const char *label, void *buf, int size) |
123 | { |
124 | if (debug < 2) |
125 | return; |
126 | debug_label(label); |
127 | fprintf(stderr, ", %d: ", size); |
128 | dump(buf, size); |
129 | fprintf(stderr, "\n"); |
130 | } |
131 | |
132 | |
133 | static void debug_dirt(const char *label, void *buf, int size) |
134 | { |
135 | const uint8_t *p = buf; |
136 | |
137 | if (!debug) |
138 | return; |
139 | if (debug == 1) { |
140 | if (size) { |
141 | fprintf(stderr, "%c%d", |
142 | (label[1] == '>' ? "FNA?" : "fna?")[*p & PT_MASK], |
143 | *p & SEQ ? 0 : 1); |
144 | } |
145 | return; |
146 | } |
147 | debug_label(label); |
148 | fprintf(stderr, ", %d+1: ", size-1); |
149 | if (size) { |
150 | fprintf(stderr, "%02x(%c%d) | ", |
151 | *p, "FNA?"[*p & PT_MASK], *p & SEQ ? 0 : 1); |
152 | dump(buf+1, size-1); |
153 | } |
154 | fprintf(stderr, "\n"); |
155 | |
156 | } |
157 | |
158 | |
159 | static void debug_timeout(const char *label) |
160 | { |
161 | if (!debug) |
162 | return; |
163 | if (debug == 1) { |
164 | fprintf(stderr, "*%c", *label); |
165 | return; |
166 | } |
167 | debug_label(label); |
168 | fprintf(stderr, "\n"); |
169 | } |
170 | |
171 | |
172 | /* ----- Statistics -------------------------------------------------------- */ |
173 | |
174 | |
175 | static void handle_usr1(int sig) |
176 | { |
177 | fprintf(stderr, "\n" |
178 | "tx_pck\t%6u\n" "tx_lost\t%6u\n" "rx_pck\t%6u\n" |
179 | "tx_frm\t%6u\n" "rx_frm\t%6u\n" "tx_frst\t%6u\n" |
180 | "rx_frst\t%6u\n" "tx_nxt\t%6u\n" "rx_nxt\t%6u\n" |
181 | "tx_ack\t%6u\n" "rx_ack\t%6u\n" "invalid\t%6u\n" |
182 | "not_rx\t%6u\n" "rx_seq\t%6u\n" "not_tx\t%6u\n" |
183 | "ack_seq\t%6u\n" "garbled\t%6u\n" "no_ack\t%6u\n" |
184 | "retry\t%6u\n" "reass\t%6u\n", |
185 | stats.tx_pck, stats.lost, stats.rx_pck, |
186 | stats.tx_frm, stats.rx_frm, stats.tx_frst, |
187 | stats.rx_frst, stats.tx_nxt, stats.rx_nxt, |
188 | stats.tx_ack, stats.rx_ack, stats.invalid, |
189 | stats.not_rx, stats.rx_seq, stats.not_tx, |
190 | stats.ack_seq, stats.garbled, stats.no_ack, |
191 | stats.retry, stats.reass); |
192 | } |
193 | |
194 | |
195 | static void handle_usr2(int sig) |
196 | { |
197 | memset(&stats, 0, sizeof(stats)); |
198 | } |
199 | |
200 | |
201 | /* ----- Timers ------------------------------------------------------------ */ |
202 | |
203 | |
204 | static struct timeval t_reass, t_ack; |
205 | |
206 | |
207 | static void start_timer(struct timeval *t, int ms) |
208 | { |
209 | assert(!t->tv_sec && !t->tv_usec); |
210 | gettimeofday(t, NULL); |
211 | t->tv_usec += 1000*ms; |
212 | while (t->tv_usec >= 1000000) { |
213 | t->tv_sec++; |
214 | t->tv_usec -= 1000000; |
215 | } |
216 | } |
217 | |
218 | |
219 | static void stop_timer(struct timeval *t) |
220 | { |
221 | assert(t->tv_sec || t->tv_usec); |
222 | t->tv_sec = 0; |
223 | t->tv_usec = 0; |
224 | } |
225 | |
226 | |
227 | static const struct timeval *next_timer(int n, ...) |
228 | { |
229 | va_list ap; |
230 | const struct timeval *next = NULL; |
231 | const struct timeval *t; |
232 | |
233 | va_start(ap, n); |
234 | while (n--) { |
235 | t = va_arg(ap, const struct timeval *); |
236 | if (!t->tv_sec && !t->tv_usec) |
237 | continue; |
238 | if (next) { |
239 | if (next->tv_sec < t->tv_sec) |
240 | continue; |
241 | if (next->tv_sec == t->tv_sec && |
242 | next->tv_usec < t->tv_usec) |
243 | continue; |
244 | } |
245 | next = t; |
246 | } |
247 | va_end(ap); |
248 | return next; |
249 | } |
250 | |
251 | |
252 | static struct timeval *timer_delta(const struct timeval *t) |
253 | { |
254 | static struct timeval d; |
255 | |
256 | if (!t) |
257 | return NULL; |
258 | |
259 | gettimeofday(&d, NULL); |
260 | d.tv_sec = t->tv_sec-d.tv_sec; |
261 | d.tv_usec = t->tv_usec-d.tv_usec; |
262 | |
263 | while (d.tv_usec < 0) { |
264 | d.tv_sec--; |
265 | d.tv_usec += 1000000; |
266 | } |
267 | if (d.tv_sec < 0) |
268 | d.tv_sec = d.tv_usec = 0; |
269 | |
270 | return &d; |
271 | } |
272 | |
273 | |
274 | /* ----- Packet/frame delivery --------------------------------------------- */ |
275 | |
276 | |
277 | static inline int send_size(void) |
278 | { |
279 | return tx_left > MAX_FRAG ? MAX_FRAG : tx_left; |
280 | } |
281 | |
282 | |
283 | static void write_buf(int fd, void *buf, int size) |
284 | { |
285 | ssize_t wrote; |
286 | |
287 | wrote = write(fd, buf, size); |
288 | if (wrote < 0) { |
289 | perror("write"); |
290 | return; |
291 | } |
292 | if (wrote != size) |
293 | fprintf(stderr, "short write: %d < %d\n", (int) wrote, size); |
294 | } |
295 | |
296 | |
297 | static void send_frame(void *buf, int size) |
298 | { |
299 | debug_dirt("->", buf, size); |
300 | write_buf(net, buf, size); |
301 | stats.tx_frm++; |
302 | } |
303 | |
304 | |
305 | static void send_more(void) |
306 | { |
307 | uint8_t *p = tx_pos-1; |
308 | |
309 | if (tx_pos == tx_packet+1) { |
310 | *p = pt_first; |
311 | stats.tx_frst++; |
312 | } else { |
313 | *p = pt_next; |
314 | stats.tx_nxt++; |
315 | } |
316 | *p |= tx_seq ? SEQ : 0; |
317 | send_frame(p, send_size()+1); |
318 | start_timer(&t_ack, T_ACK_MS); |
319 | } |
320 | |
321 | |
322 | static void send_ack(int seq) |
323 | { |
324 | uint8_t ack = pt_ack | (seq ? SEQ : 0); |
325 | |
326 | send_frame(&ack, 1); |
327 | stats.tx_ack++; |
328 | } |
329 | |
330 | |
331 | /* ----- Main events ------------------------------------------------------- */ |
332 | |
333 | |
334 | static void rx_pck(void *buf, int size) |
335 | { |
336 | const uint8_t *p = buf; |
337 | uint8_t ctrl, type, seq; |
338 | |
339 | debug_dirt("-<", buf, size); |
340 | |
341 | stats.rx_frm++; |
342 | if (size < 1) { |
343 | stats.invalid++; |
344 | return; |
345 | } |
346 | |
347 | ctrl = *p; |
348 | type = ctrl & PT_MASK; |
349 | seq = !!(ctrl & SEQ); |
350 | |
351 | switch (type) { |
352 | case pt_first: |
353 | stats.rx_frst++; |
354 | send_ack(seq); |
355 | if (rxing) { |
356 | stop_timer(&t_reass); |
357 | rxing = 0; |
358 | } |
359 | break; |
360 | case pt_next: |
361 | stats.rx_nxt++; |
362 | send_ack(seq); |
363 | if (!rxing) { |
364 | stats.not_rx++; |
365 | return; |
366 | } |
367 | if (seq == rx_seq) { |
368 | stats.rx_seq++; |
369 | return; /* retransmission */ |
370 | } |
371 | break; |
372 | case pt_ack: |
373 | stats.rx_ack++; |
374 | if (!txing) { |
375 | stats.not_tx++; |
376 | return; |
377 | } |
378 | if (seq != tx_seq) { |
379 | stats.ack_seq++; |
380 | return; |
381 | } |
382 | stop_timer(&t_ack); |
383 | tx_pos += send_size(); |
384 | tx_left -= send_size(); |
385 | if (!tx_left) { |
386 | stats.lost--; |
387 | txing = 0; |
388 | return; |
389 | } |
390 | tx_seq = !tx_seq; |
391 | retries = 0; |
392 | send_more(); |
393 | return; |
394 | default: |
395 | stats.invalid++; |
396 | return; |
397 | } |
398 | |
399 | if (!rxing) { |
400 | if (size < 5) { |
401 | stats.garbled++; |
402 | return; |
403 | } |
404 | rx_left = p[3] << 8 | p[4]; |
405 | if (rx_left > MAX_PACKET) { |
406 | stats.garbled++; |
407 | return; |
408 | } |
409 | start_timer(&t_reass, T_REASS_MS); |
410 | rxing = 1; |
411 | rx_pos = rx_packet; |
412 | } |
413 | |
414 | if (rx_left < size-1) { |
415 | stats.garbled++; |
416 | stop_timer(&t_reass); |
417 | rxing = 0; |
418 | return; |
419 | } |
420 | memcpy(rx_pos, buf+1, size-1); |
421 | rx_pos += size-1; |
422 | rx_left -= size-1; |
423 | rx_seq = seq; |
424 | |
425 | if (!rx_left) { |
426 | debug_ip("<-", rx_packet, rx_pos-(void *) rx_packet); |
427 | write_buf(tun, rx_packet, rx_pos-(void *) rx_packet); |
428 | stop_timer(&t_reass); |
429 | rxing = 0; |
430 | stats.rx_pck++; |
431 | } |
432 | } |
433 | |
434 | |
435 | static void tx_pck(void *buf, int size) |
436 | { |
437 | const uint8_t *p = buf; |
438 | |
439 | debug_ip(">-", buf, size); |
440 | assert(!txing); |
441 | txing = 1; |
442 | tx_pos = tx_packet+1; |
443 | tx_left = p[2] << 8 | p[3]; |
444 | assert(tx_left <= MAX_PACKET); |
445 | assert(tx_left == size); |
446 | /* |
447 | * @@@ We could avoid the memcpy by reading directly into "tx_packet" |
448 | */ |
449 | memcpy(tx_pos, buf, size); |
450 | tx_seq = !tx_seq; |
451 | retries = 0; |
452 | send_more(); |
453 | stats.tx_pck++; |
454 | stats.lost++; |
455 | } |
456 | |
457 | |
458 | static void ack_timeout(void) |
459 | { |
460 | debug_timeout("ACK-TO"); |
461 | assert(txing); |
462 | stop_timer(&t_ack); |
463 | if (++retries == MAX_TRIES) { |
464 | txing = 0; |
465 | stats.no_ack++; |
466 | } else { |
467 | send_more(); |
468 | stats.retry++; |
469 | } |
470 | } |
471 | |
472 | |
473 | static void reass_timeout(void) |
474 | { |
475 | debug_timeout("REASS-TO"); |
476 | assert(rxing); |
477 | stop_timer(&t_reass); |
478 | stats.reass++; |
479 | rxing = 0; |
480 | } |
481 | |
482 | |
483 | /* ----- Event dispatcher -------------------------------------------------- */ |
484 | |
485 | |
486 | static void event(void) |
487 | { |
488 | uint8_t buf[MAX_PACKET]; |
489 | const struct timeval *to; |
490 | fd_set rset; |
491 | int res; |
492 | ssize_t got; |
493 | |
494 | FD_ZERO(&rset); |
495 | FD_SET(net, &rset); |
496 | |
497 | /* only accept more work if we're idle */ |
498 | if (!txing && !rxing) |
499 | FD_SET(tun, &rset); |
500 | |
501 | to = next_timer(2, &t_reass, &t_ack); |
502 | |
503 | res = select(net > tun ? net+1 : tun+1, &rset, NULL, NULL, |
504 | timer_delta(to)); |
505 | if (res < 0) { |
506 | if (errno != EINTR) |
507 | perror("select"); |
508 | return; |
509 | } |
510 | if (!res) { |
511 | assert(to); |
512 | if (to == &t_reass) |
513 | reass_timeout(); |
514 | else |
515 | ack_timeout(); |
516 | } |
517 | if (FD_ISSET(tun, &rset)) { |
518 | got = read(tun, buf, sizeof(buf)); |
519 | if (got < 0) { |
520 | perror("read tun"); |
521 | return; |
522 | } |
523 | tx_pck(buf, got); |
524 | } |
525 | if (FD_ISSET(net, &rset)) { |
526 | got = read(net, buf, sizeof(buf)); |
527 | if (got < 0) { |
528 | perror("read net"); |
529 | return; |
530 | } |
531 | rx_pck(buf, got); |
532 | } |
533 | } |
534 | |
535 | |
536 | /* ----- Setup ------------------------------------------------------------- */ |
537 | |
538 | |
539 | static int open_net(uint16_t pan, uint16_t me, uint16_t peer) |
540 | { |
541 | struct sockaddr_ieee802154 addr; |
542 | int zero = 0; |
543 | int s; |
544 | |
545 | s = socket(PF_IEEE802154, SOCK_DGRAM, 0); |
546 | if (s < 0) { |
547 | perror("socket 802.15.4"); |
548 | exit(1); |
549 | } |
550 | |
551 | addr.family = AF_IEEE802154; |
552 | addr.addr.addr_type = IEEE802154_ADDR_SHORT; |
553 | addr.addr.pan_id = pan; |
554 | addr.addr.short_addr = me; |
555 | |
556 | if (bind(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) { |
557 | perror("bind 802.15.4"); |
558 | exit(1); |
559 | } |
560 | |
561 | addr.addr.short_addr = peer; |
562 | if (connect(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) { |
563 | perror("connect 802.15.4"); |
564 | exit(1); |
565 | } |
566 | |
567 | if (setsockopt(s, SOL_IEEE802154, WPAN_WANTACK, &zero, sizeof(zero)) |
568 | < 0) { |
569 | perror("setsockopt SOL_IEEE802154 WPAN_WANTACK"); |
570 | exit(1); |
571 | } |
572 | |
573 | return s; |
574 | } |
575 | |
576 | |
577 | static int open_tun(const char *cmd) |
578 | { |
579 | struct ifreq ifr; |
580 | int fd, res; |
581 | |
582 | fd = open(TUN_DEV, O_RDWR); |
583 | if (fd < 0) { |
584 | perror(TUN_DEV); |
585 | exit(1); |
586 | } |
587 | |
588 | memset(&ifr, 0, sizeof(ifr)); |
589 | ifr.ifr_flags = IFF_TUN | IFF_NO_PI; |
590 | |
591 | if (ioctl(fd, TUNSETIFF, (void *) &ifr) < 0) { |
592 | perror("ioctl TUNSETIFF"); |
593 | exit(1); |
594 | } |
595 | |
596 | if (!cmd) { |
597 | fprintf(stderr, "%s\n", ifr.ifr_name); |
598 | return fd; |
599 | } |
600 | |
601 | if (setenv("ITF", ifr.ifr_name, 1) < 0) { |
602 | perror("setenv"); |
603 | exit(1); |
604 | } |
605 | |
606 | res = system(cmd); |
607 | if (res < 0) { |
608 | perror("system"); |
609 | exit(1); |
610 | } |
611 | if (WIFEXITED(res)) { |
612 | if (!WEXITSTATUS(res)) |
613 | return fd; |
614 | exit(WEXITSTATUS(res)); |
615 | } |
616 | if (WIFSIGNALED(res)) { |
617 | raise(WTERMSIG(res)); |
618 | exit(1); |
619 | } |
620 | |
621 | fprintf(stderr, "cryptic exit status %d\n", res); |
622 | exit(1); |
623 | } |
624 | |
625 | |
626 | /* ----- Command-line processing ------------------------------------------- */ |
627 | |
628 | |
629 | static void usage(const char *name) |
630 | { |
631 | fprintf(stderr, |
632 | "usage: %s [-b] [-d [-d]] pan_id src_addr dst_addr [command]\n\n" |
633 | " pan_id PAN (network) identifier\n" |
634 | " src_addr source short address\n" |
635 | " dst_addr destination short address\n" |
636 | " command configuration command to run after creating the TUN interface.\n" |
637 | " The environment variable $ITF is set to the interface name.\n\n" |
638 | " -b background the process after initialization\n" |
639 | " -d ... increase verbosity of debug output\n" |
640 | , name); |
641 | exit(1); |
642 | } |
643 | |
644 | |
645 | static uint16_t addr(const char *name, const char *s) |
646 | { |
647 | char *end; |
648 | unsigned long n; |
649 | |
650 | n = strtoul(s, &end, 16); |
651 | if (*end) |
652 | usage(name); |
653 | if (n > 0xffff) |
654 | usage(name); |
655 | return n; |
656 | } |
657 | |
658 | |
659 | int main(int argc, char **argv) |
660 | { |
661 | const char *cmd = NULL; |
662 | uint16_t pan, src, dst; |
663 | int foreground = 1; |
664 | int c; |
665 | |
666 | while ((c = getopt(argc, argv, "bd")) != EOF) |
667 | switch (c) { |
668 | case 'b': |
669 | foreground = 0; |
670 | break; |
671 | case 'd': |
672 | debug++; |
673 | break; |
674 | default: |
675 | usage(*argv); |
676 | } |
677 | |
678 | switch (argc-optind) { |
679 | case 4: |
680 | cmd = argv[optind+3]; |
681 | /* fall through */ |
682 | case 3: |
683 | pan = addr(*argv, argv[optind]); |
684 | src = addr(*argv, argv[optind+1]); |
685 | dst = addr(*argv, argv[optind+2]); |
686 | break; |
687 | default: |
688 | usage(*argv); |
689 | } |
690 | |
691 | net = open_net(pan, src, dst); |
692 | tun = open_tun(cmd); |
693 | |
694 | |
695 | if (foreground) { |
696 | signal(SIGUSR1, handle_usr1); |
697 | signal(SIGUSR2, handle_usr2); |
698 | } else { |
699 | if (daemonize()) |
700 | return 0; |
701 | } |
702 | |
703 | while (1) |
704 | event(); |
705 | } |
706 |