IEEE 802.15.4 subsystem
Sign in or create your account | Project List | Help
IEEE 802.15.4 subsystem Git Source Tree
Root/
Source at commit a800eb8794253e0a3d655b1ae8bd5654d72d6e63 created 12 years 9 months ago. By Werner Almesberger, tools/dirtpan/dirtpan.c: added missing #include "daemon.h", oops | |
---|---|
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 <assert.h> |
23 | #include <sys/types.h> |
24 | #include <sys/time.h> |
25 | #include <sys/wait.h> |
26 | #include <sys/select.h> |
27 | #include <sys/ioctl.h> |
28 | #include <sys/socket.h> |
29 | #include <linux/if.h> |
30 | #include <linux/if_tun.h> |
31 | |
32 | #include <ieee802154.h> |
33 | |
34 | #include "daemon.h" |
35 | |
36 | |
37 | /* |
38 | * Control byte structure: |
39 | * |
40 | * +--7--+--6--+--5--+--4--+--3--+--2--+--1--+ |
41 | * | 0 | 0 | 0 | 0 | seq | pck_type | |
42 | * +-----+-----+-----+-----+-----+-----+-----+ |
43 | */ |
44 | |
45 | #define SEQ 4 |
46 | #define PT_MASK 3 |
47 | |
48 | enum packet_type { |
49 | pt_first = 0, |
50 | pt_next = 1, |
51 | pt_ack = 2, |
52 | }; |
53 | |
54 | |
55 | #define TUN_DEV "/dev/net/tun" |
56 | |
57 | #define MAX_FRAG (127-11-2-1) /* MHDR, FCS, control byte */ |
58 | #define MAX_PACKET 2000 |
59 | #define MAX_TRIES 5 |
60 | #define T_REASS_MS 200 |
61 | #define T_ACK_MS 50 |
62 | |
63 | |
64 | static int tun, net; |
65 | static uint8_t rx_packet[MAX_PACKET], tx_packet[MAX_PACKET+1]; |
66 | static void *rx_pos, *tx_pos; |
67 | static int rx_left, tx_left; |
68 | static int txing = 0, rxing = 0; |
69 | static int rx_seq, tx_seq = 0; |
70 | static int retries; |
71 | static int debug = 0; |
72 | |
73 | |
74 | /* ----- Debugging --------------------------------------------------------- */ |
75 | |
76 | |
77 | static void debug_label(const char *label) |
78 | { |
79 | fprintf(stderr, "%s(%c%c)", |
80 | label, txing ? 'T' : '-', rxing ? 'R' : '-'); |
81 | } |
82 | |
83 | |
84 | static void dump(const void *buf, int size) |
85 | { |
86 | const uint8_t *p = buf; |
87 | |
88 | while (size--) { |
89 | fprintf(stderr, "%s%02x", p == buf ? "" : " ", *p); |
90 | p++; |
91 | } |
92 | } |
93 | |
94 | |
95 | static void debug_ip(const char *label, void *buf, int size) |
96 | { |
97 | if (debug < 2) |
98 | return; |
99 | debug_label(label); |
100 | fprintf(stderr, ", %d: ", size); |
101 | dump(buf, size); |
102 | fprintf(stderr, "\n"); |
103 | } |
104 | |
105 | |
106 | static void debug_dirt(const char *label, void *buf, int size) |
107 | { |
108 | const uint8_t *p = buf; |
109 | |
110 | if (!debug) |
111 | return; |
112 | if (debug == 1) { |
113 | if (size) { |
114 | fprintf(stderr, "%c%d", |
115 | (label[1] == '>' ? "FNA?" : "fna?")[*p & PT_MASK], |
116 | *p & SEQ ? 0 : 1); |
117 | } |
118 | return; |
119 | } |
120 | debug_label(label); |
121 | fprintf(stderr, ", %d+1: ", size-1); |
122 | if (size) { |
123 | fprintf(stderr, "%02x(%c%d) | ", |
124 | *p, "FNA?"[*p & PT_MASK], *p & SEQ ? 0 : 1); |
125 | dump(buf+1, size-1); |
126 | } |
127 | fprintf(stderr, "\n"); |
128 | |
129 | } |
130 | |
131 | |
132 | static void debug_timeout(const char *label) |
133 | { |
134 | if (!debug) |
135 | return; |
136 | if (debug == 1) { |
137 | fprintf(stderr, "*%c", *label); |
138 | return; |
139 | } |
140 | debug_label(label); |
141 | fprintf(stderr, "\n"); |
142 | } |
143 | |
144 | |
145 | /* ----- Timers ------------------------------------------------------------ */ |
146 | |
147 | |
148 | static struct timeval t_reass, t_ack; |
149 | |
150 | |
151 | static void start_timer(struct timeval *t, int ms) |
152 | { |
153 | assert(!t->tv_sec && !t->tv_usec); |
154 | gettimeofday(t, NULL); |
155 | t->tv_usec += 1000*ms; |
156 | while (t->tv_usec >= 1000000) { |
157 | t->tv_sec++; |
158 | t->tv_usec -= 1000000; |
159 | } |
160 | } |
161 | |
162 | |
163 | static void stop_timer(struct timeval *t) |
164 | { |
165 | assert(t->tv_sec || t->tv_usec); |
166 | t->tv_sec = 0; |
167 | t->tv_usec = 0; |
168 | } |
169 | |
170 | |
171 | static const struct timeval *next_timer(int n, ...) |
172 | { |
173 | va_list ap; |
174 | const struct timeval *next = NULL; |
175 | const struct timeval *t; |
176 | |
177 | va_start(ap, n); |
178 | while (n--) { |
179 | t = va_arg(ap, const struct timeval *); |
180 | if (!t->tv_sec && !t->tv_usec) |
181 | continue; |
182 | if (next) { |
183 | if (next->tv_sec < t->tv_sec) |
184 | continue; |
185 | if (next->tv_sec == t->tv_sec && |
186 | next->tv_usec < t->tv_usec) |
187 | continue; |
188 | } |
189 | next = t; |
190 | } |
191 | va_end(ap); |
192 | return next; |
193 | } |
194 | |
195 | |
196 | static struct timeval *timer_delta(const struct timeval *t) |
197 | { |
198 | static struct timeval d; |
199 | |
200 | if (!t) |
201 | return NULL; |
202 | |
203 | gettimeofday(&d, NULL); |
204 | d.tv_sec = t->tv_sec-d.tv_sec; |
205 | d.tv_usec = t->tv_usec-d.tv_usec; |
206 | |
207 | while (d.tv_usec < 0) { |
208 | d.tv_sec--; |
209 | d.tv_usec += 1000000; |
210 | } |
211 | if (d.tv_sec < 0) |
212 | d.tv_sec = d.tv_usec = 0; |
213 | |
214 | return &d; |
215 | } |
216 | |
217 | |
218 | /* ----- Packet/frame delivery --------------------------------------------- */ |
219 | |
220 | |
221 | static inline int send_size(void) |
222 | { |
223 | return tx_left > MAX_FRAG ? MAX_FRAG : tx_left; |
224 | } |
225 | |
226 | |
227 | static void write_buf(int fd, void *buf, int size) |
228 | { |
229 | ssize_t wrote; |
230 | |
231 | wrote = write(fd, buf, size); |
232 | if (wrote < 0) { |
233 | perror("write"); |
234 | return; |
235 | } |
236 | if (wrote != size) |
237 | fprintf(stderr, "short write: %d < %d\n", (int) wrote, size); |
238 | } |
239 | |
240 | |
241 | static void send_frame(void *buf, int size) |
242 | { |
243 | debug_dirt("->", buf, size); |
244 | write_buf(net, buf, size); |
245 | } |
246 | |
247 | |
248 | static void send_more(void) |
249 | { |
250 | uint8_t *p = tx_pos-1; |
251 | |
252 | *p = (tx_pos == tx_packet+1 ? pt_first : pt_next) | (tx_seq ? SEQ : 0); |
253 | send_frame(p, send_size()+1); |
254 | start_timer(&t_ack, T_ACK_MS); |
255 | } |
256 | |
257 | |
258 | static void send_ack(int seq) |
259 | { |
260 | uint8_t ack = pt_ack | (seq ? SEQ : 0); |
261 | |
262 | send_frame(&ack, 1); |
263 | } |
264 | |
265 | |
266 | /* ----- Main events ------------------------------------------------------- */ |
267 | |
268 | |
269 | static void rx_pck(void *buf, int size) |
270 | { |
271 | const uint8_t *p = buf; |
272 | uint8_t ctrl, type, seq; |
273 | |
274 | debug_dirt("-<", buf, size); |
275 | |
276 | if (size < 1) |
277 | return; |
278 | |
279 | ctrl = *p; |
280 | type = ctrl & PT_MASK; |
281 | seq = !!(ctrl & SEQ); |
282 | |
283 | switch (type) { |
284 | case pt_first: |
285 | send_ack(seq); |
286 | if (rxing) { |
287 | stop_timer(&t_reass); |
288 | rxing = 0; |
289 | } |
290 | break; |
291 | case pt_next: |
292 | send_ack(seq); |
293 | if (!rxing) |
294 | return; |
295 | if (seq == rx_seq) |
296 | return; /* retransmission */ |
297 | break; |
298 | case pt_ack: |
299 | if (!txing) |
300 | return; |
301 | if (seq != tx_seq) |
302 | return; |
303 | stop_timer(&t_ack); |
304 | tx_pos += send_size(); |
305 | tx_left -= send_size(); |
306 | if (!tx_left) { |
307 | txing = 0; |
308 | return; |
309 | } |
310 | tx_seq = !tx_seq; |
311 | retries = 0; |
312 | send_more(); |
313 | return; |
314 | default: |
315 | abort(); |
316 | } |
317 | |
318 | if (!rxing) { |
319 | if (size < 5) |
320 | return; |
321 | rx_left = p[3] << 8 | p[4]; |
322 | if (rx_left > MAX_PACKET) |
323 | return; |
324 | start_timer(&t_reass, T_REASS_MS); |
325 | rxing = 1; |
326 | rx_pos = rx_packet; |
327 | } |
328 | |
329 | if (rx_left < size-1) { |
330 | stop_timer(&t_reass); |
331 | rxing = 0; |
332 | return; |
333 | } |
334 | memcpy(rx_pos, buf+1, size-1); |
335 | rx_pos += size-1; |
336 | rx_left -= size-1; |
337 | rx_seq = seq; |
338 | |
339 | if (!rx_left) { |
340 | debug_ip("<-", rx_packet, rx_pos-(void *) rx_packet); |
341 | write_buf(tun, rx_packet, rx_pos-(void *) rx_packet); |
342 | stop_timer(&t_reass); |
343 | rxing = 0; |
344 | } |
345 | } |
346 | |
347 | |
348 | static void tx_pck(void *buf, int size) |
349 | { |
350 | const uint8_t *p = buf; |
351 | |
352 | debug_ip(">-", buf, size); |
353 | assert(!txing); |
354 | txing = 1; |
355 | tx_pos = tx_packet+1; |
356 | tx_left = p[2] << 8 | p[3]; |
357 | assert(tx_left <= MAX_PACKET); |
358 | assert(tx_left == size); |
359 | /* |
360 | * @@@ We could avoid the memcpy by reading directly into "tx_packet" |
361 | */ |
362 | memcpy(tx_pos, buf, size); |
363 | tx_seq = !tx_seq; |
364 | retries = 0; |
365 | send_more(); |
366 | } |
367 | |
368 | |
369 | static void ack_timeout(void) |
370 | { |
371 | debug_timeout("ACK-TO"); |
372 | assert(txing); |
373 | stop_timer(&t_ack); |
374 | if (++retries == MAX_TRIES) |
375 | txing = 0; |
376 | else |
377 | send_more(); |
378 | } |
379 | |
380 | |
381 | static void reass_timeout(void) |
382 | { |
383 | debug_timeout("REASS-TO"); |
384 | assert(rxing); |
385 | stop_timer(&t_reass); |
386 | rxing = 0; |
387 | } |
388 | |
389 | |
390 | /* ----- Event dispatcher -------------------------------------------------- */ |
391 | |
392 | |
393 | static void event(void) |
394 | { |
395 | uint8_t buf[MAX_PACKET]; |
396 | const struct timeval *to; |
397 | fd_set rset; |
398 | int res; |
399 | ssize_t got; |
400 | |
401 | FD_ZERO(&rset); |
402 | FD_SET(net, &rset); |
403 | |
404 | /* only accept more work if we're idle */ |
405 | if (!txing && !rxing) |
406 | FD_SET(tun, &rset); |
407 | |
408 | to = next_timer(2, &t_reass, &t_ack); |
409 | |
410 | res = select(net > tun ? net+1 : tun+1, &rset, NULL, NULL, |
411 | timer_delta(to)); |
412 | if (res < 0) { |
413 | perror("select"); |
414 | return; |
415 | } |
416 | if (!res) { |
417 | assert(to); |
418 | if (to == &t_reass) |
419 | reass_timeout(); |
420 | else |
421 | ack_timeout(); |
422 | } |
423 | if (FD_ISSET(tun, &rset)) { |
424 | got = read(tun, buf, sizeof(buf)); |
425 | if (got < 0) { |
426 | perror("read tun"); |
427 | return; |
428 | } |
429 | tx_pck(buf, got); |
430 | } |
431 | if (FD_ISSET(net, &rset)) { |
432 | got = read(net, buf, sizeof(buf)); |
433 | if (got < 0) { |
434 | perror("read net"); |
435 | return; |
436 | } |
437 | rx_pck(buf, got); |
438 | } |
439 | } |
440 | |
441 | |
442 | /* ----- Setup ------------------------------------------------------------- */ |
443 | |
444 | |
445 | static int open_net(uint16_t pan, uint16_t me, uint16_t peer) |
446 | { |
447 | struct sockaddr_ieee802154 addr; |
448 | int zero = 0; |
449 | int s; |
450 | |
451 | s = socket(PF_IEEE802154, SOCK_DGRAM, 0); |
452 | if (s < 0) { |
453 | perror("socket 802.15.4"); |
454 | exit(1); |
455 | } |
456 | |
457 | addr.family = AF_IEEE802154; |
458 | addr.addr.addr_type = IEEE802154_ADDR_SHORT; |
459 | addr.addr.pan_id = pan; |
460 | addr.addr.short_addr = me; |
461 | |
462 | if (bind(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) { |
463 | perror("bind 802.15.4"); |
464 | exit(1); |
465 | } |
466 | |
467 | addr.addr.short_addr = peer; |
468 | if (connect(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) { |
469 | perror("connect 802.15.4"); |
470 | exit(1); |
471 | } |
472 | |
473 | if (setsockopt(s, SOL_IEEE802154, WPAN_WANTACK, &zero, sizeof(zero)) |
474 | < 0) { |
475 | perror("setsockopt SOL_IEEE802154 WPAN_WANTACK"); |
476 | exit(1); |
477 | } |
478 | |
479 | return s; |
480 | } |
481 | |
482 | |
483 | static int open_tun(const char *cmd) |
484 | { |
485 | struct ifreq ifr; |
486 | int fd, res; |
487 | |
488 | fd = open(TUN_DEV, O_RDWR); |
489 | if (fd < 0) { |
490 | perror(TUN_DEV); |
491 | exit(1); |
492 | } |
493 | |
494 | memset(&ifr, 0, sizeof(ifr)); |
495 | ifr.ifr_flags = IFF_TUN | IFF_NO_PI; |
496 | |
497 | if (ioctl(fd, TUNSETIFF, (void *) &ifr) < 0) { |
498 | perror("ioctl TUNSETIFF"); |
499 | exit(1); |
500 | } |
501 | |
502 | if (!cmd) { |
503 | fprintf(stderr, "%s\n", ifr.ifr_name); |
504 | return fd; |
505 | } |
506 | |
507 | if (setenv("ITF", ifr.ifr_name, 1) < 0) { |
508 | perror("setenv"); |
509 | exit(1); |
510 | } |
511 | |
512 | res = system(cmd); |
513 | if (res < 0) { |
514 | perror("system"); |
515 | exit(1); |
516 | } |
517 | if (WIFEXITED(res)) { |
518 | if (!WEXITSTATUS(res)) |
519 | return fd; |
520 | exit(WEXITSTATUS(res)); |
521 | } |
522 | if (WIFSIGNALED(res)) { |
523 | raise(WTERMSIG(res)); |
524 | exit(1); |
525 | } |
526 | |
527 | fprintf(stderr, "cryptic exit status %d\n", res); |
528 | exit(1); |
529 | } |
530 | |
531 | |
532 | /* ----- Command-line processing ------------------------------------------- */ |
533 | |
534 | |
535 | static void usage(const char *name) |
536 | { |
537 | fprintf(stderr, |
538 | "usage: %s [-b] [-d [-d]] pan_id src_addr dst_addr [command]\n\n" |
539 | " pan_id PAN (network) identifier\n" |
540 | " src_addr source short address\n" |
541 | " dst_addr destination short address\n" |
542 | " command configuration command to run after creating the TUN interface.\n" |
543 | " The environment variable $ITF is set to the interface name.\n\n" |
544 | " -b background the process after initialization\n" |
545 | " -d ... increase verbosity of debug output\n" |
546 | , name); |
547 | exit(1); |
548 | } |
549 | |
550 | |
551 | static uint16_t addr(const char *name, const char *s) |
552 | { |
553 | char *end; |
554 | unsigned long n; |
555 | |
556 | n = strtoul(s, &end, 16); |
557 | if (*end) |
558 | usage(name); |
559 | if (n > 0xffff) |
560 | usage(name); |
561 | return n; |
562 | } |
563 | |
564 | |
565 | int main(int argc, char **argv) |
566 | { |
567 | const char *cmd = NULL; |
568 | uint16_t pan, src, dst; |
569 | int foreground = 1; |
570 | int c; |
571 | |
572 | while ((c = getopt(argc, argv, "bd")) != EOF) |
573 | switch (c) { |
574 | case 'b': |
575 | foreground = 0; |
576 | break; |
577 | case 'd': |
578 | debug++; |
579 | break; |
580 | default: |
581 | usage(*argv); |
582 | } |
583 | |
584 | switch (argc-optind) { |
585 | case 4: |
586 | cmd = argv[optind+3]; |
587 | /* fall through */ |
588 | case 3: |
589 | pan = addr(*argv, argv[optind]); |
590 | src = addr(*argv, argv[optind+1]); |
591 | dst = addr(*argv, argv[optind+2]); |
592 | break; |
593 | default: |
594 | usage(*argv); |
595 | } |
596 | |
597 | net = open_net(pan, src, dst); |
598 | tun = open_tun(cmd); |
599 | |
600 | if (foreground || !daemonize()) |
601 | while (1) |
602 | event(); |
603 | |
604 | return 0; |
605 | } |
606 |