bpasniff.c revision 4b24738bac78ba8dca229aabb5735cf9df123abc 
1/*
2 *
3 *  BlueZ - Bluetooth protocol stack for Linux
4 *
5 *  Copyright (C) 2004-2010  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
47static volatile sig_atomic_t __io_canceled = 0;
48
49static void sig_hup(int sig)
50{
51}
52
53static void sig_term(int sig)
54{
55	__io_canceled = 1;
56}
57
58static 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
86static 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
111static 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
131static 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
173static 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
262static 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
347static 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
354static struct option main_options[] = {
355	{ "help",	0, 0, 'h' },
356	{ "device",	1, 0, 'i' },
357	{ 0, 0, 0, 0}
358};
359
360int 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