1 /*
2 *
3 * BlueZ - Bluetooth protocol stack for Linux
4 *
5 * Copyright (C) 2004-2011 Marcel Holtmann <marcel@holtmann.org>
6 *
7 *
8 * This program is free software; you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License as published by
10 * the Free Software Foundation; either version 2 of the License, or
11 * (at your option) any later version.
12 *
13 * This program is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU General Public License for more details.
17 *
18 * You should have received a copy of the GNU General Public License
19 * along with this program; if not, write to the Free Software
20 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 *
22 */
23
24 #ifdef HAVE_CONFIG_H
25 #include <config.h>
26 #endif
27
28 #include <stdio.h>
29 #include <errno.h>
30 #include <ctype.h>
31 #include <unistd.h>
32 #include <stdlib.h>
33 #include <malloc.h>
34 #include <getopt.h>
35 #include <signal.h>
36 #include <sys/ioctl.h>
37 #include <sys/socket.h>
38
39 #include <bluetooth/bluetooth.h>
40 #include <bluetooth/hci.h>
41 #include <bluetooth/hci_lib.h>
42
43 #include <netinet/in.h>
44
45 #include "parser/parser.h"
46
47 static volatile sig_atomic_t __io_canceled = 0;
48
sig_hup(int sig)49 static void sig_hup(int sig)
50 {
51 }
52
sig_term(int sig)53 static void sig_term(int sig)
54 {
55 __io_canceled = 1;
56 }
57
read_revision(int dd,char * revision,int size)58 static int read_revision(int dd, char *revision, int size)
59 {
60 struct hci_request rq;
61 unsigned char req[] = { 0x07 };
62 unsigned char buf[46];
63
64 memset(&rq, 0, sizeof(rq));
65 rq.ogf = OGF_VENDOR_CMD;
66 rq.ocf = 0x000e;
67 rq.cparam = req;
68 rq.clen = sizeof(req);
69 rq.rparam = &buf;
70 rq.rlen = sizeof(buf);
71
72 if (hci_send_req(dd, &rq, 1000) < 0)
73 return -1;
74
75 if (buf[0] > 0) {
76 errno = EIO;
77 return -1;
78 }
79
80 if (revision)
81 strncpy(revision, (char *) (buf + 1), size);
82
83 return 0;
84 }
85
enable_sniffer(int dd,uint8_t enable)86 static int enable_sniffer(int dd, uint8_t enable)
87 {
88 struct hci_request rq;
89 unsigned char req[] = { 0x00, enable };
90 unsigned char buf[1];
91
92 memset(&rq, 0, sizeof(rq));
93 rq.ogf = OGF_VENDOR_CMD;
94 rq.ocf = 0x000e;
95 rq.cparam = req;
96 rq.clen = sizeof(req);
97 rq.rparam = &buf;
98 rq.rlen = sizeof(buf);
99
100 if (hci_send_req(dd, &rq, 1000) < 0)
101 return -1;
102
103 if (buf[0] > 0) {
104 errno = EIO;
105 return -1;
106 }
107
108 return 0;
109 }
110
enable_sync(int dd,uint8_t enable,bdaddr_t * bdaddr)111 static int enable_sync(int dd, uint8_t enable, bdaddr_t *bdaddr)
112 {
113 struct hci_request rq;
114 unsigned char req[] = { 0x01, enable,
115 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
116 0x00, 0xfa, 0x00 };
117
118 memcpy(req + 2, bdaddr, 6);
119
120 memset(&rq, 0, sizeof(rq));
121 rq.ogf = OGF_VENDOR_CMD;
122 rq.ocf = 0x000e;
123 rq.cparam = req;
124 rq.clen = sizeof(req);
125
126 hci_send_req(dd, &rq, 1000);
127
128 return 0;
129 }
130
type2str(uint8_t type)131 static char *type2str(uint8_t type)
132 {
133 switch (type) {
134 case 0x00:
135 return "NULL";
136 case 0x01:
137 return "POLL";
138 case 0x02:
139 return "FHS";
140 case 0x03:
141 return "DM1";
142 case 0x04:
143 return "DH1";
144 case 0x05:
145 return "HV1";
146 case 0x06:
147 return "HV2";
148 case 0x07:
149 return "HV3";
150 case 0x08:
151 return "DV";
152 case 0x09:
153 return "AUX1";
154 case 0x0a:
155 return "DM3";
156 case 0x0b:
157 return "DH3";
158 case 0x0c:
159 return "EV4";
160 case 0x0d:
161 return "EV5";
162 case 0x0e:
163 return "DM5";
164 case 0x0f:
165 return "DH5";
166 case 0xff:
167 return "ID";
168 default:
169 return "UNK";
170 }
171 }
172
decode(unsigned char * buf,int count)173 static void decode(unsigned char *buf, int count)
174 {
175 struct frame frm;
176 uint8_t id, status, channel;
177 uint16_t num, len;
178 uint32_t time;
179 uint8_t type, addr, temp, hdr;
180 uint8_t flow, arqn, seqn, hec, llid, pflow;
181 uint16_t plen;
182
183 if (count < 7)
184 return;
185
186 id = buf[0];
187 num = ntohs(bt_get_unaligned((uint16_t *) (buf + 1)));
188 len = btohs(bt_get_unaligned((uint16_t *) (buf + 3)));
189
190 status = buf[5];
191 time = ntohl(bt_get_unaligned((uint32_t *) (buf + 6)));
192 channel = buf[10];
193
194 if (len < 8)
195 return;
196
197 type = (len < 7) ? 0xff : bt_get_unaligned((uint8_t *) (buf + 11));
198
199 if (type < 2)
200 return;
201
202 p_indent(-1, NULL);
203
204 memset(&frm, 0, sizeof(frm));
205 frm.data = buf + 12;
206 frm.data_len = count - 12;
207 frm.ptr = frm.data;
208 frm.len = frm.data_len;
209 frm.in = 0;
210 frm.master = 0;
211 frm.handle = 0;
212 frm.flags = 0;
213
214 p_indent(0, &frm);
215
216 printf("BPA: id %d num %d status 0x%02x time %d channel %2d len %d\n",
217 id, num, status, time, channel, len - 6);
218
219 if (type < 3) {
220 printf(" %s\n", type2str(type));
221 raw_dump(1, &frm);
222 return;
223 }
224
225 addr = bt_get_unaligned((uint8_t *) (buf + 12));
226 temp = bt_get_unaligned((uint8_t *) (buf + 13));
227 flow = (temp & 0x04) >> 2;
228 arqn = (temp & 0x02) >> 1;
229 seqn = (temp & 0x01);
230 hec = bt_get_unaligned((uint8_t *) (buf + 14));
231
232 hdr = bt_get_unaligned((uint8_t *) (buf + 20));
233 plen = ((hdr & 0x10) >> 4) | ((hdr & 0x08) >> 2) | (hdr & 0x04) | ((hdr & 0x02) << 2) | ((hdr & 0x01) << 4);
234 pflow = ((hdr & 0x20) >> 5);
235 llid = ((hdr & 0x80) >> 7) | ((hdr & 0x40) >> 5);
236 hdr = bt_get_unaligned((uint8_t *) (buf + 21));
237 plen = plen | ((hdr & 0x80) >> 2) | (hdr & 0x40) | ((hdr & 0x20) << 2) | ((hdr & 0x08) << 4);
238
239 p_indent(0, &frm);
240
241 printf("%s: addr 0x%02x flow %d arqn %d seqn %d hec 0x%02x llid %d pflow %d plen %d\n",
242 type2str(type), addr, flow, arqn, seqn, hec, llid, pflow, plen);
243
244 if (type == 0x03 && llid == 3) {
245 memset(&frm, 0, sizeof(frm));
246 frm.data = buf + 22;
247 frm.data_len = plen;
248 frm.ptr = frm.data;
249 frm.len = frm.data_len;
250 frm.in = 0;
251 frm.master = 1;
252 frm.handle = 0;
253 frm.flags = llid;
254
255 lmp_dump(1, &frm);
256 return;
257 }
258
259 raw_dump(1, &frm);
260 }
261
process_frames(int dev)262 static void process_frames(int dev)
263 {
264 struct sigaction sa;
265 struct hci_filter flt;
266 unsigned char *buf;
267 int dd, size = 2048;
268
269 buf = malloc(size);
270 if (!buf) {
271 fprintf(stderr, "Can't allocate buffer for hci%d: %s (%d)\n",
272 dev, strerror(errno), errno);
273 return;
274 }
275
276 dd = hci_open_dev(dev);
277 if (dd < 0) {
278 fprintf(stderr, "Can't open device hci%d: %s (%d)\n",
279 dev, strerror(errno), errno);
280 free(buf);
281 return;
282 }
283
284 hci_filter_clear(&flt);
285 hci_filter_set_ptype(HCI_VENDOR_PKT, &flt);
286 hci_filter_set_ptype(HCI_EVENT_PKT, &flt);
287 hci_filter_set_event(EVT_VENDOR, &flt);
288
289 if (setsockopt(dd, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
290 fprintf(stderr, "Can't set filter for hci%d: %s (%d)\n",
291 dev, strerror(errno), errno);
292 hci_close_dev(dd);
293 free(buf);
294 return;
295 }
296
297 memset(&sa, 0, sizeof(sa));
298 sa.sa_flags = SA_NOCLDSTOP;
299 sa.sa_handler = SIG_IGN;
300 sigaction(SIGCHLD, &sa, NULL);
301 sigaction(SIGPIPE, &sa, NULL);
302
303 sa.sa_handler = sig_term;
304 sigaction(SIGTERM, &sa, NULL);
305 sigaction(SIGINT, &sa, NULL);
306
307 sa.sa_handler = sig_hup;
308 sigaction(SIGHUP, &sa, NULL);
309
310 while (!__io_canceled) {
311 int len;
312
313 len = read(dd, buf, size);
314 if (len < 0)
315 break;
316 if (len < 2)
317 continue;
318
319 if (buf[0] == 0x04 && buf[1] == 0xff) {
320 if (buf[3] == 0x02) {
321 switch (buf[4]) {
322 case 0x00:
323 printf("Waiting for synchronization...\n");
324 break;
325 case 0x08:
326 printf("Synchronization lost\n");
327 __io_canceled = 1;
328 break;
329 default:
330 printf("Unknown event 0x%02x\n", buf[4]);
331 break;
332 }
333 }
334 }
335
336 if (buf[0] != 0xff)
337 continue;
338
339 decode(buf + 1, len - 1);
340 }
341
342 hci_close_dev(dd);
343
344 free(buf);
345 }
346
usage(void)347 static void usage(void)
348 {
349 printf("bpasniff - Utility for the BPA 100/105 sniffers\n\n");
350 printf("Usage:\n"
351 "\tbpasniff [-i <dev>] <master-bdaddr>\n");
352 }
353
354 static struct option main_options[] = {
355 { "help", 0, 0, 'h' },
356 { "device", 1, 0, 'i' },
357 { 0, 0, 0, 0}
358 };
359
main(int argc,char * argv[])360 int main(int argc, char *argv[])
361 {
362 struct hci_dev_info di;
363 struct hci_version ver;
364 char rev[46];
365 bdaddr_t bdaddr;
366 int dd, opt, dev = 0;
367
368 bacpy(&bdaddr, BDADDR_ANY);
369
370 while ((opt=getopt_long(argc, argv, "+i:h", main_options, NULL)) != -1) {
371 switch (opt) {
372 case 'i':
373 dev = hci_devid(optarg);
374 if (dev < 0) {
375 perror("Invalid device");
376 exit(1);
377 }
378 break;
379
380 case 'h':
381 default:
382 usage();
383 exit(0);
384 }
385 }
386
387 argc -= optind;
388 argv += optind;
389 optind = 0;
390
391 argc -= optind;
392 argv += optind;
393 optind = 0;
394
395 if (argc < 1) {
396 usage();
397 exit(1);
398 }
399
400 str2ba(argv[0], &bdaddr);
401
402 dd = hci_open_dev(dev);
403 if (dd < 0) {
404 fprintf(stderr, "Can't open device hci%d: %s (%d)\n",
405 dev, strerror(errno), errno);
406 exit(1);
407 }
408
409 if (hci_devinfo(dev, &di) < 0) {
410 fprintf(stderr, "Can't get device info for hci%d: %s (%d)\n",
411 dev, strerror(errno), errno);
412 hci_close_dev(dd);
413 exit(1);
414 }
415
416 if (hci_read_local_version(dd, &ver, 1000) < 0) {
417 fprintf(stderr, "Can't read version info for hci%d: %s (%d)\n",
418 dev, strerror(errno), errno);
419 hci_close_dev(dd);
420 exit(1);
421 }
422
423 if (ver.manufacturer != 12) {
424 fprintf(stderr, "Can't find sniffer at hci%d: %s (%d)\n",
425 dev, strerror(ENOSYS), ENOSYS);
426 hci_close_dev(dd);
427 exit(1);
428 }
429
430 if (read_revision(dd, rev, sizeof(rev)) < 0) {
431 fprintf(stderr, "Can't read revision info for hci%d: %s (%d)\n",
432 dev, strerror(errno), errno);
433 hci_close_dev(dd);
434 exit(1);
435 }
436
437 printf("%s\n", rev);
438
439 if (enable_sniffer(dd, 0x01) < 0) {
440 fprintf(stderr, "Can't enable sniffer for hci%d: %s (%d)\n",
441 dev, strerror(errno), errno);
442 hci_close_dev(dd);
443 exit(1);
444 }
445
446 if (enable_sync(dd, 0x01, &bdaddr) < 0) {
447 fprintf(stderr, "Can't enable sync for hci%d: %s (%d)\n",
448 dev, strerror(errno), errno);
449 enable_sniffer(dd, 0x00);
450 hci_close_dev(dd);
451 exit(1);
452 }
453
454 init_parser(DUMP_EXT | DUMP_VERBOSE, ~0L, 0, DEFAULT_COMPID, -1, -1);
455
456 process_frames(dev);
457
458 if (enable_sync(dd, 0x00, &bdaddr) < 0) {
459 fprintf(stderr, "Can't disable sync for hci%d: %s (%d)\n",
460 dev, strerror(errno), errno);
461 enable_sniffer(dd, 0x00);
462 hci_close_dev(dd);
463 exit(1);
464 }
465
466 if (enable_sniffer(dd, 0x00) < 0) {
467 fprintf(stderr, "Can't disable sniffer for hci%d: %s (%d)\n",
468 dev, strerror(errno), errno);
469 hci_close_dev(dd);
470 exit(1);
471 }
472
473 hci_close_dev(dd);
474
475 return 0;
476 }
477