1 /*
2 * Copyright (c) 2015 Intel Corporation.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #define EXTRA_DEBUG 0
18
19 #include <stdio.h>
20 #include <stdlib.h>
21 #include <stdbool.h>
22 #include <stdarg.h>
23 #include <syslog.h>
24 #include <errno.h>
25 #include <unistd.h>
26 #include <string.h>
27 #include <fcntl.h>
28 #include <signal.h>
29 #include <sys/uio.h>
30 #include <sys/signalfd.h>
31 #include <sys/time.h>
32 #include <linux/if_ether.h>
33 #include <glib.h>
34 #include <pcap/pcap.h>
35
36 #define PIPE_IN ".in"
37 #define PIPE_OUT ".out"
38 #define DUMMY_RADIO_15_4_FRAME_TYPE 0xF0
39
40 static uint8_t input1[128];
41 static uint8_t input1_len, input1_offset, input1_type;
42
43 static uint8_t input2[128];
44 static uint8_t input2_len, input2_offset, input2_type;
45
46 static GMainLoop *main_loop = NULL;
47 static char *path = NULL;
48 static char *pipe_1_in = NULL, *pipe_1_out = NULL;
49 static char *pipe_2_in = NULL, *pipe_2_out = NULL;
50 static struct pcap_fd *pcap;
51 static int fd1_in = -1, fd2_in = -1;
52
53 struct pcap_fd {
54 int fd;
55 };
56
57 #define PCAP_FILE_HDR_SIZE (sizeof(struct pcap_file_header))
58
59 struct pcap_frame {
60 uint32_t ts_sec;
61 uint32_t ts_usec;
62 uint32_t caplen;
63 uint32_t len;
64 } __attribute__ ((packed));
65 #define PCAP_FRAME_SIZE (sizeof(struct pcap_frame))
66
67 struct debug_desc {
68 const char *name;
69 const char *file;
70 #define DEBUG_FLAG_DEFAULT (0)
71 #define DEBUG_FLAG_PRINT (1 << 0)
72 unsigned int flags;
73 } __attribute__((aligned(8)));
74
debug(const char * format,...)75 void debug(const char *format, ...)
76 {
77 va_list ap;
78
79 va_start(ap, format);
80
81 vsyslog(LOG_DEBUG, format, ap);
82
83 va_end(ap);
84 }
85
86 #define DBG(fmt, arg...) do { \
87 debug("%s:%s() " fmt, __FILE__, __FUNCTION__ , ## arg); \
88 } while (0)
89
log_init(const char * debug,gboolean detach,gboolean perror)90 int log_init(const char *debug, gboolean detach, gboolean perror)
91 {
92 int option = LOG_NDELAY | LOG_PID;
93 const char *name = NULL, *file = NULL;
94
95 if (perror)
96 option |= LOG_PERROR;
97
98 openlog("monitor", option, LOG_DAEMON);
99
100 syslog(LOG_INFO, "monitor 15.4");
101
102 return 0;
103 }
104
log_cleanup(void)105 void log_cleanup(void)
106 {
107 syslog(LOG_INFO, "Exit");
108
109 closelog();
110 }
111
monitor_pcap_free(void)112 void monitor_pcap_free(void)
113 {
114 if (!pcap)
115 return;
116
117 if (pcap->fd >= 0)
118 close(pcap->fd);
119
120 g_free(pcap);
121 }
122
monitor_pcap_create(const char * pathname)123 bool monitor_pcap_create(const char *pathname)
124 {
125 struct pcap_file_header hdr;
126 ssize_t len;
127
128 pcap = g_new0(struct pcap_fd, 1);
129 pcap->fd = open(pathname, O_WRONLY | O_CREAT | O_TRUNC | O_CLOEXEC,
130 S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
131 if (pcap->fd < 0) {
132 DBG("Failed to create PCAP file");
133 g_free(pcap);
134 return false;
135 }
136
137 memset(&hdr, 0, PCAP_FILE_HDR_SIZE);
138 hdr.magic = 0xa1b2c3d4;
139 hdr.version_major = 0x0002;
140 hdr.version_minor = 0x0004;
141 hdr.thiszone = 0;
142 hdr.sigfigs = 0;
143 hdr.snaplen = 0x0000ffff;
144 /*
145 * http://www.tcpdump.org/linktypes.html
146 * LINKTYPE_IEEE802_15_4_NOFCS : 230
147 */
148 hdr.linktype = 0x000000E6;
149
150 len = write(pcap->fd, &hdr, PCAP_FILE_HDR_SIZE);
151 if (len < 0) {
152 DBG("Failed to write PCAP header");
153 goto failed;
154 }
155
156 if (len != PCAP_FILE_HDR_SIZE) {
157 DBG("Written PCAP header size mimatch\n");
158 goto failed;
159 }
160
161 return true;
162
163 failed:
164 monitor_pcap_free();
165
166 return false;
167 }
168
monitor_pcap_write(const void * data,uint32_t size)169 bool monitor_pcap_write(const void *data, uint32_t size)
170 {
171 struct pcap_frame frame;
172 struct timeval tv;
173 ssize_t len;
174
175 if (!pcap)
176 return false;
177
178 memset(&frame, 0, PCAP_FRAME_SIZE);
179
180 gettimeofday(&tv, NULL);
181 frame.ts_sec = tv.tv_sec;
182 frame.ts_usec = tv.tv_usec;
183 frame.caplen = size;
184 frame.len = size;
185
186 len = write(pcap->fd, &frame, PCAP_FRAME_SIZE);
187 if (len < 0 || len != PCAP_FRAME_SIZE) {
188 DBG("Failed to write PCAP frame or size mismatch %d\n", len);
189 return false;
190 }
191
192 fsync(pcap->fd);
193
194 len = write(pcap->fd, data, size);
195 if (len < 0 || len != size) {
196 DBG("Failed to write PCAP data or size mismatch %d\n", len);
197 return false;
198 }
199
200 fsync(pcap->fd);
201 return true;
202 }
203
fifo_handler1(GIOChannel * channel,GIOCondition cond,gpointer user_data)204 static gboolean fifo_handler1(GIOChannel *channel, GIOCondition cond,
205 gpointer user_data)
206 {
207 static bool starting = true;
208 unsigned char buf[1];
209 ssize_t result;
210 int fd;
211
212 if (cond & (G_IO_NVAL | G_IO_ERR | G_IO_HUP)) {
213 DBG("First pipe closed");
214 return FALSE;
215 }
216
217 memset(buf, 0, sizeof(buf));
218 fd = g_io_channel_unix_get_fd(channel);
219
220 result = read(fd, buf, 1);
221 if (result != 1) {
222 DBG("Failed to read %lu", result);
223 return FALSE;
224 }
225
226 if (write(fd2_in, buf, 1) < 0) {
227 DBG("Error, write failed to %s", pipe_2_in);
228 return FALSE;
229 }
230 #if EXTRA_DEBUG
231 DBG("[%d/%d] starting %d fifo 1 read from %d and wrote %d bytes to %d",
232 input1_offset, input1_len, starting, fd, 1, fd2_in);
233 #endif
234
235 if (starting && input1_len == 0 && input1_offset == 0 &&
236 buf[0] == 0 && input1_type == 0) {
237 /* sync byte */
238 goto done;
239 } else if (starting && input1_len == 0 && input1_offset == 0
240 && buf[0] != 0) {
241 starting = false;
242 }
243
244 #if EXTRA_DEBUG
245 DBG("Pipe 1 byte 0x%x", buf[0]);
246 #endif
247 if (input1_len == 0 && input1_offset == 0 &&
248 buf[0] == DUMMY_RADIO_15_4_FRAME_TYPE) {
249 input1_type = buf[0];
250 #if EXTRA_DEBUG
251 DBG("Frame starting in pipe 1");
252 #endif
253 goto done;
254 }
255
256 if (input1_len == 0 && input1_offset == 0 &&
257 input1_type == DUMMY_RADIO_15_4_FRAME_TYPE) {
258 input1_len = buf[0];
259 #if EXTRA_DEBUG
260 DBG("Expecting pipe 1 buf len %d\n", input1_len);
261 #endif
262 goto done;
263 }
264
265 if (input1_len) {
266 input1[input1_offset++] = buf[0];
267 }
268
269 if (input1_len && input1_len == input1_offset) {
270 DBG("Received %d bytes in pipe 1", input1_len);
271
272 monitor_pcap_write(input1, input1_len);
273 input1_len = input1_offset = 0;
274 memset(input1, 0, sizeof(input1));
275
276 fsync(fd2_in);
277 }
278
279 done:
280 return TRUE;
281 }
282
fifo_handler2(GIOChannel * channel,GIOCondition cond,gpointer user_data)283 static gboolean fifo_handler2(GIOChannel *channel, GIOCondition cond,
284 gpointer user_data)
285 {
286 static bool starting = true;
287 unsigned char buf[1];
288 ssize_t result;
289 int fd;
290
291 if (cond & (G_IO_NVAL | G_IO_ERR | G_IO_HUP)) {
292 DBG("Second pipe closed");
293 return FALSE;
294 }
295
296 memset(buf, 0, sizeof(buf));
297 fd = g_io_channel_unix_get_fd(channel);
298
299 result = read(fd, buf, 1);
300 if (result != 1) {
301 DBG("Failed to read %lu", result);
302 return FALSE;
303 }
304
305 if (write(fd1_in, buf, 1) < 0) {
306 DBG("Error, write failed to %s", pipe_1_in);
307 return FALSE;
308 }
309 #if EXTRA_DEBUG
310 DBG("[%d/%d] starting %d fifo 2 read from %d and wrote %d bytes to %d",
311 input2_offset, input2_len, starting, fd, 1, fd1_in);
312 #endif
313 if (starting && input2_len == 0 && input2_offset == 0 &&
314 buf[0] == 0 && input2_type == 0) {
315 /* sync byte */
316 goto done;
317 } else if (starting && input2_len == 0 && input2_offset == 0
318 && buf[0] != 0) {
319 starting = false;
320 }
321
322 #if EXTRA_DEBUG
323 DBG("Pipe 2 byte 0x%x", buf[0]);
324 #endif
325 if (input2_len == 0 && input2_offset == 0 &&
326 buf[0] == DUMMY_RADIO_15_4_FRAME_TYPE) {
327 #if EXTRA_DEBUG
328 DBG("Frame starting in pipe 2");
329 #endif
330 input2_type = buf[0];
331 goto done;
332 }
333
334 if (input2_len == 0 && input2_offset == 0 &&
335 input2_type == DUMMY_RADIO_15_4_FRAME_TYPE) {
336 input2_len = buf[0];
337 #if EXTRA_DEBUG
338 DBG("Expecting pipe 2 buf len %d\n", input2_len);
339 #endif
340 goto done;
341 }
342
343 if (input2_len) {
344 input2[input2_offset++] = buf[0];
345 }
346
347 if (input2_len && input2_len == input2_offset) {
348 DBG("Received %d bytes in pipe 2", input2_len);
349
350 monitor_pcap_write(input2, input2_len);
351 input2_len = input2_offset = 0;
352 memset(input2, 0, sizeof(input2));
353
354 fsync(fd1_in);
355 }
356
357 done:
358 return TRUE;
359 }
360
setup_fifofd1(void)361 static int setup_fifofd1(void)
362 {
363 GIOChannel *channel;
364 guint source;
365 int fd;
366
367 fd = open(pipe_1_out, O_RDONLY);
368 if (fd < 0) {
369 DBG("Failed to open fifo %s", pipe_1_out);
370 return fd;
371 }
372
373 DBG("Pipe 1 OUT fd %d", fd);
374
375 channel = g_io_channel_unix_new(fd);
376
377 g_io_channel_set_close_on_unref(channel, TRUE);
378
379 source = g_io_add_watch(channel,
380 G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
381 fifo_handler1, NULL);
382
383 g_io_channel_unref(channel);
384
385 return source;
386 }
387
setup_fifofd2(void)388 static int setup_fifofd2(void)
389 {
390 GIOChannel *channel;
391 guint source;
392 int fd;
393
394 fd = open(pipe_2_out, O_RDONLY);
395 if (fd < 0) {
396 DBG("Failed to open fifo %s", pipe_2_out);
397 return fd;
398 }
399
400 DBG("Pipe 2 OUT fd %d", fd);
401
402 channel = g_io_channel_unix_new(fd);
403
404 g_io_channel_set_close_on_unref(channel, TRUE);
405
406 source = g_io_add_watch(channel,
407 G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
408 fifo_handler2, NULL);
409
410 g_io_channel_unref(channel);
411
412 return source;
413 }
414
main(int argc,char * argv[])415 int main(int argc, char *argv[])
416 {
417 int fifo1 = -1, fifo2 = -1, ret;
418 char *pipe1 = "/tmp/ip-15-4-1";
419 char *pipe2 = "/tmp/ip-15-4-2";
420
421 if (argc < 2) {
422 printf("Usage: %s <pcapfile> [<pipe_1> <pipe_2>]\n", argv[0]);
423 printf(" e.g.: monitor_15_4 sample.pcap [/tmp/ip-15-4-1 /tmp/ip-15-4-2]\n");
424 exit(-EINVAL);
425 }
426
427 if (argc == 3)
428 pipe1 = argv[2];
429
430 if (argc == 4) {
431 pipe1 = argv[2];
432 pipe2 = argv[3];
433 }
434
435 main_loop = g_main_loop_new(NULL, FALSE);
436
437 pipe_1_in = g_strconcat(pipe1, PIPE_IN, NULL);
438 pipe_1_out = g_strconcat(pipe1, PIPE_OUT, NULL);
439 pipe_2_in = g_strconcat(pipe2, PIPE_IN, NULL);
440 pipe_2_out = g_strconcat(pipe2, PIPE_OUT, NULL);
441 path = g_strdup(argv[1]);
442
443 log_init("log", FALSE, argc > 4 ? TRUE : FALSE);
444
445 DBG("Pipe 1 IN %s OUT %s", pipe_1_in, pipe_1_out);
446 DBG("Pipe 2 IN %s OUT %s", pipe_2_in, pipe_2_out);
447
448 if (!monitor_pcap_create(path)) {
449 g_free(path);
450 exit(-EINVAL);
451 }
452
453 fifo1 = setup_fifofd1();
454 if (fifo1 < 0) {
455 ret = -EINVAL;
456 goto exit;
457 }
458
459 fifo2 = setup_fifofd2();
460 if (fifo2 < 0) {
461 ret = -EINVAL;
462 goto exit;
463 }
464
465 fd1_in = open(pipe_1_in, O_WRONLY);
466 if (fd1_in < 0) {
467 DBG("Failed to open fifo %s", pipe_1_in);
468 ret = -EINVAL;
469 goto exit;
470 }
471
472 fd2_in = open(pipe_2_in, O_WRONLY);
473 if (fd2_in < 0) {
474 DBG("Failed to open fifo %s", pipe_2_in);
475 ret = -EINVAL;
476 goto exit;
477 }
478
479 DBG("Pipe 1 IN %d, pipe 2 IN %d", fd1_in, fd2_in);
480
481 g_main_loop_run(main_loop);
482 ret = 0;
483 exit:
484 if (fifo1 >= 0)
485 g_source_remove(fifo1);
486
487 if (fifo2 >= 0)
488 g_source_remove(fifo2);
489
490 if (fd1_in >= 0)
491 close(fd1_in);
492
493 if (fd2_in >= 0)
494 close(fd2_in);
495
496 g_free(path);
497 g_free(pipe_1_in);
498 g_free(pipe_1_out);
499 g_free(pipe_2_in);
500 g_free(pipe_2_out);
501 monitor_pcap_free();
502 log_cleanup();
503 g_main_loop_unref(main_loop);
504
505 exit(ret);
506 }
507
