1 /* 2 * le.c 3 * 4 * Copyright (c) 2015 Takanori Watanabe <takawata@freebsd.org> 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions 9 * are met: 10 * 1. Redistributions of source code must retain the above copyright 11 * notice, this list of conditions and the following disclaimer. 12 * 2. Redistributions in binary form must reproduce the above copyright 13 * notice, this list of conditions and the following disclaimer in the 14 * documentation and/or other materials provided with the distribution. 15 * 16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 26 * SUCH DAMAGE. 27 * 28 * $Id: hccontrol.c,v 1.5 2003/09/05 00:38:24 max Exp $ 29 * $FreeBSD$ 30 */ 31 32 #include <sys/types.h> 33 #include <sys/ioctl.h> 34 #include <sys/sysctl.h> 35 #include <sys/select.h> 36 #include <assert.h> 37 #include <bitstring.h> 38 #include <err.h> 39 #include <errno.h> 40 #include <netgraph/ng_message.h> 41 #include <errno.h> 42 #include <stdbool.h> 43 #include <stdio.h> 44 #include <stdlib.h> 45 #include <string.h> 46 #include <unistd.h> 47 #include <stdint.h> 48 #define L2CAP_SOCKET_CHECKED 49 #include <bluetooth.h> 50 #include "hccontrol.h" 51 52 static int le_set_scan_param(int s, int argc, char *argv[]); 53 static int le_set_scan_enable(int s, int argc, char *argv[]); 54 static int parse_param(int argc, char *argv[], char *buf, int *len); 55 static int le_set_scan_response(int s, int argc, char *argv[]); 56 static int le_read_supported_states(int s, int argc, char *argv[]); 57 static int le_read_local_supported_features(int s, int argc ,char *argv[]); 58 static int set_le_event_mask(int s, uint64_t mask); 59 static int set_event_mask(int s, uint64_t mask); 60 static int le_enable(int s, int argc, char *argv[]); 61 static int le_set_advertising_enable(int s, int argc, char *argv[]); 62 static int le_set_advertising_param(int s, int argc, char *argv[]); 63 static int le_read_advertising_channel_tx_power(int s, int argc, char *argv[]); 64 static int le_scan(int s, int argc, char *argv[]); 65 static void handle_le_event(ng_hci_event_pkt_t* e, bool verbose); 66 static int le_read_white_list_size(int s, int argc, char *argv[]); 67 static int le_clear_white_list(int s, int argc, char *argv[]); 68 static int le_add_device_to_white_list(int s, int argc, char *argv[]); 69 static int le_remove_device_from_white_list(int s, int argc, char *argv[]); 70 71 static int 72 le_set_scan_param(int s, int argc, char *argv[]) 73 { 74 int type; 75 int interval; 76 int window; 77 int adrtype; 78 int policy; 79 int n; 80 81 ng_hci_le_set_scan_parameters_cp cp; 82 ng_hci_le_set_scan_parameters_rp rp; 83 84 if (argc != 5) 85 return (USAGE); 86 87 if (strcmp(argv[0], "active") == 0) 88 type = 1; 89 else if (strcmp(argv[0], "passive") == 0) 90 type = 0; 91 else 92 return (USAGE); 93 94 interval = (int)(atof(argv[1])/0.625); 95 interval = (interval < 4)? 4: interval; 96 window = (int)(atof(argv[2])/0.625); 97 window = (window < 4) ? 4 : interval; 98 99 if (strcmp(argv[3], "public") == 0) 100 adrtype = 0; 101 else if (strcmp(argv[3], "random") == 0) 102 adrtype = 1; 103 else 104 return (USAGE); 105 106 if (strcmp(argv[4], "all") == 0) 107 policy = 0; 108 else if (strcmp(argv[4], "whitelist") == 0) 109 policy = 1; 110 else 111 return (USAGE); 112 113 cp.le_scan_type = type; 114 cp.le_scan_interval = interval; 115 cp.own_address_type = adrtype; 116 cp.le_scan_window = window; 117 cp.scanning_filter_policy = policy; 118 n = sizeof(rp); 119 120 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 121 NG_HCI_OCF_LE_SET_SCAN_PARAMETERS), 122 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 123 return (ERROR); 124 125 if (rp.status != 0x00) { 126 fprintf(stdout, "Status: %s [%#02x]\n", 127 hci_status2str(rp.status), rp.status); 128 return (FAILED); 129 } 130 131 return (OK); 132 } 133 134 static int 135 le_set_scan_enable(int s, int argc, char *argv[]) 136 { 137 ng_hci_le_set_scan_enable_cp cp; 138 ng_hci_le_set_scan_enable_rp rp; 139 int n, enable = 0; 140 141 if (argc != 1) 142 return (USAGE); 143 144 if (strcmp(argv[0], "enable") == 0) 145 enable = 1; 146 else if (strcmp(argv[0], "disable") != 0) 147 return (USAGE); 148 149 n = sizeof(rp); 150 cp.le_scan_enable = enable; 151 cp.filter_duplicates = 0; 152 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 153 NG_HCI_OCF_LE_SET_SCAN_ENABLE), 154 (void *)&cp, sizeof(cp), 155 (void *)&rp, &n) == ERROR) 156 return (ERROR); 157 158 if (rp.status != 0x00) { 159 fprintf(stdout, "Status: %s [%#02x]\n", 160 hci_status2str(rp.status), rp.status); 161 return (FAILED); 162 } 163 164 fprintf(stdout, "LE Scan: %s\n", 165 enable? "Enabled" : "Disabled"); 166 167 return (OK); 168 } 169 170 static int 171 parse_param(int argc, char *argv[], char *buf, int *len) 172 { 173 char *buflast = buf + (*len); 174 char *curbuf = buf; 175 char *token,*lenpos; 176 int ch; 177 int datalen; 178 uint16_t value; 179 optreset = 1; 180 optind = 0; 181 while ((ch = getopt(argc, argv , "n:f:u:")) != -1) { 182 switch(ch){ 183 case 'n': 184 datalen = strlen(optarg); 185 if ((curbuf + datalen + 2) >= buflast) 186 goto done; 187 curbuf[0] = datalen + 1; 188 curbuf[1] = 8; 189 curbuf += 2; 190 memcpy(curbuf, optarg, datalen); 191 curbuf += datalen; 192 break; 193 case 'f': 194 if (curbuf+3 > buflast) 195 goto done; 196 curbuf[0] = 2; 197 curbuf[1] = 1; 198 curbuf[2] = (uint8_t)strtol(optarg, NULL, 16); 199 curbuf += 3; 200 break; 201 case 'u': 202 if ((buf+2) >= buflast) 203 goto done; 204 lenpos = curbuf; 205 curbuf[1] = 2; 206 *lenpos = 1; 207 curbuf += 2; 208 while ((token = strsep(&optarg, ",")) != NULL) { 209 value = strtol(token, NULL, 16); 210 if ((curbuf+2) >= buflast) 211 break; 212 curbuf[0] = value &0xff; 213 curbuf[1] = (value>>8)&0xff; 214 curbuf += 2; 215 *lenpos += 2; 216 } 217 218 } 219 } 220 done: 221 *len = curbuf - buf; 222 223 return (OK); 224 } 225 226 static int 227 le_set_scan_response(int s, int argc, char *argv[]) 228 { 229 ng_hci_le_set_scan_response_data_cp cp; 230 ng_hci_le_set_scan_response_data_rp rp; 231 int n; 232 int len; 233 char buf[NG_HCI_ADVERTISING_DATA_SIZE]; 234 235 len = sizeof(buf); 236 parse_param(argc, argv, buf, &len); 237 memset(cp.scan_response_data, 0, sizeof(cp.scan_response_data)); 238 cp.scan_response_data_length = len; 239 memcpy(cp.scan_response_data, buf, len); 240 n = sizeof(rp); 241 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 242 NG_HCI_OCF_LE_SET_SCAN_RESPONSE_DATA), 243 (void *)&cp, sizeof(cp), 244 (void *)&rp, &n) == ERROR) 245 return (ERROR); 246 247 if (rp.status != 0x00) { 248 fprintf(stdout, "Status: %s [%#02x]\n", 249 hci_status2str(rp.status), rp.status); 250 return (FAILED); 251 } 252 253 return (OK); 254 } 255 256 static int 257 le_read_local_supported_features(int s, int argc ,char *argv[]) 258 { 259 ng_hci_le_read_local_supported_features_rp rp; 260 int n = sizeof(rp); 261 262 union { 263 uint64_t raw; 264 uint8_t octets[8]; 265 } le_features; 266 267 char buffer[2048]; 268 269 if (hci_simple_request(s, 270 NG_HCI_OPCODE(NG_HCI_OGF_LE, 271 NG_HCI_OCF_LE_READ_LOCAL_SUPPORTED_FEATURES), 272 (void *)&rp, &n) == ERROR) 273 return (ERROR); 274 275 if (rp.status != 0x00) { 276 fprintf(stdout, "Status: %s [%#02x]\n", 277 hci_status2str(rp.status), rp.status); 278 return (FAILED); 279 } 280 281 le_features.raw = rp.le_features; 282 283 fprintf(stdout, "LE Features: "); 284 for(int i = 0; i < 8; i++) 285 fprintf(stdout, " %#02x", le_features.octets[i]); 286 fprintf(stdout, "\n%s\n", hci_le_features2str(le_features.octets, 287 buffer, sizeof(buffer))); 288 fprintf(stdout, "\n"); 289 290 return (OK); 291 } 292 293 static int 294 le_read_supported_states(int s, int argc, char *argv[]) 295 { 296 ng_hci_le_read_supported_states_rp rp; 297 int n = sizeof(rp); 298 299 if (hci_simple_request(s, NG_HCI_OPCODE( 300 NG_HCI_OGF_LE, 301 NG_HCI_OCF_LE_READ_SUPPORTED_STATES), 302 (void *)&rp, &n) == ERROR) 303 return (ERROR); 304 305 if (rp.status != 0x00) { 306 fprintf(stdout, "Status: %s [%#02x]\n", 307 hci_status2str(rp.status), rp.status); 308 return (FAILED); 309 } 310 311 fprintf(stdout, "LE States: %jx\n", rp.le_states); 312 313 return (OK); 314 } 315 316 static int 317 set_le_event_mask(int s, uint64_t mask) 318 { 319 ng_hci_le_set_event_mask_cp semc; 320 ng_hci_le_set_event_mask_rp rp; 321 int i, n; 322 323 n = sizeof(rp); 324 325 for (i=0; i < NG_HCI_LE_EVENT_MASK_SIZE; i++) { 326 semc.event_mask[i] = mask&0xff; 327 mask >>= 8; 328 } 329 if(hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 330 NG_HCI_OCF_LE_SET_EVENT_MASK), 331 (void *)&semc, sizeof(semc), (void *)&rp, &n) == ERROR) 332 return (ERROR); 333 334 if (rp.status != 0x00) { 335 fprintf(stdout, "Status: %s [%#02x]\n", 336 hci_status2str(rp.status), rp.status); 337 return (FAILED); 338 } 339 340 return (OK); 341 } 342 343 static int 344 set_event_mask(int s, uint64_t mask) 345 { 346 ng_hci_set_event_mask_cp semc; 347 ng_hci_set_event_mask_rp rp; 348 int i, n; 349 350 n = sizeof(rp); 351 352 for (i=0; i < NG_HCI_EVENT_MASK_SIZE; i++) { 353 semc.event_mask[i] = mask&0xff; 354 mask >>= 8; 355 } 356 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_HC_BASEBAND, 357 NG_HCI_OCF_SET_EVENT_MASK), 358 (void *)&semc, sizeof(semc), (void *)&rp, &n) == ERROR) 359 return (ERROR); 360 361 if (rp.status != 0x00) { 362 fprintf(stdout, "Status: %s [%#02x]\n", 363 hci_status2str(rp.status), rp.status); 364 return (FAILED); 365 } 366 367 return (OK); 368 } 369 370 static 371 int le_enable(int s, int argc, char *argv[]) 372 { 373 int result; 374 375 if (argc != 1) 376 return (USAGE); 377 378 if (strcasecmp(argv[0], "enable") == 0) { 379 result = set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT | 380 NG_HCI_EVENT_MASK_LE); 381 if (result != OK) 382 return result; 383 result = set_le_event_mask(s, NG_HCI_LE_EVENT_MASK_ALL); 384 if (result == OK) { 385 fprintf(stdout, "LE enabled\n"); 386 return (OK); 387 } else 388 return result; 389 } else if (strcasecmp(argv[0], "disable") == 0) { 390 result = set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT); 391 if (result == OK) { 392 fprintf(stdout, "LE disabled\n"); 393 return (OK); 394 } else 395 return result; 396 } else 397 return (USAGE); 398 } 399 400 static int 401 le_set_advertising_enable(int s, int argc, char *argv[]) 402 { 403 ng_hci_le_set_advertise_enable_cp cp; 404 ng_hci_le_set_advertise_enable_rp rp; 405 int n, enable = 0; 406 407 if (argc != 1) 408 return USAGE; 409 410 if (strcmp(argv[0], "enable") == 0) 411 enable = 1; 412 else if (strcmp(argv[0], "disable") != 0) 413 return USAGE; 414 415 n = sizeof(rp); 416 cp.advertising_enable = enable; 417 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 418 NG_HCI_OCF_LE_SET_ADVERTISE_ENABLE), 419 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 420 return (ERROR); 421 422 if (rp.status != 0x00) { 423 fprintf(stdout, "Status: %s [%#02x]\n", 424 hci_status2str(rp.status), rp.status); 425 return (FAILED); 426 } 427 fprintf(stdout, "LE Advertising %s\n", (enable ? "enabled" : "disabled")); 428 429 return (OK); 430 } 431 432 static int 433 le_set_advertising_param(int s, int argc, char *argv[]) 434 { 435 ng_hci_le_set_advertising_parameters_cp cp; 436 ng_hci_le_set_advertising_parameters_rp rp; 437 438 int n, ch; 439 440 cp.advertising_interval_min = 0x800; 441 cp.advertising_interval_max = 0x800; 442 cp.advertising_type = 0; 443 cp.own_address_type = 0; 444 cp.direct_address_type = 0; 445 446 cp.advertising_channel_map = 7; 447 cp.advertising_filter_policy = 0; 448 449 optreset = 1; 450 optind = 0; 451 while ((ch = getopt(argc, argv , "m:M:t:o:p:a:c:f:")) != -1) { 452 switch(ch) { 453 case 'm': 454 cp.advertising_interval_min = 455 (uint16_t)(strtod(optarg, NULL)/0.625); 456 break; 457 case 'M': 458 cp.advertising_interval_max = 459 (uint16_t)(strtod(optarg, NULL)/0.625); 460 break; 461 case 't': 462 cp.advertising_type = 463 (uint8_t)strtod(optarg, NULL); 464 break; 465 case 'o': 466 cp.own_address_type = 467 (uint8_t)strtod(optarg, NULL); 468 break; 469 case 'p': 470 cp.direct_address_type = 471 (uint8_t)strtod(optarg, NULL); 472 break; 473 case 'a': 474 if (!bt_aton(optarg, &cp.direct_address)) { 475 struct hostent *he = NULL; 476 477 if ((he = bt_gethostbyname(optarg)) == NULL) 478 return (USAGE); 479 480 memcpy(&cp.direct_address, he->h_addr, sizeof(cp.direct_address)); 481 } 482 break; 483 case 'c': 484 cp.advertising_channel_map = 485 (uint8_t)strtod(optarg, NULL); 486 break; 487 case 'f': 488 cp.advertising_filter_policy = 489 (uint8_t)strtod(optarg, NULL); 490 break; 491 } 492 } 493 494 n = sizeof(rp); 495 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 496 NG_HCI_OCF_LE_SET_ADVERTISING_PARAMETERS), 497 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 498 return (ERROR); 499 500 if (rp.status != 0x00) { 501 fprintf(stdout, "Status: %s [%#02x]\n", 502 hci_status2str(rp.status), rp.status); 503 return (FAILED); 504 } 505 506 return (OK); 507 } 508 509 static int 510 le_read_advertising_channel_tx_power(int s, int argc, char *argv[]) 511 { 512 ng_hci_le_read_advertising_channel_tx_power_rp rp; 513 int n; 514 515 n = sizeof(rp); 516 517 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 518 NG_HCI_OCF_LE_READ_ADVERTISING_CHANNEL_TX_POWER), 519 (void *)&rp, &n) == ERROR) 520 return (ERROR); 521 522 if (rp.status != 0x00) { 523 fprintf(stdout, "Status: %s [%#02x]\n", 524 hci_status2str(rp.status), rp.status); 525 return (FAILED); 526 } 527 528 fprintf(stdout, "Advertising transmit power level: %d dBm\n", 529 (int8_t)rp.transmit_power_level); 530 531 return (OK); 532 } 533 534 static int 535 le_set_advertising_data(int s, int argc, char *argv[]) 536 { 537 ng_hci_le_set_advertising_data_cp cp; 538 ng_hci_le_set_advertising_data_rp rp; 539 int n, len; 540 541 n = sizeof(rp); 542 543 char buf[NG_HCI_ADVERTISING_DATA_SIZE]; 544 545 len = sizeof(buf); 546 parse_param(argc, argv, buf, &len); 547 memset(cp.advertising_data, 0, sizeof(cp.advertising_data)); 548 cp.advertising_data_length = len; 549 memcpy(cp.advertising_data, buf, len); 550 551 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 552 NG_HCI_OCF_LE_SET_ADVERTISING_DATA), 553 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 554 return (ERROR); 555 556 if (rp.status != 0x00) { 557 fprintf(stdout, "Status: %s [%#02x]\n", 558 hci_status2str(rp.status), rp.status); 559 return (FAILED); 560 } 561 562 return (OK); 563 } 564 static int 565 le_read_buffer_size(int s, int argc, char *argv[]) 566 { 567 union { 568 ng_hci_le_read_buffer_size_rp v1; 569 ng_hci_le_read_buffer_size_rp_v2 v2; 570 } rp; 571 572 int n, ch; 573 uint8_t v; 574 uint16_t cmd; 575 576 optreset = 1; 577 optind = 0; 578 579 /* Default to version 1*/ 580 v = 1; 581 cmd = NG_HCI_OCF_LE_READ_BUFFER_SIZE; 582 583 while ((ch = getopt(argc, argv , "v:")) != -1) { 584 switch(ch) { 585 case 'v': 586 v = (uint8_t)strtol(optarg, NULL, 16); 587 if (v == 2) 588 cmd = NG_HCI_OCF_LE_READ_BUFFER_SIZE_V2; 589 else if (v > 2) 590 return (USAGE); 591 break; 592 default: 593 v = 1; 594 } 595 } 596 597 n = sizeof(rp); 598 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, cmd), 599 (void *)&rp, &n) == ERROR) 600 return (ERROR); 601 602 if (rp.v1.status != 0x00) { 603 fprintf(stdout, "Status: %s [%#02x]\n", 604 hci_status2str(rp.v1.status), rp.v1.status); 605 return (FAILED); 606 } 607 608 fprintf(stdout, "ACL data packet length: %d\n", 609 rp.v1.hc_le_data_packet_length); 610 fprintf(stdout, "Number of ACL data packets: %d\n", 611 rp.v1.hc_total_num_le_data_packets); 612 613 if (v == 2) { 614 fprintf(stdout, "ISO data packet length: %d\n", 615 rp.v2.hc_iso_data_packet_length); 616 fprintf(stdout, "Number of ISO data packets: %d\n", 617 rp.v2.hc_total_num_iso_data_packets); 618 } 619 620 return (OK); 621 } 622 623 static int 624 le_scan(int s, int argc, char *argv[]) 625 { 626 int n, bufsize, scancount, numscans; 627 bool verbose; 628 uint8_t active = 0; 629 char ch; 630 631 char b[512]; 632 ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b; 633 634 ng_hci_le_set_scan_parameters_cp scan_param_cp; 635 ng_hci_le_set_scan_parameters_rp scan_param_rp; 636 637 ng_hci_le_set_scan_enable_cp scan_enable_cp; 638 ng_hci_le_set_scan_enable_rp scan_enable_rp; 639 640 optreset = 1; 641 optind = 0; 642 verbose = false; 643 numscans = 1; 644 645 while ((ch = getopt(argc, argv , "an:v")) != -1) { 646 switch(ch) { 647 case 'a': 648 active = 1; 649 break; 650 case 'n': 651 numscans = (uint8_t)strtol(optarg, NULL, 10); 652 break; 653 case 'v': 654 verbose = true; 655 break; 656 } 657 } 658 659 scan_param_cp.le_scan_type = active; 660 scan_param_cp.le_scan_interval = (uint16_t)(100/0.625); 661 scan_param_cp.le_scan_window = (uint16_t)(50/0.625); 662 /* Address type public */ 663 scan_param_cp.own_address_type = 0; 664 /* 'All' filter policy */ 665 scan_param_cp.scanning_filter_policy = 0; 666 n = sizeof(scan_param_rp); 667 668 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 669 NG_HCI_OCF_LE_SET_SCAN_PARAMETERS), 670 (void *)&scan_param_cp, sizeof(scan_param_cp), 671 (void *)&scan_param_rp, &n) == ERROR) 672 return (ERROR); 673 674 if (scan_param_rp.status != 0x00) { 675 fprintf(stdout, "LE_Set_Scan_Parameters failed. Status: %s [%#02x]\n", 676 hci_status2str(scan_param_rp.status), 677 scan_param_rp.status); 678 return (FAILED); 679 } 680 681 /* Enable scanning */ 682 n = sizeof(scan_enable_rp); 683 scan_enable_cp.le_scan_enable = 1; 684 scan_enable_cp.filter_duplicates = 1; 685 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 686 NG_HCI_OCF_LE_SET_SCAN_ENABLE), 687 (void *)&scan_enable_cp, sizeof(scan_enable_cp), 688 (void *)&scan_enable_rp, &n) == ERROR) 689 return (ERROR); 690 691 if (scan_enable_rp.status != 0x00) { 692 fprintf(stdout, "LE_Scan_Enable enable failed. Status: %s [%#02x]\n", 693 hci_status2str(scan_enable_rp.status), 694 scan_enable_rp.status); 695 return (FAILED); 696 } 697 698 scancount = 0; 699 while (scancount < numscans) { 700 /* wait for scan events */ 701 bufsize = sizeof(b); 702 if (hci_recv(s, b, &bufsize) == ERROR) { 703 return (ERROR); 704 } 705 706 if (bufsize < sizeof(*e)) { 707 errno = EIO; 708 return (ERROR); 709 } 710 scancount++; 711 if (e->event == NG_HCI_EVENT_LE) { 712 fprintf(stdout, "Scan %d\n", scancount); 713 handle_le_event(e, verbose); 714 } 715 } 716 717 fprintf(stdout, "Scan complete\n"); 718 719 /* Disable scanning */ 720 n = sizeof(scan_enable_rp); 721 scan_enable_cp.le_scan_enable = 0; 722 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 723 NG_HCI_OCF_LE_SET_SCAN_ENABLE), 724 (void *)&scan_enable_cp, sizeof(scan_enable_cp), 725 (void *)&scan_enable_rp, &n) == ERROR) 726 return (ERROR); 727 728 if (scan_enable_rp.status != 0x00) { 729 fprintf(stdout, "LE_Scan_Enable disable failed. Status: %s [%#02x]\n", 730 hci_status2str(scan_enable_rp.status), 731 scan_enable_rp.status); 732 return (FAILED); 733 } 734 735 return (OK); 736 } 737 738 static void handle_le_event(ng_hci_event_pkt_t* e, bool verbose) 739 { 740 int rc; 741 ng_hci_le_ep *leer = 742 (ng_hci_le_ep *)(e + 1); 743 ng_hci_le_advertising_report_ep *advrep = 744 (ng_hci_le_advertising_report_ep *)(leer + 1); 745 ng_hci_le_advreport *reports = 746 (ng_hci_le_advreport *)(advrep + 1); 747 748 if (leer->subevent_code == NG_HCI_LEEV_ADVREP) { 749 fprintf(stdout, "Scan result, num_reports: %d\n", 750 advrep->num_reports); 751 for(rc = 0; rc < advrep->num_reports; rc++) { 752 uint8_t length = (uint8_t)reports[rc].length_data; 753 fprintf(stdout, "\tBD_ADDR %s \n", 754 hci_bdaddr2str(&reports[rc].bdaddr)); 755 fprintf(stdout, "\tAddress type: %s\n", 756 hci_addrtype2str(reports[rc].addr_type)); 757 if (length > 0 && verbose) { 758 dump_adv_data(length, reports[rc].data); 759 print_adv_data(length, reports[rc].data); 760 fprintf(stdout, 761 "\tRSSI: %d dBm\n", 762 (int8_t)reports[rc].data[length]); 763 fprintf(stdout, "\n"); 764 } 765 } 766 } 767 } 768 769 static int 770 le_read_white_list_size(int s, int argc, char *argv[]) 771 { 772 ng_hci_le_read_white_list_size_rp rp; 773 int n; 774 775 n = sizeof(rp); 776 777 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 778 NG_HCI_OCF_LE_READ_WHITE_LIST_SIZE), 779 (void *)&rp, &n) == ERROR) 780 return (ERROR); 781 782 if (rp.status != 0x00) { 783 fprintf(stdout, "Status: %s [%#02x]\n", 784 hci_status2str(rp.status), rp.status); 785 return (FAILED); 786 } 787 788 fprintf(stdout, "White list size: %d\n", 789 (uint8_t)rp.white_list_size); 790 791 return (OK); 792 } 793 794 static int 795 le_clear_white_list(int s, int argc, char *argv[]) 796 { 797 ng_hci_le_clear_white_list_rp rp; 798 int n; 799 800 n = sizeof(rp); 801 802 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 803 NG_HCI_OCF_LE_CLEAR_WHITE_LIST), 804 (void *)&rp, &n) == ERROR) 805 return (ERROR); 806 807 if (rp.status != 0x00) { 808 fprintf(stdout, "Status: %s [%#02x]\n", 809 hci_status2str(rp.status), rp.status); 810 return (FAILED); 811 } 812 813 fprintf(stdout, "White list cleared\n"); 814 815 return (OK); 816 } 817 818 static int 819 le_add_device_to_white_list(int s, int argc, char *argv[]) 820 { 821 ng_hci_le_add_device_to_white_list_cp cp; 822 ng_hci_le_add_device_to_white_list_rp rp; 823 int n; 824 char ch; 825 optreset = 1; 826 optind = 0; 827 bool addr_set = false; 828 829 n = sizeof(rp); 830 831 cp.address_type = 0x00; 832 833 while ((ch = getopt(argc, argv , "t:a:")) != -1) { 834 switch(ch) { 835 case 't': 836 if (strcmp(optarg, "public") == 0) 837 cp.address_type = 0x00; 838 else if (strcmp(optarg, "random") == 0) 839 cp.address_type = 0x01; 840 else 841 return (USAGE); 842 break; 843 case 'a': 844 addr_set = true; 845 if (!bt_aton(optarg, &cp.address)) { 846 struct hostent *he = NULL; 847 848 if ((he = bt_gethostbyname(optarg)) == NULL) 849 return (USAGE); 850 851 memcpy(&cp.address, he->h_addr, 852 sizeof(cp.address)); 853 } 854 break; 855 } 856 } 857 858 if (addr_set == false) 859 return (USAGE); 860 861 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 862 NG_HCI_OCF_LE_ADD_DEVICE_TO_WHITE_LIST), 863 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 864 return (ERROR); 865 866 if (rp.status != 0x00) { 867 fprintf(stdout, "Status: %s [%#02x]\n", 868 hci_status2str(rp.status), rp.status); 869 return (FAILED); 870 } 871 872 fprintf(stdout, "Address added to white list\n"); 873 874 return (OK); 875 } 876 877 static int 878 le_remove_device_from_white_list(int s, int argc, char *argv[]) 879 { 880 ng_hci_le_remove_device_from_white_list_cp cp; 881 ng_hci_le_remove_device_from_white_list_rp rp; 882 int n; 883 char ch; 884 optreset = 1; 885 optind = 0; 886 bool addr_set = false; 887 888 n = sizeof(rp); 889 890 cp.address_type = 0x00; 891 892 while ((ch = getopt(argc, argv , "t:a:")) != -1) { 893 switch(ch) { 894 case 't': 895 if (strcmp(optarg, "public") == 0) 896 cp.address_type = 0x00; 897 else if (strcmp(optarg, "random") == 0) 898 cp.address_type = 0x01; 899 else 900 return (USAGE); 901 break; 902 case 'a': 903 addr_set = true; 904 if (!bt_aton(optarg, &cp.address)) { 905 struct hostent *he = NULL; 906 907 if ((he = bt_gethostbyname(optarg)) == NULL) 908 return (USAGE); 909 910 memcpy(&cp.address, he->h_addr, 911 sizeof(cp.address)); 912 } 913 break; 914 } 915 } 916 917 if (addr_set == false) 918 return (USAGE); 919 920 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 921 NG_HCI_OCF_LE_ADD_DEVICE_TO_WHITE_LIST), 922 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 923 return (ERROR); 924 925 if (rp.status != 0x00) { 926 fprintf(stdout, "Status: %s [%#02x]\n", 927 hci_status2str(rp.status), rp.status); 928 return (FAILED); 929 } 930 931 fprintf(stdout, "Address removed from white list\n"); 932 933 return (OK); 934 } 935 936 struct hci_command le_commands[] = { 937 { 938 "le_enable", 939 "le_enable [enable|disable] \n" 940 "Enable LE event ", 941 &le_enable, 942 }, 943 { 944 "le_read_local_supported_features", 945 "le_read_local_supported_features\n" 946 "read local supported features mask", 947 &le_read_local_supported_features, 948 }, 949 { 950 "le_read_supported_states", 951 "le_read_supported_states\n" 952 "read supported status" 953 , 954 &le_read_supported_states, 955 }, 956 { 957 "le_set_scan_response", 958 "le_set_scan_response -n $name -f $flag -u $uuid16,$uuid16 \n" 959 "set LE scan response data" 960 , 961 &le_set_scan_response, 962 }, 963 { 964 "le_set_scan_enable", 965 "le_set_scan_enable [enable|disable] \n" 966 "enable or disable LE device scan", 967 &le_set_scan_enable 968 }, 969 { 970 "le_set_scan_param", 971 "le_set_scan_param [active|passive] interval(ms) window(ms) [public|random] [all|whitelist] \n" 972 "set LE device scan parameter", 973 &le_set_scan_param 974 }, 975 { 976 "le_set_advertising_enable", 977 "le_set_advertising_enable [enable|disable] \n" 978 "start or stop advertising", 979 &le_set_advertising_enable 980 }, 981 { 982 "le_read_advertising_channel_tx_power", 983 "le_read_advertising_channel_tx_power\n" 984 "read host advertising transmit poser level (dBm)", 985 &le_read_advertising_channel_tx_power 986 }, 987 { 988 "le_set_advertising_param", 989 "le_set_advertising_param [-m min_interval(ms)] [-M max_interval(ms)]\n" 990 "[-t advertising_type] [-o own_address_type] [-p peer_address_type]\n" 991 "[-c advertising_channel_map] [-f advertising_filter_policy]\n" 992 "[-a peer_address]\n" 993 "set LE device advertising parameters", 994 &le_set_advertising_param 995 }, 996 { 997 "le_set_advertising_data", 998 "le_set_advertising_data -n $name -f $flag -u $uuid16,$uuid16 \n" 999 "set LE device advertising packed data", 1000 &le_set_advertising_data 1001 }, 1002 { 1003 "le_read_buffer_size", 1004 "le_read_buffer_size [-v 1|2]\n" 1005 "Read the maximum size of ACL and ISO data packets", 1006 &le_read_buffer_size 1007 }, 1008 { 1009 "le_scan", 1010 "le_scan [-a] [-v] [-n number_of_scans]\n" 1011 "Do an LE scan", 1012 &le_scan 1013 }, 1014 { 1015 "le_read_white_list_size", 1016 "le_read_white_list_size\n" 1017 "Read total number of white list entries that can be stored", 1018 &le_read_white_list_size 1019 }, 1020 { 1021 "le_clear_white_list", 1022 "le_clear_white_list\n" 1023 "Clear the white list in the controller", 1024 &le_clear_white_list 1025 }, 1026 { 1027 "le_add_device_to_white_list", 1028 "le_add_device_to_white_list\n" 1029 "[-t public|random] -a address\n" 1030 "Add device to the white list", 1031 &le_add_device_to_white_list 1032 }, 1033 { 1034 "le_remove_device_from_white_list", 1035 "le_remove_device_from_white_list\n" 1036 "[-t public|random] -a address\n" 1037 "Remove device from the white list", 1038 &le_remove_device_from_white_list 1039 }, 1040 }; 1041