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