Fix uninitialized struct sockaddr_hci memory passed to bind()
[bluez-hcidump:bluez-hcidump.git] / src / hcidump.c
1 /*
2  *
3  *  BlueZ - Bluetooth protocol stack for Linux
4  *
5  *  Copyright (C) 2000-2002  Maxim Krasnyansky <maxk@qualcomm.com>
6  *  Copyright (C) 2003-2010  Marcel Holtmann <marcel@holtmann.org>
7  *
8  *
9  *  This program is free software; you can redistribute it and/or modify
10  *  it under the terms of the GNU General Public License as published by
11  *  the Free Software Foundation; either version 2 of the License, or
12  *  (at your option) any later version.
13  *
14  *  This program is distributed in the hope that it will be useful,
15  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17  *  GNU General Public License for more details.
18  *
19  *  You should have received a copy of the GNU General Public License
20  *  along with this program; if not, write to the Free Software
21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
22  *
23  */
24
25 #ifdef HAVE_CONFIG_H
26 #include <config.h>
27 #endif
28
29 #include <stdio.h>
30 #include <errno.h>
31 #include <fcntl.h>
32 #include <unistd.h>
33 #include <stdlib.h>
34 #include <string.h>
35 #include <getopt.h>
36 #include <sys/poll.h>
37 #include <sys/stat.h>
38 #include <sys/types.h>
39 #include <sys/ioctl.h>
40 #include <sys/socket.h>
41
42 #include <bluetooth/bluetooth.h>
43 #include <bluetooth/hci.h>
44 #include <bluetooth/hci_lib.h>
45
46 #include <arpa/inet.h>
47 #include <netinet/in.h>
48 #include <netdb.h>
49
50 #include "parser/parser.h"
51 #include "parser/sdp.h"
52
53 #if __BYTE_ORDER == __LITTLE_ENDIAN
54 static inline uint64_t ntoh64(uint64_t n)
55 {
56         uint64_t h;
57         uint64_t tmp = ntohl(n & 0x00000000ffffffff);
58         h = ntohl(n >> 32);
59         h |= tmp << 32;
60         return h;
61 }
62 #elif __BYTE_ORDER == __BIG_ENDIAN
63 #define ntoh64(x) (x)
64 #else
65 #error "Unknown byte order"
66 #endif
67 #define hton64(x) ntoh64(x)
68
69 #define SNAP_LEN        HCI_MAX_FRAME_SIZE
70 #define DEFAULT_PORT    "10839";
71
72 /* Modes */
73 enum {
74         PARSE,
75         READ,
76         WRITE,
77         SERVER,
78         PPPDUMP,
79         AUDIO
80 };
81
82 /* Default options */
83 static int  snap_len = SNAP_LEN;
84 static int  mode = PARSE;
85 static int  permcheck = 1;
86 static char *dump_file = NULL;
87 static char *pppdump_file = NULL;
88 static char *audio_file = NULL;
89 static char *dump_addr;
90 static char *dump_port = DEFAULT_PORT;
91 static int af = AF_UNSPEC;
92
93 struct hcidump_hdr {
94         uint16_t        len;
95         uint8_t         in;
96         uint8_t         pad;
97         uint32_t        ts_sec;
98         uint32_t        ts_usec;
99 } __attribute__ ((packed));
100 #define HCIDUMP_HDR_SIZE (sizeof(struct hcidump_hdr))
101
102 struct btsnoop_hdr {
103         uint8_t         id[8];          /* Identification Pattern */
104         uint32_t        version;        /* Version Number = 1 */
105         uint32_t        type;           /* Datalink Type */
106 } __attribute__ ((packed));
107 #define BTSNOOP_HDR_SIZE (sizeof(struct btsnoop_hdr))
108
109 struct btsnoop_pkt {
110         uint32_t        size;           /* Original Length */
111         uint32_t        len;            /* Included Length */
112         uint32_t        flags;          /* Packet Flags */
113         uint32_t        drops;          /* Cumulative Drops */
114         uint64_t        ts;             /* Timestamp microseconds */
115         uint8_t         data[0];        /* Packet Data */
116 } __attribute__ ((packed));
117 #define BTSNOOP_PKT_SIZE (sizeof(struct btsnoop_pkt))
118
119 static uint8_t btsnoop_id[] = { 0x62, 0x74, 0x73, 0x6e, 0x6f, 0x6f, 0x70, 0x00 };
120
121 static uint32_t btsnoop_version = 0;
122 static uint32_t btsnoop_type = 0;
123
124 struct pktlog_hdr {
125         uint32_t        len;
126         uint64_t        ts;
127         uint8_t         type;
128 } __attribute__ ((packed));
129 #define PKTLOG_HDR_SIZE (sizeof(struct pktlog_hdr))
130
131 static inline int read_n(int fd, char *buf, int len)
132 {
133         int t = 0, w;
134
135         while (len > 0) {
136                 if ((w = read(fd, buf, len)) < 0) {
137                         if (errno == EINTR || errno == EAGAIN)
138                                 continue;
139                         return -1;
140                 }
141                 if (!w)
142                         return 0;
143                 len -= w; buf += w; t += w;
144         }
145         return t;
146 }
147
148 static inline int write_n(int fd, char *buf, int len)
149 {
150         int t = 0, w;
151
152         while (len > 0) {
153                 if ((w = write(fd, buf, len)) < 0) {
154                         if (errno == EINTR || errno == EAGAIN)
155                                 continue;
156                         return -1;
157                 }
158                 if (!w)
159                         return 0;
160                 len -= w; buf += w; t += w;
161         }
162         return t;
163 }
164
165 static int process_frames(int dev, int sock, int fd, unsigned long flags)
166 {
167         struct cmsghdr *cmsg;
168         struct msghdr msg;
169         struct iovec  iv;
170         struct hcidump_hdr *dh;
171         struct btsnoop_pkt *dp;
172         struct frame frm;
173         struct pollfd fds[2];
174         int nfds = 0;
175         char *buf, *ctrl;
176         int len, hdr_size = HCIDUMP_HDR_SIZE;
177
178         if (sock < 0)
179                 return -1;
180
181         if (snap_len < SNAP_LEN)
182                 snap_len = SNAP_LEN;
183
184         if (flags & DUMP_BTSNOOP)
185                 hdr_size = BTSNOOP_PKT_SIZE;
186
187         buf = malloc(snap_len + hdr_size);
188         if (!buf) {
189                 perror("Can't allocate data buffer");
190                 return -1;
191         }
192
193         dh = (void *) buf;
194         dp = (void *) buf;
195         frm.data = buf + hdr_size;
196
197         ctrl = malloc(100);
198         if (!ctrl) {
199                 free(buf);
200                 perror("Can't allocate control buffer");
201                 return -1;
202         }
203
204         if (dev == HCI_DEV_NONE)
205                 printf("system: ");
206         else
207                 printf("device: hci%d ", dev);
208
209         printf("snap_len: %d filter: 0x%lx\n", snap_len, parser.filter);
210
211         memset(&msg, 0, sizeof(msg));
212
213         if (mode == SERVER) {
214                 struct btsnoop_hdr *hdr = (void *) buf;
215
216                 btsnoop_version = 1;
217                 btsnoop_type = 1002;
218
219                 memcpy(hdr->id, btsnoop_id, sizeof(btsnoop_id));
220                 hdr->version = htonl(btsnoop_version);
221                 hdr->type = htonl(btsnoop_type);
222
223                 printf("btsnoop version: %d datalink type: %d\n",
224                                                 btsnoop_version, btsnoop_type);
225
226                 len = write(fd, buf, BTSNOOP_HDR_SIZE);
227                 if (len < 0) {
228                         perror("Can't create dump header");
229                         return -1;
230                 }
231
232                 if (len != BTSNOOP_HDR_SIZE) {
233                         fprintf(stderr, "Header size mismatch\n");
234                         return -1;
235                 }
236
237                 fds[nfds].fd = fd;
238                 fds[nfds].events = POLLIN;
239                 fds[nfds].revents = 0;
240                 nfds++;
241         }
242
243         fds[nfds].fd = sock;
244         fds[nfds].events = POLLIN;
245         fds[nfds].revents = 0;
246         nfds++;
247
248         while (1) {
249                 int i, n = poll(fds, nfds, -1);
250                 if (n <= 0)
251                         continue;
252
253                 for (i = 0; i < nfds; i++) {
254                         if (fds[i].revents & (POLLHUP | POLLERR | POLLNVAL)) {
255                                 if (fds[i].fd == sock)
256                                         printf("device: disconnected\n");
257                                 else
258                                         printf("client: disconnect\n");
259                                 return 0;
260                         }
261                 }
262
263                 if (mode == SERVER) {
264                         len = recv(fd, buf, snap_len, MSG_DONTWAIT);
265                         if (len == 0) {
266                                 printf("client: disconnect\n");
267                                 return 0;
268                         }
269                         if (len < 0 && errno != EAGAIN && errno != EINTR) {
270                                 perror("Connection read failure");
271                                 return -1;
272                         }
273                 }
274
275                 iv.iov_base = frm.data;
276                 iv.iov_len  = snap_len;
277
278                 msg.msg_iov = &iv;
279                 msg.msg_iovlen = 1;
280                 msg.msg_control = ctrl;
281                 msg.msg_controllen = 100;
282
283                 len = recvmsg(sock, &msg, MSG_DONTWAIT);
284                 if (len < 0) {
285                         if (errno == EAGAIN || errno == EINTR)
286                                 continue;
287                         perror("Receive failed");
288                         return -1;
289                 }
290
291                 /* Process control message */
292                 frm.data_len = len;
293                 frm.dev_id = dev;
294                 frm.in = 0;
295                 frm.pppdump_fd = parser.pppdump_fd;
296                 frm.audio_fd   = parser.audio_fd;
297
298                 cmsg = CMSG_FIRSTHDR(&msg);
299                 while (cmsg) {
300                         switch (cmsg->cmsg_type) {
301                         case HCI_CMSG_DIR:
302                                 memcpy(&frm.in, CMSG_DATA(cmsg), sizeof(int));
303                                 break;
304                         case HCI_CMSG_TSTAMP:
305                                 memcpy(&frm.ts, CMSG_DATA(cmsg),
306                                                 sizeof(struct timeval));
307                                 break;
308                         }
309                         cmsg = CMSG_NXTHDR(&msg, cmsg);
310                 }
311
312                 frm.ptr = frm.data;
313                 frm.len = frm.data_len;
314
315                 switch (mode) {
316                 case WRITE:
317                 case SERVER:
318                         /* Save or send dump */
319                         if (flags & DUMP_BTSNOOP) {
320                                 uint64_t ts;
321                                 uint8_t pkt_type = ((uint8_t *) frm.data)[0];
322                                 dp->size = htonl(frm.data_len);
323                                 dp->len  = dp->size;
324                                 dp->flags = ntohl(frm.in & 0x01);
325                                 dp->drops = 0;
326                                 ts = (frm.ts.tv_sec - 946684800ll) * 1000000ll + frm.ts.tv_usec;
327                                 dp->ts = hton64(ts + 0x00E03AB44A676000ll);
328                                 if (pkt_type == HCI_COMMAND_PKT ||
329                                                 pkt_type == HCI_EVENT_PKT)
330                                         dp->flags |= ntohl(0x02);
331                         } else {
332                                 dh->len = htobs(frm.data_len);
333                                 dh->in  = frm.in;
334                                 dh->ts_sec  = htobl(frm.ts.tv_sec);
335                                 dh->ts_usec = htobl(frm.ts.tv_usec);
336                         }
337
338                         if (write_n(fd, buf, frm.data_len + hdr_size) < 0) {
339                                 perror("Write error");
340                                 return -1;
341                         }
342                         break;
343
344                 default:
345                         /* Parse and print */
346                         parse(&frm);
347                         break;
348                 }
349         }
350
351         return 0;
352 }
353
354 static void read_dump(int fd)
355 {
356         struct hcidump_hdr dh;
357         struct btsnoop_pkt dp;
358         struct pktlog_hdr ph;
359         struct frame frm;
360         uint8_t pkt_type;
361         int err;
362
363         frm.data = malloc(HCI_MAX_FRAME_SIZE);
364         if (!frm.data) {
365                 perror("Can't allocate data buffer");
366                 exit(1);
367         }
368
369         while (1) {
370                 if (parser.flags & DUMP_PKTLOG)
371                         err = read_n(fd, (void *) &ph, PKTLOG_HDR_SIZE);
372                 else if (parser.flags & DUMP_BTSNOOP)
373                         err = read_n(fd, (void *) &dp, BTSNOOP_PKT_SIZE);
374                 else
375                         err = read_n(fd, (void *) &dh, HCIDUMP_HDR_SIZE);
376
377                 if (err < 0)
378                         goto failed;
379                 if (!err)
380                         return;
381
382                 if (parser.flags & DUMP_PKTLOG) {
383                         switch (ph.type) {
384                         case 0x00:
385                                 ((uint8_t *) frm.data)[0] = HCI_COMMAND_PKT;
386                                 frm.in = 0;
387                                 break;
388                         case 0x01:
389                                 ((uint8_t *) frm.data)[0] = HCI_EVENT_PKT;
390                                 frm.in = 1;
391                                 break;
392                         case 0x02:
393                                 ((uint8_t *) frm.data)[0] = HCI_ACLDATA_PKT;
394                                 frm.in = 0;
395                                 break;
396                         case 0x03:
397                                 ((uint8_t *) frm.data)[0] = HCI_ACLDATA_PKT;
398                                 frm.in = 1;
399                                 break;
400                         default:
401                                 lseek(fd, ntohl(ph.len) - 9, SEEK_CUR);
402                                 continue;
403                         }
404
405                         frm.data_len = ntohl(ph.len) - 8;
406                         err = read_n(fd, frm.data + 1, frm.data_len - 1);
407                 } else if (parser.flags & DUMP_BTSNOOP) {
408                         switch (btsnoop_type) {
409                         case 1001:
410                                 if (ntohl(dp.flags) & 0x02) {
411                                         if (ntohl(dp.flags) & 0x01)
412                                                 pkt_type = HCI_EVENT_PKT;
413                                         else
414                                                 pkt_type = HCI_COMMAND_PKT;
415                                 } else
416                                         pkt_type = HCI_ACLDATA_PKT;
417
418                                 ((uint8_t *) frm.data)[0] = pkt_type;
419
420                                 frm.data_len = ntohl(dp.len) + 1;
421                                 err = read_n(fd, frm.data + 1, frm.data_len - 1);
422                                 break;
423
424                         case 1002:
425                                 frm.data_len = ntohl(dp.len);
426                                 err = read_n(fd, frm.data, frm.data_len);
427                                 break;
428                         }
429                 } else {
430                         frm.data_len = btohs(dh.len);
431                         err = read_n(fd, frm.data, frm.data_len);
432                 }
433
434                 if (err < 0)
435                         goto failed;
436                 if (!err)
437                         return;
438
439                 frm.ptr = frm.data;
440                 frm.len = frm.data_len;
441
442                 if (parser.flags & DUMP_PKTLOG) {
443                         uint64_t ts;
444                         ts = ntoh64(ph.ts);
445                         frm.ts.tv_sec = ts >> 32;
446                         frm.ts.tv_usec = ts & 0xffffffff;
447                 } else if (parser.flags & DUMP_BTSNOOP) {
448                         uint64_t ts;
449                         frm.in = ntohl(dp.flags) & 0x01;
450                         ts = ntoh64(dp.ts) - 0x00E03AB44A676000ll;
451                         frm.ts.tv_sec = (ts / 1000000ll) + 946684800ll;
452                         frm.ts.tv_usec = ts % 1000000ll;
453                 } else {
454                         frm.in = dh.in;
455                         frm.ts.tv_sec  = btohl(dh.ts_sec);
456                         frm.ts.tv_usec = btohl(dh.ts_usec);
457                 }
458
459                 parse(&frm);
460         }
461
462 failed:
463         perror("Read failed");
464         exit(1);
465 }
466
467 static int open_file(char *file, int mode, unsigned long flags)
468 {
469         unsigned char buf[BTSNOOP_HDR_SIZE];
470         struct btsnoop_hdr *hdr = (struct btsnoop_hdr *) buf;
471         int fd, len, open_flags;
472
473         if (mode == WRITE || mode == PPPDUMP || mode == AUDIO)
474                 open_flags = O_WRONLY | O_CREAT | O_TRUNC;
475         else
476                 open_flags = O_RDONLY;
477
478         fd = open(file, open_flags, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
479         if (fd < 0) {
480                 perror("Can't open dump file");
481                 exit(1);
482         }
483
484         if (mode == READ) {
485                 len = read(fd, buf, BTSNOOP_HDR_SIZE);
486                 if (len != BTSNOOP_HDR_SIZE) {
487                         lseek(fd, 0, SEEK_SET);
488                         return fd;
489                 }
490
491                 if (!memcmp(hdr->id, btsnoop_id, sizeof(btsnoop_id))) {
492                         parser.flags |= DUMP_BTSNOOP;
493
494                         btsnoop_version = ntohl(hdr->version);
495                         btsnoop_type = ntohl(hdr->type);
496
497                         printf("btsnoop version: %d datalink type: %d\n",
498                                                 btsnoop_version, btsnoop_type);
499
500                         if (btsnoop_version != 1) {
501                                 fprintf(stderr, "Unsupported BTSnoop version\n");
502                                 exit(1);
503                         }
504
505                         if (btsnoop_type != 1001 && btsnoop_type != 1002) {
506                                 fprintf(stderr, "Unsupported BTSnoop datalink type\n");
507                                 exit(1);
508                         }
509                 } else {
510                         if (buf[0] == 0x00 && buf[1] == 0x00) {
511                                 parser.flags |= DUMP_PKTLOG;
512                                 printf("packet logger data format\n");
513                         }
514
515                         parser.flags &= ~DUMP_BTSNOOP;
516                         lseek(fd, 0, SEEK_SET);
517                         return fd;
518                 }
519         } else {
520                 if (flags & DUMP_BTSNOOP) {
521                         btsnoop_version = 1;
522                         btsnoop_type = 1002;
523
524                         memcpy(hdr->id, btsnoop_id, sizeof(btsnoop_id));
525                         hdr->version = htonl(btsnoop_version);
526                         hdr->type = htonl(btsnoop_type);
527
528                         printf("btsnoop version: %d datalink type: %d\n",
529                                                 btsnoop_version, btsnoop_type);
530
531                         len = write(fd, buf, BTSNOOP_HDR_SIZE);
532                         if (len < 0) {
533                                 perror("Can't create dump header");
534                                 exit(1);
535                         }
536
537                         if (len != BTSNOOP_HDR_SIZE) {
538                                 fprintf(stderr, "Header size mismatch\n");
539                                 exit(1);
540                         }
541                 }
542         }
543
544         return fd;
545 }
546
547 static int open_socket(int dev, unsigned long flags)
548 {
549         struct sockaddr_hci addr;
550         struct hci_filter flt;
551         struct hci_dev_info di;
552         int sk, dd, opt;
553
554         if (permcheck && dev != HCI_DEV_NONE) {
555                 dd = hci_open_dev(dev);
556                 if (dd < 0) {
557                         perror("Can't open device");
558                         return -1;
559                 }
560
561                 if (hci_devinfo(dev, &di) < 0) {
562                         perror("Can't get device info");
563                         return -1;
564                 }
565
566                 opt = hci_test_bit(HCI_RAW, &di.flags);
567                 if (ioctl(dd, HCISETRAW, opt) < 0) {
568                         if (errno == EACCES) {
569                                 perror("Can't access device");
570                                 return -1;
571                         }
572                 }
573
574                 hci_close_dev(dd);
575         }
576
577         /* Create HCI socket */
578         sk = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
579         if (sk < 0) {
580                 perror("Can't create raw socket");
581                 return -1;
582         }
583
584         opt = 1;
585         if (setsockopt(sk, SOL_HCI, HCI_DATA_DIR, &opt, sizeof(opt)) < 0) {
586                 perror("Can't enable data direction info");
587                 return -1;
588         }
589
590         opt = 1;
591         if (setsockopt(sk, SOL_HCI, HCI_TIME_STAMP, &opt, sizeof(opt)) < 0) {
592                 perror("Can't enable time stamp");
593                 return -1;
594         }
595
596         /* Setup filter */
597         hci_filter_clear(&flt);
598         hci_filter_all_ptypes(&flt);
599         hci_filter_all_events(&flt);
600         if (setsockopt(sk, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
601                 perror("Can't set filter");
602                 return -1;
603         }
604
605         /* Bind socket to the HCI device */
606         memset(&addr, 0, sizeof(addr));
607         addr.hci_family = AF_BLUETOOTH;
608         addr.hci_dev = dev;
609         if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
610                 printf("Can't attach to device hci%d. %s(%d)\n", 
611                                         dev, strerror(errno), errno);
612                 return -1;
613         }
614
615         return sk;
616 }
617
618 static int create_datagram(unsigned short port)
619 {
620         struct sockaddr_in addr;
621         int sk, opt = 1;
622
623         sk = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
624         if (sk < 0)
625                 return -1;
626
627         if (setsockopt(sk, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0) {
628                 close(sk);
629                 return -1;
630         }
631
632         memset(&addr, 0, sizeof(addr));
633         addr.sin_family = AF_INET;
634         addr.sin_port = htons(port);
635         addr.sin_addr.s_addr = htonl(INADDR_BROADCAST);
636
637         if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
638                 close(sk);
639                 return -1;
640         }
641
642         return sk;
643 }
644
645 static unsigned char ping_data[] = { 'p', 'i', 'n', 'g' };
646 static unsigned char pong_data[] = { 'p', 'o', 'n', 'g' };
647
648 static void handle_datagram(int sk)
649 {
650         struct sockaddr_in addr;
651         socklen_t addr_len = sizeof(addr);
652         unsigned char buf[64];
653         ssize_t len;
654
655         len = recvfrom(sk, buf, sizeof(buf), MSG_DONTWAIT,
656                                 (struct sockaddr *) &addr, &addr_len);
657
658         if (len != sizeof(ping_data))
659                 return;
660
661         if (memcmp(buf, ping_data, sizeof(ping_data)) != 0)
662                 return;
663
664         len = sendto(sk, pong_data, sizeof(pong_data), 0,
665                                 (struct sockaddr *) &addr, sizeof(addr));
666 }
667
668 static int wait_connection(char *addr, char *port)
669 {
670         char hname[100], hport[10];
671         struct addrinfo *ai, *runp;
672         struct addrinfo hints;
673         struct pollfd fds[3];
674         unsigned int nfds = 0;
675         int err, opt, datagram;
676
677         memset(&hints, 0, sizeof (hints));
678         hints.ai_flags = AI_PASSIVE | AI_ADDRCONFIG;
679         hints.ai_socktype = SOCK_STREAM;
680         hints.ai_protocol = IPPROTO_TCP;
681
682         err = getaddrinfo(dump_addr, dump_port, &hints, &ai);
683         if (err < 0) {
684                 printf("Can't get address info: %s\n", gai_strerror(err));
685                 return -1;
686         }
687
688         runp = ai;
689
690         datagram = create_datagram(atoi(dump_port));
691         if (datagram < 0) {
692                 printf("server: no discover protocol\n");
693         } else {
694                 fds[nfds].fd = datagram;
695                 fds[nfds].events = POLLIN;
696                 nfds++;
697         }
698
699         while (runp != NULL && nfds < sizeof(fds) / sizeof(fds[0])) {
700                 fds[nfds].fd = socket(runp->ai_family, runp->ai_socktype,
701                                                         runp->ai_protocol);
702                 if (fds[nfds].fd < 0) {
703                         perror("Can't create socket");
704                         return -1;
705                 }
706
707                 fds[nfds].events = POLLIN;
708
709                 opt = 1;
710                 setsockopt(fds[nfds].fd, SOL_SOCKET, SO_REUSEADDR,
711                                                         &opt, sizeof(opt));
712
713                 opt = 0;
714                 setsockopt(fds[nfds].fd, SOL_SOCKET, SO_KEEPALIVE,
715                                                         &opt, sizeof(opt));
716
717                 if (bind(fds[nfds].fd, runp->ai_addr, runp->ai_addrlen) < 0) {
718                         if (errno != EADDRINUSE) {
719                                 perror("Can't bind socket");
720                                 return -1;
721                         }
722
723                         close(fds[nfds].fd);
724                 } else {
725                         if (listen(fds[nfds].fd, SOMAXCONN) < 0) {
726                                 perror("Can't listen on socket");
727                                 return -1;
728                         }
729
730                         getnameinfo(runp->ai_addr, runp->ai_addrlen,
731                                                         hname, sizeof(hname),
732                                                         hport, sizeof(hport),
733                                                         NI_NUMERICSERV);
734
735                         printf("server: %s:%s snap_len: %d filter: 0x%lx\n",
736                                         hname, hport, snap_len, parser.filter);
737
738                         nfds++;
739                 }
740
741                 runp = runp->ai_next;
742         }
743
744         freeaddrinfo(ai);
745
746         while (1) {
747                 unsigned int i;
748                 int n = poll(fds, nfds, -1);
749                 if (n <= 0)
750                         continue;
751
752                 for (i = 0; i < nfds; i++) {
753                         struct sockaddr_storage rem;
754                         socklen_t remlen = sizeof(rem);
755                         int sk;
756
757                         if (!(fds[i].revents & POLLIN))
758                                 continue;
759
760                         if (fds[i].fd == datagram) {
761                                 handle_datagram(datagram);
762                                 continue;
763                         }
764
765                         sk = accept(fds[i].fd, (struct sockaddr *) &rem, &remlen);
766                         if (sk < 0)
767                                 continue;
768
769                         getnameinfo((struct sockaddr *) &rem, remlen,
770                                                         hname, sizeof(hname),
771                                                         hport, sizeof(hport),
772                                                         NI_NUMERICSERV);
773
774                         printf("client: %s:%s snap_len: %d filter: 0x%lx\n",
775                                         hname, hport, snap_len, parser.filter);
776
777                         for (n = 0; n < (int) nfds; n++)
778                                 close(fds[n].fd);
779
780                         return sk;
781                 }
782         }
783
784         return -1;
785 }
786
787 static int run_server(int dev, char *addr, char *port, unsigned long flags)
788 {
789         while (1) {
790                 int dd, sk;
791
792                 sk = wait_connection(addr, port);
793                 if (sk < 0)
794                         continue;
795
796                 //fcntl(sk, F_SETFL, O_NONBLOCK);
797
798                 dd = open_socket(dev, flags);
799                 if (dd < 0) {
800                         close(sk);
801                         continue;
802                 }
803
804                 process_frames(dev, dd, sk, flags);
805
806                 close(dd);
807                 close(sk);
808         }
809
810         return 0;
811 }
812
813 static struct {
814         char *name;
815         int  flag;
816 } filters[] = {
817         { "lmp",        FILT_LMP        },
818         { "hci",        FILT_HCI        },
819         { "sco",        FILT_SCO        },
820         { "l2cap",      FILT_L2CAP      },
821         { "rfcomm",     FILT_RFCOMM     },
822         { "sdp",        FILT_SDP        },
823         { "bnep",       FILT_BNEP       },
824         { "cmtp",       FILT_CMTP       },
825         { "hidp",       FILT_HIDP       },
826         { "hcrp",       FILT_HCRP       },
827         { "avdtp",      FILT_AVDTP      },
828         { "avctp",      FILT_AVCTP      },
829         { "obex",       FILT_OBEX       },
830         { "capi",       FILT_CAPI       },
831         { "ppp",        FILT_PPP        },
832         { "csr",        FILT_CSR        },
833         { "dga",        FILT_DGA        },
834         { 0 }
835 };
836
837 static unsigned long parse_filter(int argc, char **argv)
838 {
839         unsigned long filter = 0;
840         int i,n;
841
842         for (i = 0; i < argc; i++) {
843                 for (n = 0; filters[n].name; n++) {
844                         if (!strcasecmp(filters[n].name, argv[i])) {
845                                 filter |= filters[n].flag;
846                                 break;
847                         }
848                 }
849         }
850
851         return filter;
852 }
853
854 static void usage(void)
855 {
856         printf(
857         "Usage: hcidump [OPTION...] [filter]\n"
858         "  -i, --device=hci_dev       HCI device\n"
859         "  -l, --snap-len=len         Snap len (in bytes)\n"
860         "  -p, --psm=psm              Default PSM\n"
861         "  -m, --manufacturer=compid  Default manufacturer\n"
862         "  -w, --save-dump=file       Save dump to a file\n"
863         "  -r, --read-dump=file       Read dump from a file\n"
864         "  -d, --wait-dump=host       Wait on a host and send\n"
865         "  -t, --ts                   Display time stamps\n"
866         "  -a, --ascii                Dump data in ascii\n"
867         "  -x, --hex                  Dump data in hex\n"
868         "  -X, --ext                  Dump data in hex and ascii\n"
869         "  -R, --raw                  Dump raw data\n"
870         "  -C, --cmtp=psm             PSM for CMTP\n"
871         "  -H, --hcrp=psm             PSM for HCRP\n"
872         "  -O, --obex=channel         Channel for OBEX\n"
873         "  -P, --ppp=channel          Channel for PPP\n"
874         "  -D, --pppdump=file         Extract PPP traffic\n"
875         "  -A, --audio=file           Extract SCO audio data\n"
876         "  -Y, --novendor             No vendor commands or events\n"
877         "  -4, --ipv4                 Use IPv4 as transport\n"
878         "  -6  --ipv6                 Use IPv6 as transport\n"
879         "  -h, --help                 Give this help list\n"
880         "  -v, --version              Give version information\n"
881         "      --usage                Give a short usage message\n"
882         );
883 }
884
885 static struct option main_options[] = {
886         { "device",             1, 0, 'i' },
887         { "snap-len",           1, 0, 'l' },
888         { "psm",                1, 0, 'p' },
889         { "manufacturer",       1, 0, 'm' },
890         { "save-dump",          1, 0, 'w' },
891         { "read-dump",          1, 0, 'r' },
892         { "wait-dump",          1, 0, 'd' },
893         { "timestamp",          0, 0, 't' },
894         { "ascii",              0, 0, 'a' },
895         { "hex",                0, 0, 'x' },
896         { "ext",                0, 0, 'X' },
897         { "raw",                0, 0, 'R' },
898         { "cmtp",               1, 0, 'C' },
899         { "hcrp",               1, 0, 'H' },
900         { "obex",               1, 0, 'O' },
901         { "ppp",                1, 0, 'P' },
902         { "pppdump",            1, 0, 'D' },
903         { "audio",              1, 0, 'A' },
904         { "novendor",           0, 0, 'Y' },
905         { "nopermcheck",        0, 0, 'Z' },
906         { "ipv4",               0, 0, '4' },
907         { "ipv6",               0, 0, '6' },
908         { "help",               0, 0, 'h' },
909         { "version",            0, 0, 'v' },
910         { 0 }
911 };
912
913 int main(int argc, char *argv[])
914 {
915         unsigned long flags = 0;
916         unsigned long filter = 0;
917         int device = 0;
918         int defpsm = 0;
919         int defcompid = DEFAULT_COMPID;
920         int opt, pppdump_fd = -1, audio_fd = -1;
921
922         while ((opt=getopt_long(argc, argv, "i:l:p:m:w:r:d:taxXRC:H:O:P:D:A:YZ46hv", main_options, NULL)) != -1) {
923                 switch(opt) {
924                 case 'i':
925                         if (strcasecmp(optarg, "none") && strcasecmp(optarg, "system"))
926                                 device = atoi(optarg + 3);
927                         else
928                                 device = HCI_DEV_NONE;
929                         break;
930
931                 case 'l': 
932                         snap_len = atoi(optarg);
933                         break;
934
935                 case 'p': 
936                         defpsm = atoi(optarg);
937                         break;
938
939                 case 'm':
940                         defcompid = atoi(optarg);
941                         break;
942
943                 case 'w':
944                         mode = WRITE;
945                         dump_file = strdup(optarg);
946                         break;
947
948                 case 'r':
949                         mode = READ;
950                         dump_file = strdup(optarg);
951                         break;
952
953                 case 'd':
954                         mode = SERVER;
955                         dump_addr = optarg;
956                         break;
957
958                 case 't': 
959                         flags |= DUMP_TSTAMP;
960                         break;
961
962                 case 'a': 
963                         flags |= DUMP_ASCII;
964                         break;
965
966                 case 'x':
967                         flags |= DUMP_HEX;
968                         break;
969
970                 case 'X':
971                         flags |= DUMP_EXT;
972                         break;
973
974                 case 'R': 
975                         flags |= DUMP_RAW;
976                         break;
977
978                 case 'C': 
979                         set_proto(0, atoi(optarg), 0, SDP_UUID_CMTP);
980                         break;
981
982                 case 'H':
983                         set_proto(0, atoi(optarg), 0, SDP_UUID_HARDCOPY_CONTROL_CHANNEL);
984                         break;
985
986                 case 'O':
987                         set_proto(0, 0, atoi(optarg), SDP_UUID_OBEX);
988                         break;
989
990                 case 'P':
991                         set_proto(0, 0, atoi(optarg), SDP_UUID_LAN_ACCESS_PPP);
992                         break;
993
994                 case 'D':
995                         pppdump_file = strdup(optarg);
996                         break;
997
998                 case 'A':
999                         audio_file = strdup(optarg);
1000                         break;
1001
1002                 case 'Y':
1003                         flags |= DUMP_NOVENDOR;
1004                         break;
1005
1006                 case 'Z':
1007                         permcheck = 0;
1008                         break;
1009
1010                 case '4':
1011                         af = AF_INET;
1012                         break;
1013
1014                 case '6':
1015                         af = AF_INET6;
1016                         break;
1017
1018                 case 'v':
1019                         printf("%s\n", VERSION);
1020                         exit(0);
1021
1022                 case 'h':
1023                 default:
1024                         usage();
1025                         exit(0);
1026                 }
1027         }
1028
1029         argc -= optind;
1030         argv += optind;
1031         optind = 0;
1032
1033         printf("HCI sniffer - Bluetooth packet analyzer ver %s\n", VERSION);
1034
1035         if (argc > 0)
1036                 filter = parse_filter(argc, argv);
1037
1038         /* Default settings */
1039         if (!filter)
1040                 filter = ~0L;
1041
1042         if (pppdump_file)
1043                 pppdump_fd = open_file(pppdump_file, PPPDUMP, flags);
1044
1045         if (audio_file)
1046                 audio_fd = open_file(audio_file, AUDIO, flags);
1047
1048         switch (mode) {
1049         case PARSE:
1050                 flags |= DUMP_VERBOSE;
1051                 init_parser(flags, filter, defpsm, defcompid, pppdump_fd, audio_fd);
1052                 process_frames(device, open_socket(device, flags), -1, flags);
1053                 break;
1054
1055         case READ:
1056                 flags |= DUMP_VERBOSE;
1057                 init_parser(flags, filter, defpsm, defcompid, pppdump_fd, audio_fd);
1058                 read_dump(open_file(dump_file, mode, flags));
1059                 break;
1060
1061         case WRITE:
1062                 flags |= DUMP_BTSNOOP;
1063                 process_frames(device, open_socket(device, flags),
1064                                 open_file(dump_file, mode, flags), flags);
1065                 break;
1066
1067         case SERVER:
1068                 flags |= DUMP_BTSNOOP;
1069                 init_parser(flags, filter, defpsm, defcompid, pppdump_fd, audio_fd);
1070                 run_server(device, dump_addr, dump_port, flags);
1071                 break;
1072         }
1073
1074         return 0;
1075 }