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
