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 static int le_connect(int s, int argc, char *argv[]); 71 static void handle_le_connection_event(ng_hci_event_pkt_t* e, bool verbose); 72 static int le_read_channel_map(int s, int argc, char *argv[]); 73 static void handle_le_remote_features_event(ng_hci_event_pkt_t* e); 74 static int le_rand(int s, int argc, char *argv[]); 75 76 static int 77 le_set_scan_param(int s, int argc, char *argv[]) 78 { 79 int type; 80 int interval; 81 int window; 82 int adrtype; 83 int policy; 84 int n; 85 86 ng_hci_le_set_scan_parameters_cp cp; 87 ng_hci_le_set_scan_parameters_rp rp; 88 89 if (argc != 5) 90 return (USAGE); 91 92 if (strcmp(argv[0], "active") == 0) 93 type = 1; 94 else if (strcmp(argv[0], "passive") == 0) 95 type = 0; 96 else 97 return (USAGE); 98 99 interval = (int)(atof(argv[1])/0.625); 100 interval = (interval < 4)? 4: interval; 101 window = (int)(atof(argv[2])/0.625); 102 window = (window < 4) ? 4 : interval; 103 104 if (strcmp(argv[3], "public") == 0) 105 adrtype = 0; 106 else if (strcmp(argv[3], "random") == 0) 107 adrtype = 1; 108 else 109 return (USAGE); 110 111 if (strcmp(argv[4], "all") == 0) 112 policy = 0; 113 else if (strcmp(argv[4], "whitelist") == 0) 114 policy = 1; 115 else 116 return (USAGE); 117 118 cp.le_scan_type = type; 119 cp.le_scan_interval = interval; 120 cp.own_address_type = adrtype; 121 cp.le_scan_window = window; 122 cp.scanning_filter_policy = policy; 123 n = sizeof(rp); 124 125 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 126 NG_HCI_OCF_LE_SET_SCAN_PARAMETERS), 127 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 128 return (ERROR); 129 130 if (rp.status != 0x00) { 131 fprintf(stdout, "Status: %s [%#02x]\n", 132 hci_status2str(rp.status), rp.status); 133 return (FAILED); 134 } 135 136 return (OK); 137 } 138 139 static int 140 le_set_scan_enable(int s, int argc, char *argv[]) 141 { 142 ng_hci_le_set_scan_enable_cp cp; 143 ng_hci_le_set_scan_enable_rp rp; 144 int n, enable = 0; 145 146 if (argc != 1) 147 return (USAGE); 148 149 if (strcmp(argv[0], "enable") == 0) 150 enable = 1; 151 else if (strcmp(argv[0], "disable") != 0) 152 return (USAGE); 153 154 n = sizeof(rp); 155 cp.le_scan_enable = enable; 156 cp.filter_duplicates = 0; 157 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 158 NG_HCI_OCF_LE_SET_SCAN_ENABLE), 159 (void *)&cp, sizeof(cp), 160 (void *)&rp, &n) == ERROR) 161 return (ERROR); 162 163 if (rp.status != 0x00) { 164 fprintf(stdout, "Status: %s [%#02x]\n", 165 hci_status2str(rp.status), rp.status); 166 return (FAILED); 167 } 168 169 fprintf(stdout, "LE Scan: %s\n", 170 enable? "Enabled" : "Disabled"); 171 172 return (OK); 173 } 174 175 static int 176 parse_param(int argc, char *argv[], char *buf, int *len) 177 { 178 char *buflast = buf + (*len); 179 char *curbuf = buf; 180 char *token,*lenpos; 181 int ch; 182 int datalen; 183 uint16_t value; 184 optreset = 1; 185 optind = 0; 186 while ((ch = getopt(argc, argv , "n:f:u:")) != -1) { 187 switch(ch){ 188 case 'n': 189 datalen = strlen(optarg); 190 if ((curbuf + datalen + 2) >= buflast) 191 goto done; 192 curbuf[0] = datalen + 1; 193 curbuf[1] = 8; 194 curbuf += 2; 195 memcpy(curbuf, optarg, datalen); 196 curbuf += datalen; 197 break; 198 case 'f': 199 if (curbuf+3 > buflast) 200 goto done; 201 curbuf[0] = 2; 202 curbuf[1] = 1; 203 curbuf[2] = (uint8_t)strtol(optarg, NULL, 16); 204 curbuf += 3; 205 break; 206 case 'u': 207 if ((buf+2) >= buflast) 208 goto done; 209 lenpos = curbuf; 210 curbuf[1] = 2; 211 *lenpos = 1; 212 curbuf += 2; 213 while ((token = strsep(&optarg, ",")) != NULL) { 214 value = strtol(token, NULL, 16); 215 if ((curbuf+2) >= buflast) 216 break; 217 curbuf[0] = value &0xff; 218 curbuf[1] = (value>>8)&0xff; 219 curbuf += 2; 220 *lenpos += 2; 221 } 222 223 } 224 } 225 done: 226 *len = curbuf - buf; 227 228 return (OK); 229 } 230 231 static int 232 le_set_scan_response(int s, int argc, char *argv[]) 233 { 234 ng_hci_le_set_scan_response_data_cp cp; 235 ng_hci_le_set_scan_response_data_rp rp; 236 int n; 237 int len; 238 char buf[NG_HCI_ADVERTISING_DATA_SIZE]; 239 240 len = sizeof(buf); 241 parse_param(argc, argv, buf, &len); 242 memset(cp.scan_response_data, 0, sizeof(cp.scan_response_data)); 243 cp.scan_response_data_length = len; 244 memcpy(cp.scan_response_data, buf, len); 245 n = sizeof(rp); 246 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 247 NG_HCI_OCF_LE_SET_SCAN_RESPONSE_DATA), 248 (void *)&cp, sizeof(cp), 249 (void *)&rp, &n) == ERROR) 250 return (ERROR); 251 252 if (rp.status != 0x00) { 253 fprintf(stdout, "Status: %s [%#02x]\n", 254 hci_status2str(rp.status), rp.status); 255 return (FAILED); 256 } 257 258 return (OK); 259 } 260 261 static int 262 le_read_local_supported_features(int s, int argc ,char *argv[]) 263 { 264 ng_hci_le_read_local_supported_features_rp rp; 265 int n = sizeof(rp); 266 267 union { 268 uint64_t raw; 269 uint8_t octets[8]; 270 } le_features; 271 272 char buffer[2048]; 273 274 if (hci_simple_request(s, 275 NG_HCI_OPCODE(NG_HCI_OGF_LE, 276 NG_HCI_OCF_LE_READ_LOCAL_SUPPORTED_FEATURES), 277 (void *)&rp, &n) == ERROR) 278 return (ERROR); 279 280 if (rp.status != 0x00) { 281 fprintf(stdout, "Status: %s [%#02x]\n", 282 hci_status2str(rp.status), rp.status); 283 return (FAILED); 284 } 285 286 le_features.raw = rp.le_features; 287 288 fprintf(stdout, "LE Features: "); 289 for(int i = 0; i < 8; i++) 290 fprintf(stdout, " %#02x", le_features.octets[i]); 291 fprintf(stdout, "\n%s\n", hci_le_features2str(le_features.octets, 292 buffer, sizeof(buffer))); 293 fprintf(stdout, "\n"); 294 295 return (OK); 296 } 297 298 static int 299 le_read_supported_states(int s, int argc, char *argv[]) 300 { 301 ng_hci_le_read_supported_states_rp rp; 302 int n = sizeof(rp); 303 304 if (hci_simple_request(s, NG_HCI_OPCODE( 305 NG_HCI_OGF_LE, 306 NG_HCI_OCF_LE_READ_SUPPORTED_STATES), 307 (void *)&rp, &n) == ERROR) 308 return (ERROR); 309 310 if (rp.status != 0x00) { 311 fprintf(stdout, "Status: %s [%#02x]\n", 312 hci_status2str(rp.status), rp.status); 313 return (FAILED); 314 } 315 316 fprintf(stdout, "LE States: %jx\n", rp.le_states); 317 318 return (OK); 319 } 320 321 static int 322 set_le_event_mask(int s, uint64_t mask) 323 { 324 ng_hci_le_set_event_mask_cp semc; 325 ng_hci_le_set_event_mask_rp rp; 326 int i, n; 327 328 n = sizeof(rp); 329 330 for (i=0; i < NG_HCI_LE_EVENT_MASK_SIZE; i++) { 331 semc.event_mask[i] = mask&0xff; 332 mask >>= 8; 333 } 334 if(hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 335 NG_HCI_OCF_LE_SET_EVENT_MASK), 336 (void *)&semc, sizeof(semc), (void *)&rp, &n) == ERROR) 337 return (ERROR); 338 339 if (rp.status != 0x00) { 340 fprintf(stdout, "Status: %s [%#02x]\n", 341 hci_status2str(rp.status), rp.status); 342 return (FAILED); 343 } 344 345 return (OK); 346 } 347 348 static int 349 set_event_mask(int s, uint64_t mask) 350 { 351 ng_hci_set_event_mask_cp semc; 352 ng_hci_set_event_mask_rp rp; 353 int i, n; 354 355 n = sizeof(rp); 356 357 for (i=0; i < NG_HCI_EVENT_MASK_SIZE; i++) { 358 semc.event_mask[i] = mask&0xff; 359 mask >>= 8; 360 } 361 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_HC_BASEBAND, 362 NG_HCI_OCF_SET_EVENT_MASK), 363 (void *)&semc, sizeof(semc), (void *)&rp, &n) == ERROR) 364 return (ERROR); 365 366 if (rp.status != 0x00) { 367 fprintf(stdout, "Status: %s [%#02x]\n", 368 hci_status2str(rp.status), rp.status); 369 return (FAILED); 370 } 371 372 return (OK); 373 } 374 375 static 376 int le_enable(int s, int argc, char *argv[]) 377 { 378 int result; 379 380 if (argc != 1) 381 return (USAGE); 382 383 if (strcasecmp(argv[0], "enable") == 0) { 384 result = set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT | 385 NG_HCI_EVENT_MASK_LE); 386 if (result != OK) 387 return result; 388 result = set_le_event_mask(s, NG_HCI_LE_EVENT_MASK_ALL); 389 if (result == OK) { 390 fprintf(stdout, "LE enabled\n"); 391 return (OK); 392 } else 393 return result; 394 } else if (strcasecmp(argv[0], "disable") == 0) { 395 result = set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT); 396 if (result == OK) { 397 fprintf(stdout, "LE disabled\n"); 398 return (OK); 399 } else 400 return result; 401 } else 402 return (USAGE); 403 } 404 405 static int 406 le_set_advertising_enable(int s, int argc, char *argv[]) 407 { 408 ng_hci_le_set_advertise_enable_cp cp; 409 ng_hci_le_set_advertise_enable_rp rp; 410 int n, enable = 0; 411 412 if (argc != 1) 413 return USAGE; 414 415 if (strcmp(argv[0], "enable") == 0) 416 enable = 1; 417 else if (strcmp(argv[0], "disable") != 0) 418 return USAGE; 419 420 n = sizeof(rp); 421 cp.advertising_enable = enable; 422 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 423 NG_HCI_OCF_LE_SET_ADVERTISE_ENABLE), 424 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 425 return (ERROR); 426 427 if (rp.status != 0x00) { 428 fprintf(stdout, "Status: %s [%#02x]\n", 429 hci_status2str(rp.status), rp.status); 430 return (FAILED); 431 } 432 fprintf(stdout, "LE Advertising %s\n", (enable ? "enabled" : "disabled")); 433 434 return (OK); 435 } 436 437 static int 438 le_set_advertising_param(int s, int argc, char *argv[]) 439 { 440 ng_hci_le_set_advertising_parameters_cp cp; 441 ng_hci_le_set_advertising_parameters_rp rp; 442 443 int n, ch; 444 445 cp.advertising_interval_min = 0x800; 446 cp.advertising_interval_max = 0x800; 447 cp.advertising_type = 0; 448 cp.own_address_type = 0; 449 cp.direct_address_type = 0; 450 451 cp.advertising_channel_map = 7; 452 cp.advertising_filter_policy = 0; 453 454 optreset = 1; 455 optind = 0; 456 while ((ch = getopt(argc, argv , "m:M:t:o:p:a:c:f:")) != -1) { 457 switch(ch) { 458 case 'm': 459 cp.advertising_interval_min = 460 (uint16_t)(strtod(optarg, NULL)/0.625); 461 break; 462 case 'M': 463 cp.advertising_interval_max = 464 (uint16_t)(strtod(optarg, NULL)/0.625); 465 break; 466 case 't': 467 cp.advertising_type = 468 (uint8_t)strtod(optarg, NULL); 469 break; 470 case 'o': 471 cp.own_address_type = 472 (uint8_t)strtod(optarg, NULL); 473 break; 474 case 'p': 475 cp.direct_address_type = 476 (uint8_t)strtod(optarg, NULL); 477 break; 478 case 'a': 479 if (!bt_aton(optarg, &cp.direct_address)) { 480 struct hostent *he = NULL; 481 482 if ((he = bt_gethostbyname(optarg)) == NULL) 483 return (USAGE); 484 485 memcpy(&cp.direct_address, he->h_addr, sizeof(cp.direct_address)); 486 } 487 break; 488 case 'c': 489 cp.advertising_channel_map = 490 (uint8_t)strtod(optarg, NULL); 491 break; 492 case 'f': 493 cp.advertising_filter_policy = 494 (uint8_t)strtod(optarg, NULL); 495 break; 496 } 497 } 498 499 n = sizeof(rp); 500 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 501 NG_HCI_OCF_LE_SET_ADVERTISING_PARAMETERS), 502 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 503 return (ERROR); 504 505 if (rp.status != 0x00) { 506 fprintf(stdout, "Status: %s [%#02x]\n", 507 hci_status2str(rp.status), rp.status); 508 return (FAILED); 509 } 510 511 return (OK); 512 } 513 514 static int 515 le_read_advertising_channel_tx_power(int s, int argc, char *argv[]) 516 { 517 ng_hci_le_read_advertising_channel_tx_power_rp rp; 518 int n; 519 520 n = sizeof(rp); 521 522 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 523 NG_HCI_OCF_LE_READ_ADVERTISING_CHANNEL_TX_POWER), 524 (void *)&rp, &n) == ERROR) 525 return (ERROR); 526 527 if (rp.status != 0x00) { 528 fprintf(stdout, "Status: %s [%#02x]\n", 529 hci_status2str(rp.status), rp.status); 530 return (FAILED); 531 } 532 533 fprintf(stdout, "Advertising transmit power level: %d dBm\n", 534 (int8_t)rp.transmit_power_level); 535 536 return (OK); 537 } 538 539 static int 540 le_set_advertising_data(int s, int argc, char *argv[]) 541 { 542 ng_hci_le_set_advertising_data_cp cp; 543 ng_hci_le_set_advertising_data_rp rp; 544 int n, len; 545 546 n = sizeof(rp); 547 548 char buf[NG_HCI_ADVERTISING_DATA_SIZE]; 549 550 len = sizeof(buf); 551 parse_param(argc, argv, buf, &len); 552 memset(cp.advertising_data, 0, sizeof(cp.advertising_data)); 553 cp.advertising_data_length = len; 554 memcpy(cp.advertising_data, buf, len); 555 556 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 557 NG_HCI_OCF_LE_SET_ADVERTISING_DATA), 558 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 559 return (ERROR); 560 561 if (rp.status != 0x00) { 562 fprintf(stdout, "Status: %s [%#02x]\n", 563 hci_status2str(rp.status), rp.status); 564 return (FAILED); 565 } 566 567 return (OK); 568 } 569 static int 570 le_read_buffer_size(int s, int argc, char *argv[]) 571 { 572 union { 573 ng_hci_le_read_buffer_size_rp v1; 574 ng_hci_le_read_buffer_size_rp_v2 v2; 575 } rp; 576 577 int n, ch; 578 uint8_t v; 579 uint16_t cmd; 580 581 optreset = 1; 582 optind = 0; 583 584 /* Default to version 1*/ 585 v = 1; 586 cmd = NG_HCI_OCF_LE_READ_BUFFER_SIZE; 587 588 while ((ch = getopt(argc, argv , "v:")) != -1) { 589 switch(ch) { 590 case 'v': 591 v = (uint8_t)strtol(optarg, NULL, 16); 592 if (v == 2) 593 cmd = NG_HCI_OCF_LE_READ_BUFFER_SIZE_V2; 594 else if (v > 2) 595 return (USAGE); 596 break; 597 default: 598 v = 1; 599 } 600 } 601 602 n = sizeof(rp); 603 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, cmd), 604 (void *)&rp, &n) == ERROR) 605 return (ERROR); 606 607 if (rp.v1.status != 0x00) { 608 fprintf(stdout, "Status: %s [%#02x]\n", 609 hci_status2str(rp.v1.status), rp.v1.status); 610 return (FAILED); 611 } 612 613 fprintf(stdout, "ACL data packet length: %d\n", 614 rp.v1.hc_le_data_packet_length); 615 fprintf(stdout, "Number of ACL data packets: %d\n", 616 rp.v1.hc_total_num_le_data_packets); 617 618 if (v == 2) { 619 fprintf(stdout, "ISO data packet length: %d\n", 620 rp.v2.hc_iso_data_packet_length); 621 fprintf(stdout, "Number of ISO data packets: %d\n", 622 rp.v2.hc_total_num_iso_data_packets); 623 } 624 625 return (OK); 626 } 627 628 static int 629 le_scan(int s, int argc, char *argv[]) 630 { 631 int n, bufsize, scancount, numscans; 632 bool verbose; 633 uint8_t active = 0; 634 char ch; 635 636 char b[512]; 637 ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b; 638 639 ng_hci_le_set_scan_parameters_cp scan_param_cp; 640 ng_hci_le_set_scan_parameters_rp scan_param_rp; 641 642 ng_hci_le_set_scan_enable_cp scan_enable_cp; 643 ng_hci_le_set_scan_enable_rp scan_enable_rp; 644 645 optreset = 1; 646 optind = 0; 647 verbose = false; 648 numscans = 1; 649 650 while ((ch = getopt(argc, argv , "an:v")) != -1) { 651 switch(ch) { 652 case 'a': 653 active = 1; 654 break; 655 case 'n': 656 numscans = (uint8_t)strtol(optarg, NULL, 10); 657 break; 658 case 'v': 659 verbose = true; 660 break; 661 } 662 } 663 664 scan_param_cp.le_scan_type = active; 665 scan_param_cp.le_scan_interval = (uint16_t)(100/0.625); 666 scan_param_cp.le_scan_window = (uint16_t)(50/0.625); 667 /* Address type public */ 668 scan_param_cp.own_address_type = 0; 669 /* 'All' filter policy */ 670 scan_param_cp.scanning_filter_policy = 0; 671 n = sizeof(scan_param_rp); 672 673 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 674 NG_HCI_OCF_LE_SET_SCAN_PARAMETERS), 675 (void *)&scan_param_cp, sizeof(scan_param_cp), 676 (void *)&scan_param_rp, &n) == ERROR) 677 return (ERROR); 678 679 if (scan_param_rp.status != 0x00) { 680 fprintf(stdout, "LE_Set_Scan_Parameters failed. Status: %s [%#02x]\n", 681 hci_status2str(scan_param_rp.status), 682 scan_param_rp.status); 683 return (FAILED); 684 } 685 686 /* Enable scanning */ 687 n = sizeof(scan_enable_rp); 688 scan_enable_cp.le_scan_enable = 1; 689 scan_enable_cp.filter_duplicates = 1; 690 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 691 NG_HCI_OCF_LE_SET_SCAN_ENABLE), 692 (void *)&scan_enable_cp, sizeof(scan_enable_cp), 693 (void *)&scan_enable_rp, &n) == ERROR) 694 return (ERROR); 695 696 if (scan_enable_rp.status != 0x00) { 697 fprintf(stdout, "LE_Scan_Enable enable failed. Status: %s [%#02x]\n", 698 hci_status2str(scan_enable_rp.status), 699 scan_enable_rp.status); 700 return (FAILED); 701 } 702 703 scancount = 0; 704 while (scancount < numscans) { 705 /* wait for scan events */ 706 bufsize = sizeof(b); 707 if (hci_recv(s, b, &bufsize) == ERROR) { 708 return (ERROR); 709 } 710 711 if (bufsize < sizeof(*e)) { 712 errno = EIO; 713 return (ERROR); 714 } 715 scancount++; 716 if (e->event == NG_HCI_EVENT_LE) { 717 fprintf(stdout, "Scan %d\n", scancount); 718 handle_le_event(e, verbose); 719 } 720 } 721 722 fprintf(stdout, "Scan complete\n"); 723 724 /* Disable scanning */ 725 n = sizeof(scan_enable_rp); 726 scan_enable_cp.le_scan_enable = 0; 727 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 728 NG_HCI_OCF_LE_SET_SCAN_ENABLE), 729 (void *)&scan_enable_cp, sizeof(scan_enable_cp), 730 (void *)&scan_enable_rp, &n) == ERROR) 731 return (ERROR); 732 733 if (scan_enable_rp.status != 0x00) { 734 fprintf(stdout, "LE_Scan_Enable disable failed. Status: %s [%#02x]\n", 735 hci_status2str(scan_enable_rp.status), 736 scan_enable_rp.status); 737 return (FAILED); 738 } 739 740 return (OK); 741 } 742 743 static void handle_le_event(ng_hci_event_pkt_t* e, bool verbose) 744 { 745 int rc; 746 ng_hci_le_ep *leer = 747 (ng_hci_le_ep *)(e + 1); 748 ng_hci_le_advertising_report_ep *advrep = 749 (ng_hci_le_advertising_report_ep *)(leer + 1); 750 ng_hci_le_advreport *reports = 751 (ng_hci_le_advreport *)(advrep + 1); 752 753 if (leer->subevent_code == NG_HCI_LEEV_ADVREP) { 754 fprintf(stdout, "Scan result, num_reports: %d\n", 755 advrep->num_reports); 756 for(rc = 0; rc < advrep->num_reports; rc++) { 757 uint8_t length = (uint8_t)reports[rc].length_data; 758 fprintf(stdout, "\tBD_ADDR %s \n", 759 hci_bdaddr2str(&reports[rc].bdaddr)); 760 fprintf(stdout, "\tAddress type: %s\n", 761 hci_addrtype2str(reports[rc].addr_type)); 762 if (length > 0 && verbose) { 763 dump_adv_data(length, reports[rc].data); 764 print_adv_data(length, reports[rc].data); 765 fprintf(stdout, 766 "\tRSSI: %d dBm\n", 767 (int8_t)reports[rc].data[length]); 768 fprintf(stdout, "\n"); 769 } 770 } 771 } 772 } 773 774 static int 775 le_read_white_list_size(int s, int argc, char *argv[]) 776 { 777 ng_hci_le_read_white_list_size_rp rp; 778 int n; 779 780 n = sizeof(rp); 781 782 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 783 NG_HCI_OCF_LE_READ_WHITE_LIST_SIZE), 784 (void *)&rp, &n) == ERROR) 785 return (ERROR); 786 787 if (rp.status != 0x00) { 788 fprintf(stdout, "Status: %s [%#02x]\n", 789 hci_status2str(rp.status), rp.status); 790 return (FAILED); 791 } 792 793 fprintf(stdout, "White list size: %d\n", 794 (uint8_t)rp.white_list_size); 795 796 return (OK); 797 } 798 799 static int 800 le_clear_white_list(int s, int argc, char *argv[]) 801 { 802 ng_hci_le_clear_white_list_rp rp; 803 int n; 804 805 n = sizeof(rp); 806 807 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 808 NG_HCI_OCF_LE_CLEAR_WHITE_LIST), 809 (void *)&rp, &n) == ERROR) 810 return (ERROR); 811 812 if (rp.status != 0x00) { 813 fprintf(stdout, "Status: %s [%#02x]\n", 814 hci_status2str(rp.status), rp.status); 815 return (FAILED); 816 } 817 818 fprintf(stdout, "White list cleared\n"); 819 820 return (OK); 821 } 822 823 static int 824 le_add_device_to_white_list(int s, int argc, char *argv[]) 825 { 826 ng_hci_le_add_device_to_white_list_cp cp; 827 ng_hci_le_add_device_to_white_list_rp rp; 828 int n; 829 char ch; 830 optreset = 1; 831 optind = 0; 832 bool addr_set = false; 833 834 n = sizeof(rp); 835 836 cp.address_type = 0x00; 837 838 while ((ch = getopt(argc, argv , "t:a:")) != -1) { 839 switch(ch) { 840 case 't': 841 if (strcmp(optarg, "public") == 0) 842 cp.address_type = 0x00; 843 else if (strcmp(optarg, "random") == 0) 844 cp.address_type = 0x01; 845 else 846 return (USAGE); 847 break; 848 case 'a': 849 addr_set = true; 850 if (!bt_aton(optarg, &cp.address)) { 851 struct hostent *he = NULL; 852 853 if ((he = bt_gethostbyname(optarg)) == NULL) 854 return (USAGE); 855 856 memcpy(&cp.address, he->h_addr, 857 sizeof(cp.address)); 858 } 859 break; 860 } 861 } 862 863 if (addr_set == false) 864 return (USAGE); 865 866 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 867 NG_HCI_OCF_LE_ADD_DEVICE_TO_WHITE_LIST), 868 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 869 return (ERROR); 870 871 if (rp.status != 0x00) { 872 fprintf(stdout, "Status: %s [%#02x]\n", 873 hci_status2str(rp.status), rp.status); 874 return (FAILED); 875 } 876 877 fprintf(stdout, "Address added to white list\n"); 878 879 return (OK); 880 } 881 882 static int 883 le_remove_device_from_white_list(int s, int argc, char *argv[]) 884 { 885 ng_hci_le_remove_device_from_white_list_cp cp; 886 ng_hci_le_remove_device_from_white_list_rp rp; 887 int n; 888 char ch; 889 optreset = 1; 890 optind = 0; 891 bool addr_set = false; 892 893 n = sizeof(rp); 894 895 cp.address_type = 0x00; 896 897 while ((ch = getopt(argc, argv , "t:a:")) != -1) { 898 switch(ch) { 899 case 't': 900 if (strcmp(optarg, "public") == 0) 901 cp.address_type = 0x00; 902 else if (strcmp(optarg, "random") == 0) 903 cp.address_type = 0x01; 904 else 905 return (USAGE); 906 break; 907 case 'a': 908 addr_set = true; 909 if (!bt_aton(optarg, &cp.address)) { 910 struct hostent *he = NULL; 911 912 if ((he = bt_gethostbyname(optarg)) == NULL) 913 return (USAGE); 914 915 memcpy(&cp.address, he->h_addr, 916 sizeof(cp.address)); 917 } 918 break; 919 } 920 } 921 922 if (addr_set == false) 923 return (USAGE); 924 925 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 926 NG_HCI_OCF_LE_ADD_DEVICE_TO_WHITE_LIST), 927 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 928 return (ERROR); 929 930 if (rp.status != 0x00) { 931 fprintf(stdout, "Status: %s [%#02x]\n", 932 hci_status2str(rp.status), rp.status); 933 return (FAILED); 934 } 935 936 fprintf(stdout, "Address removed from white list\n"); 937 938 return (OK); 939 } 940 941 static int 942 le_connect(int s, int argc, char *argv[]) 943 { 944 ng_hci_le_create_connection_cp cp; 945 ng_hci_status_rp rp; 946 char b[512]; 947 ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b; 948 949 int n, scancount, bufsize; 950 char ch; 951 bool addr_set = false; 952 bool verbose = false; 953 954 optreset = 1; 955 optind = 0; 956 957 /* minimal scan interval (2.5ms) */ 958 cp.scan_interval = htole16(4); 959 cp.scan_window = htole16(4); 960 961 /* Don't use the whitelist */ 962 cp.filter_policy = 0x00; 963 964 /* Default to public peer address */ 965 cp.peer_addr_type = 0x00; 966 967 /* Own address type public */ 968 cp.own_address_type = 0x00; 969 970 /* 18.75ms min connection interval */ 971 cp.conn_interval_min = htole16(0x000F); 972 /* 18.75ms max connection interval */ 973 cp.conn_interval_max = htole16(0x000F); 974 975 /* 0 events connection latency */ 976 cp.conn_latency = htole16(0x0000); 977 978 /* 32s supervision timeout */ 979 cp.supervision_timeout = htole16(0x0C80); 980 981 /* Min CE Length 0.625 ms */ 982 cp.min_ce_length = htole16(1); 983 /* Max CE Length 0.625 ms */ 984 cp.max_ce_length = htole16(1); 985 986 while ((ch = getopt(argc, argv , "a:t:v")) != -1) { 987 switch(ch) { 988 case 't': 989 if (strcmp(optarg, "public") == 0) 990 cp.peer_addr_type = 0x00; 991 else if (strcmp(optarg, "random") == 0) 992 cp.peer_addr_type = 0x01; 993 else 994 return (USAGE); 995 break; 996 case 'a': 997 addr_set = true; 998 if (!bt_aton(optarg, &cp.peer_addr)) { 999 struct hostent *he = NULL; 1000 1001 if ((he = bt_gethostbyname(optarg)) == NULL) 1002 return (USAGE); 1003 1004 memcpy(&cp.peer_addr, he->h_addr, 1005 sizeof(cp.peer_addr)); 1006 } 1007 break; 1008 case 'v': 1009 verbose = true; 1010 break; 1011 } 1012 } 1013 1014 if (addr_set == false) 1015 return (USAGE); 1016 1017 n = sizeof(rp); 1018 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 1019 NG_HCI_OCF_LE_CREATE_CONNECTION), 1020 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 1021 return (ERROR); 1022 1023 if (rp.status != 0x00) { 1024 fprintf(stdout, 1025 "Create connection failed. Status: %s [%#02x]\n", 1026 hci_status2str(rp.status), rp.status); 1027 return (FAILED); 1028 } 1029 1030 scancount = 0; 1031 while (scancount < 3) { 1032 /* wait for connection events */ 1033 bufsize = sizeof(b); 1034 if (hci_recv(s, b, &bufsize) == ERROR) { 1035 return (ERROR); 1036 } 1037 1038 if (bufsize < sizeof(*e)) { 1039 errno = EIO; 1040 return (ERROR); 1041 } 1042 scancount++; 1043 if (e->event == NG_HCI_EVENT_LE) { 1044 handle_le_connection_event(e, verbose); 1045 break; 1046 } 1047 } 1048 1049 return (OK); 1050 } 1051 1052 static void handle_le_connection_event(ng_hci_event_pkt_t* e, bool verbose) 1053 { 1054 ng_hci_le_ep *ev_pkt; 1055 ng_hci_le_connection_complete_ep *conn_event; 1056 1057 ev_pkt = (ng_hci_le_ep *)(e + 1); 1058 1059 if (ev_pkt->subevent_code == NG_HCI_LEEV_CON_COMPL) { 1060 conn_event =(ng_hci_le_connection_complete_ep *)(ev_pkt + 1); 1061 fprintf(stdout, "Handle: %d\n", le16toh(conn_event->handle)); 1062 if (verbose) { 1063 fprintf(stdout, 1064 "Status: %s\n", 1065 hci_status2str(conn_event->status)); 1066 fprintf(stdout, 1067 "Role: %s\n", 1068 hci_role2str(conn_event->role)); 1069 fprintf(stdout, 1070 "Address Type: %s\n", 1071 hci_addrtype2str(conn_event->address_type)); 1072 fprintf(stdout, 1073 "Address: %s\n", 1074 hci_bdaddr2str(&conn_event->address)); 1075 fprintf(stdout, 1076 "Interval: %.2fms\n", 1077 6.25 * le16toh(conn_event->interval)); 1078 fprintf(stdout, 1079 "Latency: %d events\n", conn_event->latency); 1080 fprintf(stdout, 1081 "Supervision timeout: %dms\n", 1082 10 * le16toh(conn_event->supervision_timeout)); 1083 fprintf(stdout, 1084 "Master clock accuracy: %s\n", 1085 hci_mc_accuracy2str( 1086 conn_event->master_clock_accuracy)); 1087 } 1088 } 1089 } 1090 1091 static int 1092 le_read_channel_map(int s, int argc, char *argv[]) 1093 { 1094 ng_hci_le_read_channel_map_cp cp; 1095 ng_hci_le_read_channel_map_rp rp; 1096 int n; 1097 char buffer[2048]; 1098 1099 /* parse command parameters */ 1100 switch (argc) { 1101 case 1: 1102 /* connection handle */ 1103 if (sscanf(argv[0], "%d", &n) != 1 || n <= 0 || n > 0x0eff) 1104 return (USAGE); 1105 1106 cp.connection_handle = (uint16_t) (n & 0x0fff); 1107 cp.connection_handle = htole16(cp.connection_handle); 1108 break; 1109 1110 default: 1111 return (USAGE); 1112 } 1113 1114 n = sizeof(rp); 1115 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 1116 NG_HCI_OCF_LE_READ_CHANNEL_MAP), 1117 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 1118 return (ERROR); 1119 1120 if (rp.status != 0x00) { 1121 fprintf(stdout, 1122 "Read channel map failed. Status: %s [%#02x]\n", 1123 hci_status2str(rp.status), rp.status); 1124 return (FAILED); 1125 } 1126 1127 fprintf(stdout, "Connection handle: %d\n", 1128 le16toh(rp.connection_handle)); 1129 fprintf(stdout, "Used channels:\n"); 1130 fprintf(stdout, "\n%s\n", hci_le_chanmap2str(rp.le_channel_map, 1131 buffer, sizeof(buffer))); 1132 1133 return (OK); 1134 } /* le_read_channel_map */ 1135 1136 static int 1137 le_read_remote_features(int s, int argc, char *argv[]) 1138 { 1139 ng_hci_le_read_remote_used_features_cp cp; 1140 ng_hci_status_rp rp; 1141 int n, bufsize; 1142 char b[512]; 1143 1144 ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b; 1145 1146 /* parse command parameters */ 1147 switch (argc) { 1148 case 1: 1149 /* connection handle */ 1150 if (sscanf(argv[0], "%d", &n) != 1 || n <= 0 || n > 0x0eff) 1151 return (USAGE); 1152 1153 cp.connection_handle = (uint16_t) (n & 0x0fff); 1154 cp.connection_handle = htole16(cp.connection_handle); 1155 break; 1156 1157 default: 1158 return (USAGE); 1159 } 1160 1161 n = sizeof(rp); 1162 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 1163 NG_HCI_OCF_LE_READ_REMOTE_USED_FEATURES), 1164 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 1165 return (ERROR); 1166 1167 if (rp.status != 0x00) { 1168 fprintf(stdout, 1169 "Read remote features failed. Status: %s [%#02x]\n", 1170 hci_status2str(rp.status), rp.status); 1171 return (FAILED); 1172 } 1173 1174 /* wait for connection events */ 1175 bufsize = sizeof(b); 1176 if (hci_recv(s, b, &bufsize) == ERROR) { 1177 return (ERROR); 1178 } 1179 1180 if (bufsize < sizeof(*e)) { 1181 errno = EIO; 1182 return (ERROR); 1183 } 1184 if (e->event == NG_HCI_EVENT_LE) { 1185 handle_le_remote_features_event(e); 1186 } 1187 1188 return (OK); 1189 } /* le_read_remote_features */ 1190 1191 static void handle_le_remote_features_event(ng_hci_event_pkt_t* e) 1192 { 1193 ng_hci_le_ep *ev_pkt; 1194 ng_hci_le_read_remote_features_ep *feat_event; 1195 char buffer[2048]; 1196 1197 ev_pkt = (ng_hci_le_ep *)(e + 1); 1198 1199 if (ev_pkt->subevent_code == NG_HCI_LEEV_READ_REMOTE_FEATURES_COMPL) { 1200 feat_event =(ng_hci_le_read_remote_features_ep *)(ev_pkt + 1); 1201 fprintf(stdout, "Handle: %d\n", 1202 le16toh(feat_event->connection_handle)); 1203 fprintf(stdout, 1204 "Status: %s\n", 1205 hci_status2str(feat_event->status)); 1206 fprintf(stdout, "Features:\n%s\n", 1207 hci_le_features2str(feat_event->features, 1208 buffer, sizeof(buffer))); 1209 } 1210 } /* handle_le_remote_features_event */ 1211 1212 static int le_rand(int s, int argc, char *argv[]) 1213 { 1214 ng_hci_le_rand_rp rp; 1215 int n; 1216 1217 n = sizeof(rp); 1218 1219 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 1220 NG_HCI_OCF_LE_RAND), 1221 (void *)&rp, &n) == ERROR) 1222 return (ERROR); 1223 1224 if (rp.status != 0x00) { 1225 fprintf(stdout, "Status: %s [%#02x]\n", 1226 hci_status2str(rp.status), rp.status); 1227 return (FAILED); 1228 } 1229 1230 fprintf(stdout, 1231 "Random number : %08llx\n", 1232 (unsigned long long)le64toh(rp.random_number)); 1233 1234 return (OK); 1235 } 1236 1237 1238 1239 struct hci_command le_commands[] = { 1240 { 1241 "le_enable", 1242 "le_enable [enable|disable] \n" 1243 "Enable LE event ", 1244 &le_enable, 1245 }, 1246 { 1247 "le_read_local_supported_features", 1248 "le_read_local_supported_features\n" 1249 "read local supported features mask", 1250 &le_read_local_supported_features, 1251 }, 1252 { 1253 "le_read_supported_states", 1254 "le_read_supported_states\n" 1255 "read supported status" 1256 , 1257 &le_read_supported_states, 1258 }, 1259 { 1260 "le_set_scan_response", 1261 "le_set_scan_response -n $name -f $flag -u $uuid16,$uuid16 \n" 1262 "set LE scan response data" 1263 , 1264 &le_set_scan_response, 1265 }, 1266 { 1267 "le_set_scan_enable", 1268 "le_set_scan_enable [enable|disable] \n" 1269 "enable or disable LE device scan", 1270 &le_set_scan_enable 1271 }, 1272 { 1273 "le_set_scan_param", 1274 "le_set_scan_param [active|passive] interval(ms) window(ms) [public|random] [all|whitelist] \n" 1275 "set LE device scan parameter", 1276 &le_set_scan_param 1277 }, 1278 { 1279 "le_set_advertising_enable", 1280 "le_set_advertising_enable [enable|disable] \n" 1281 "start or stop advertising", 1282 &le_set_advertising_enable 1283 }, 1284 { 1285 "le_read_advertising_channel_tx_power", 1286 "le_read_advertising_channel_tx_power\n" 1287 "read host advertising transmit poser level (dBm)", 1288 &le_read_advertising_channel_tx_power 1289 }, 1290 { 1291 "le_set_advertising_param", 1292 "le_set_advertising_param [-m min_interval(ms)] [-M max_interval(ms)]\n" 1293 "[-t advertising_type] [-o own_address_type] [-p peer_address_type]\n" 1294 "[-c advertising_channel_map] [-f advertising_filter_policy]\n" 1295 "[-a peer_address]\n" 1296 "set LE device advertising parameters", 1297 &le_set_advertising_param 1298 }, 1299 { 1300 "le_set_advertising_data", 1301 "le_set_advertising_data -n $name -f $flag -u $uuid16,$uuid16 \n" 1302 "set LE device advertising packed data", 1303 &le_set_advertising_data 1304 }, 1305 { 1306 "le_read_buffer_size", 1307 "le_read_buffer_size [-v 1|2]\n" 1308 "Read the maximum size of ACL and ISO data packets", 1309 &le_read_buffer_size 1310 }, 1311 { 1312 "le_scan", 1313 "le_scan [-a] [-v] [-n number_of_scans]\n" 1314 "Do an LE scan", 1315 &le_scan 1316 }, 1317 { 1318 "le_read_white_list_size", 1319 "le_read_white_list_size\n" 1320 "Read total number of white list entries that can be stored", 1321 &le_read_white_list_size 1322 }, 1323 { 1324 "le_clear_white_list", 1325 "le_clear_white_list\n" 1326 "Clear the white list in the controller", 1327 &le_clear_white_list 1328 }, 1329 { 1330 "le_add_device_to_white_list", 1331 "le_add_device_to_white_list\n" 1332 "[-t public|random] -a address\n" 1333 "Add device to the white list", 1334 &le_add_device_to_white_list 1335 }, 1336 { 1337 "le_remove_device_from_white_list", 1338 "le_remove_device_from_white_list\n" 1339 "[-t public|random] -a address\n" 1340 "Remove device from the white list", 1341 &le_remove_device_from_white_list 1342 }, 1343 { 1344 "le_connect", 1345 "le_connect -a address [-t public|random] [-v]\n" 1346 "Connect to an LE device", 1347 &le_connect 1348 }, 1349 { 1350 "le_read_channel_map", 1351 "le_read_channel_map <connection_handle>\n" 1352 "Read the channel map for a connection", 1353 &le_read_channel_map 1354 }, 1355 { 1356 "le_read_remote_features", 1357 "le_read_remote_features <connection_handle>\n" 1358 "Read supported features for the device\n" 1359 "identified by the connection handle", 1360 &le_read_remote_features 1361 }, 1362 { 1363 "le_rand", 1364 "le_rand\n" 1365 "Generate 64 bits of random data", 1366 &le_rand 1367 }, 1368 }; 1369