Home | History | Annotate | Download | only in src
      1 /*
      2  *
      3  *  BlueZ - Bluetooth protocol stack for Linux
      4  *
      5  *  Copyright (C) 2004-2011  Marcel Holtmann <marcel (at) 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 
     49 static void sig_hup(int sig)
     50 {
     51 }
     52 
     53 static void sig_term(int sig)
     54 {
     55 	__io_canceled = 1;
     56 }
     57 
     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 
     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 
    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 
    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 
    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 
    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 
    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 
    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