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 <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 51 static int le_set_scan_param(int s, int argc, char *argv[]); 52 static int le_set_scan_enable(int s, int argc, char *argv[]); 53 static int parse_param(int argc, char *argv[], char *buf, int *len); 54 static int le_set_scan_response(int s, int argc, char *argv[]); 55 static int le_read_supported_states(int s, int argc, char *argv[]); 56 static int le_read_local_supported_features(int s, int argc ,char *argv[]); 57 static int set_le_event_mask(int s, uint64_t mask); 58 static int set_event_mask(int s, uint64_t mask); 59 static int le_enable(int s, int argc, char *argv[]); 60 static int le_set_advertising_enable(int s, int argc, char *argv[]); 61 static int le_set_advertising_param(int s, int argc, char *argv[]); 62 static int le_read_advertising_channel_tx_power(int s, int argc, char *argv[]); 63 64 static int 65 le_set_scan_param(int s, int argc, char *argv[]) 66 { 67 int type; 68 int interval; 69 int window; 70 int adrtype; 71 int policy; 72 int n; 73 74 ng_hci_le_set_scan_parameters_cp cp; 75 ng_hci_le_set_scan_parameters_rp rp; 76 77 if (argc != 5) 78 return (USAGE); 79 80 if (strcmp(argv[0], "active") == 0) 81 type = 1; 82 else if (strcmp(argv[0], "passive") == 0) 83 type = 0; 84 else 85 return (USAGE); 86 87 interval = (int)(atof(argv[1])/0.625); 88 interval = (interval < 4)? 4: interval; 89 window = (int)(atof(argv[2])/0.625); 90 window = (window < 4) ? 4 : interval; 91 92 if (strcmp(argv[3], "public") == 0) 93 adrtype = 0; 94 else if (strcmp(argv[3], "random") == 0) 95 adrtype = 1; 96 else 97 return (USAGE); 98 99 if (strcmp(argv[4], "all") == 0) 100 policy = 0; 101 else if (strcmp(argv[4], "whitelist") == 0) 102 policy = 1; 103 else 104 return (USAGE); 105 106 cp.le_scan_type = type; 107 cp.le_scan_interval = interval; 108 cp.own_address_type = adrtype; 109 cp.le_scan_window = window; 110 cp.scanning_filter_policy = policy; 111 n = sizeof(rp); 112 113 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 114 NG_HCI_OCF_LE_SET_SCAN_PARAMETERS), 115 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 116 return (ERROR); 117 118 if (rp.status != 0x00) { 119 fprintf(stdout, "Status: %s [%#02x]\n", 120 hci_status2str(rp.status), rp.status); 121 return (FAILED); 122 } 123 124 return (OK); 125 } 126 127 static int 128 le_set_scan_enable(int s, int argc, char *argv[]) 129 { 130 ng_hci_le_set_scan_enable_cp cp; 131 ng_hci_le_set_scan_enable_rp rp; 132 int n, enable = 0; 133 134 if (argc != 1) 135 return (USAGE); 136 137 if (strcmp(argv[0], "enable") == 0) 138 enable = 1; 139 else if (strcmp(argv[0], "disable") != 0) 140 return (USAGE); 141 142 n = sizeof(rp); 143 cp.le_scan_enable = enable; 144 cp.filter_duplicates = 0; 145 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 146 NG_HCI_OCF_LE_SET_SCAN_ENABLE), 147 (void *)&cp, sizeof(cp), 148 (void *)&rp, &n) == ERROR) 149 return (ERROR); 150 151 if (rp.status != 0x00) { 152 fprintf(stdout, "Status: %s [%#02x]\n", 153 hci_status2str(rp.status), rp.status); 154 return (FAILED); 155 } 156 157 fprintf(stdout, "LE Scan: %s\n", 158 enable? "Enabled" : "Disabled"); 159 160 return (OK); 161 } 162 163 static int 164 parse_param(int argc, char *argv[], char *buf, int *len) 165 { 166 char *buflast = buf + (*len); 167 char *curbuf = buf; 168 char *token,*lenpos; 169 int ch; 170 int datalen; 171 uint16_t value; 172 optreset = 1; 173 optind = 0; 174 while ((ch = getopt(argc, argv , "n:f:u:")) != -1) { 175 switch(ch){ 176 case 'n': 177 datalen = strlen(optarg); 178 if ((curbuf + datalen + 2) >= buflast) 179 goto done; 180 curbuf[0] = datalen + 1; 181 curbuf[1] = 8; 182 curbuf += 2; 183 memcpy(curbuf, optarg, datalen); 184 curbuf += datalen; 185 break; 186 case 'f': 187 if (curbuf+3 > buflast) 188 goto done; 189 curbuf[0] = 2; 190 curbuf[1] = 1; 191 curbuf[2] = (uint8_t)strtol(optarg, NULL, 16); 192 curbuf += 3; 193 break; 194 case 'u': 195 if ((buf+2) >= buflast) 196 goto done; 197 lenpos = curbuf; 198 curbuf[1] = 2; 199 *lenpos = 1; 200 curbuf += 2; 201 while ((token = strsep(&optarg, ",")) != NULL) { 202 value = strtol(token, NULL, 16); 203 if ((curbuf+2) >= buflast) 204 break; 205 curbuf[0] = value &0xff; 206 curbuf[1] = (value>>8)&0xff; 207 curbuf += 2; 208 *lenpos += 2; 209 } 210 211 } 212 } 213 done: 214 *len = curbuf - buf; 215 216 return (OK); 217 } 218 219 static int 220 le_set_scan_response(int s, int argc, char *argv[]) 221 { 222 ng_hci_le_set_scan_response_data_cp cp; 223 ng_hci_le_set_scan_response_data_rp rp; 224 int n; 225 int len; 226 char buf[NG_HCI_ADVERTISING_DATA_SIZE]; 227 228 len = sizeof(buf); 229 parse_param(argc, argv, buf, &len); 230 memset(cp.scan_response_data, 0, sizeof(cp.scan_response_data)); 231 cp.scan_response_data_length = len; 232 memcpy(cp.scan_response_data, buf, len); 233 n = sizeof(rp); 234 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 235 NG_HCI_OCF_LE_SET_SCAN_RESPONSE_DATA), 236 (void *)&cp, sizeof(cp), 237 (void *)&rp, &n) == ERROR) 238 return (ERROR); 239 240 if (rp.status != 0x00) { 241 fprintf(stdout, "Status: %s [%#02x]\n", 242 hci_status2str(rp.status), rp.status); 243 return (FAILED); 244 } 245 246 return (OK); 247 } 248 249 static int 250 le_read_local_supported_features(int s, int argc ,char *argv[]) 251 { 252 ng_hci_le_read_local_supported_features_rp rp; 253 int n = sizeof(rp); 254 255 union { 256 uint64_t raw; 257 uint8_t octets[8]; 258 } le_features; 259 260 char buffer[2048]; 261 262 if (hci_simple_request(s, 263 NG_HCI_OPCODE(NG_HCI_OGF_LE, 264 NG_HCI_OCF_LE_READ_LOCAL_SUPPORTED_FEATURES), 265 (void *)&rp, &n) == ERROR) 266 return (ERROR); 267 268 if (rp.status != 0x00) { 269 fprintf(stdout, "Status: %s [%#02x]\n", 270 hci_status2str(rp.status), rp.status); 271 return (FAILED); 272 } 273 274 le_features.raw = rp.le_features; 275 276 fprintf(stdout, "LE Features: "); 277 for(int i = 0; i < 8; i++) 278 fprintf(stdout, " %#02x", le_features.octets[i]); 279 fprintf(stdout, "\n%s\n", hci_le_features2str(le_features.octets, 280 buffer, sizeof(buffer))); 281 fprintf(stdout, "\n"); 282 283 return (OK); 284 } 285 286 static int 287 le_read_supported_states(int s, int argc, char *argv[]) 288 { 289 ng_hci_le_read_supported_states_rp rp; 290 int n = sizeof(rp); 291 292 if (hci_simple_request(s, NG_HCI_OPCODE( 293 NG_HCI_OGF_LE, 294 NG_HCI_OCF_LE_READ_SUPPORTED_STATES), 295 (void *)&rp, &n) == ERROR) 296 return (ERROR); 297 298 if (rp.status != 0x00) { 299 fprintf(stdout, "Status: %s [%#02x]\n", 300 hci_status2str(rp.status), rp.status); 301 return (FAILED); 302 } 303 304 fprintf(stdout, "LE States: %jx\n", rp.le_states); 305 306 return (OK); 307 } 308 309 static int 310 set_le_event_mask(int s, uint64_t mask) 311 { 312 ng_hci_le_set_event_mask_cp semc; 313 ng_hci_le_set_event_mask_rp rp; 314 int i, n; 315 316 n = sizeof(rp); 317 318 for (i=0; i < NG_HCI_LE_EVENT_MASK_SIZE; i++) { 319 semc.event_mask[i] = mask&0xff; 320 mask >>= 8; 321 } 322 if(hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 323 NG_HCI_OCF_LE_SET_EVENT_MASK), 324 (void *)&semc, sizeof(semc), (void *)&rp, &n) == ERROR) 325 return (ERROR); 326 327 if (rp.status != 0x00) { 328 fprintf(stdout, "Status: %s [%#02x]\n", 329 hci_status2str(rp.status), rp.status); 330 return (FAILED); 331 } 332 333 return (OK); 334 } 335 336 static int 337 set_event_mask(int s, uint64_t mask) 338 { 339 ng_hci_set_event_mask_cp semc; 340 ng_hci_set_event_mask_rp rp; 341 int i, n; 342 343 n = sizeof(rp); 344 345 for (i=0; i < NG_HCI_EVENT_MASK_SIZE; i++) { 346 semc.event_mask[i] = mask&0xff; 347 mask >>= 8; 348 } 349 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_HC_BASEBAND, 350 NG_HCI_OCF_SET_EVENT_MASK), 351 (void *)&semc, sizeof(semc), (void *)&rp, &n) == ERROR) 352 return (ERROR); 353 354 if (rp.status != 0x00) { 355 fprintf(stdout, "Status: %s [%#02x]\n", 356 hci_status2str(rp.status), rp.status); 357 return (FAILED); 358 } 359 360 return (OK); 361 } 362 363 static 364 int le_enable(int s, int argc, char *argv[]) 365 { 366 int result; 367 368 if (argc != 1) 369 return (USAGE); 370 371 if (strcasecmp(argv[0], "enable") == 0) { 372 result = set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT | 373 NG_HCI_EVENT_MASK_LE); 374 if (result != OK) 375 return result; 376 result = set_le_event_mask(s, NG_HCI_LE_EVENT_MASK_ALL); 377 if (result == OK) { 378 fprintf(stdout, "LE enabled\n"); 379 return (OK); 380 } else 381 return result; 382 } else if (strcasecmp(argv[0], "disable") == 0) { 383 result = set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT); 384 if (result == OK) { 385 fprintf(stdout, "LE disabled\n"); 386 return (OK); 387 } else 388 return result; 389 } else 390 return (USAGE); 391 } 392 393 static int 394 le_set_advertising_enable(int s, int argc, char *argv[]) 395 { 396 ng_hci_le_set_advertise_enable_cp cp; 397 ng_hci_le_set_advertise_enable_rp rp; 398 int n, enable = 0; 399 400 if (argc != 1) 401 return USAGE; 402 403 if (strcmp(argv[0], "enable") == 0) 404 enable = 1; 405 else if (strcmp(argv[0], "disable") != 0) 406 return USAGE; 407 408 n = sizeof(rp); 409 cp.advertising_enable = enable; 410 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 411 NG_HCI_OCF_LE_SET_ADVERTISE_ENABLE), 412 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 413 return (ERROR); 414 415 if (rp.status != 0x00) { 416 fprintf(stdout, "Status: %s [%#02x]\n", 417 hci_status2str(rp.status), rp.status); 418 return (FAILED); 419 } 420 fprintf(stdout, "LE Advertising %s\n", (enable ? "enabled" : "disabled")); 421 422 return (OK); 423 } 424 425 static int 426 le_set_advertising_param(int s, int argc, char *argv[]) 427 { 428 ng_hci_le_set_advertising_parameters_cp cp; 429 ng_hci_le_set_advertising_parameters_rp rp; 430 431 int n, ch; 432 433 cp.advertising_interval_min = 0x800; 434 cp.advertising_interval_max = 0x800; 435 cp.advertising_type = 0; 436 cp.own_address_type = 0; 437 cp.direct_address_type = 0; 438 439 cp.advertising_channel_map = 7; 440 cp.advertising_filter_policy = 0; 441 442 optreset = 1; 443 optind = 0; 444 while ((ch = getopt(argc, argv , "m:M:t:o:p:a:c:f:")) != -1) { 445 switch(ch) { 446 case 'm': 447 cp.advertising_interval_min = 448 (uint16_t)(strtod(optarg, NULL)/0.625); 449 break; 450 case 'M': 451 cp.advertising_interval_max = 452 (uint16_t)(strtod(optarg, NULL)/0.625); 453 break; 454 case 't': 455 cp.advertising_type = 456 (uint8_t)strtod(optarg, NULL); 457 break; 458 case 'o': 459 cp.own_address_type = 460 (uint8_t)strtod(optarg, NULL); 461 break; 462 case 'p': 463 cp.direct_address_type = 464 (uint8_t)strtod(optarg, NULL); 465 break; 466 case 'a': 467 if (!bt_aton(optarg, &cp.direct_address)) { 468 struct hostent *he = NULL; 469 470 if ((he = bt_gethostbyname(optarg)) == NULL) 471 return (USAGE); 472 473 memcpy(&cp.direct_address, he->h_addr, sizeof(cp.direct_address)); 474 } 475 break; 476 case 'c': 477 cp.advertising_channel_map = 478 (uint8_t)strtod(optarg, NULL); 479 break; 480 case 'f': 481 cp.advertising_filter_policy = 482 (uint8_t)strtod(optarg, NULL); 483 break; 484 } 485 } 486 487 n = sizeof(rp); 488 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 489 NG_HCI_OCF_LE_SET_ADVERTISING_PARAMETERS), 490 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 491 return (ERROR); 492 493 if (rp.status != 0x00) { 494 fprintf(stdout, "Status: %s [%#02x]\n", 495 hci_status2str(rp.status), rp.status); 496 return (FAILED); 497 } 498 499 return (OK); 500 } 501 502 static int 503 le_read_advertising_channel_tx_power(int s, int argc, char *argv[]) 504 { 505 ng_hci_le_read_advertising_channel_tx_power_rp rp; 506 int n; 507 508 n = sizeof(rp); 509 510 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 511 NG_HCI_OCF_LE_READ_ADVERTISING_CHANNEL_TX_POWER), 512 (void *)&rp, &n) == ERROR) 513 return (ERROR); 514 515 if (rp.status != 0x00) { 516 fprintf(stdout, "Status: %s [%#02x]\n", 517 hci_status2str(rp.status), rp.status); 518 return (FAILED); 519 } 520 521 fprintf(stdout, "Advertising transmit power level: %d dBm\n", 522 (int8_t)rp.transmit_power_level); 523 524 return (OK); 525 } 526 527 static int 528 le_set_advertising_data(int s, int argc, char *argv[]) 529 { 530 ng_hci_le_set_advertising_data_cp cp; 531 ng_hci_le_set_advertising_data_rp rp; 532 int n, len; 533 534 n = sizeof(rp); 535 536 char buf[NG_HCI_ADVERTISING_DATA_SIZE]; 537 538 len = sizeof(buf); 539 parse_param(argc, argv, buf, &len); 540 memset(cp.advertising_data, 0, sizeof(cp.advertising_data)); 541 cp.advertising_data_length = len; 542 memcpy(cp.advertising_data, buf, len); 543 544 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 545 NG_HCI_OCF_LE_SET_ADVERTISING_DATA), 546 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 547 return (ERROR); 548 549 if (rp.status != 0x00) { 550 fprintf(stdout, "Status: %s [%#02x]\n", 551 hci_status2str(rp.status), rp.status); 552 return (FAILED); 553 } 554 555 return (OK); 556 } 557 static int 558 le_read_buffer_size(int s, int argc, char *argv[]) 559 { 560 union { 561 ng_hci_le_read_buffer_size_rp v1; 562 ng_hci_le_read_buffer_size_rp_v2 v2; 563 } rp; 564 565 int n, ch; 566 uint8_t v; 567 uint16_t cmd; 568 569 optreset = 1; 570 optind = 0; 571 572 /* Default to version 1*/ 573 v = 1; 574 cmd = NG_HCI_OCF_LE_READ_BUFFER_SIZE; 575 576 while ((ch = getopt(argc, argv , "v:")) != -1) { 577 switch(ch) { 578 case 'v': 579 v = (uint8_t)strtol(optarg, NULL, 16); 580 if (v == 2) 581 cmd = NG_HCI_OCF_LE_READ_BUFFER_SIZE_V2; 582 else if (v > 2) 583 return (USAGE); 584 break; 585 default: 586 v = 1; 587 } 588 } 589 590 n = sizeof(rp); 591 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, cmd), 592 (void *)&rp, &n) == ERROR) 593 return (ERROR); 594 595 if (rp.v1.status != 0x00) { 596 fprintf(stdout, "Status: %s [%#02x]\n", 597 hci_status2str(rp.v1.status), rp.v1.status); 598 return (FAILED); 599 } 600 601 fprintf(stdout, "ACL data packet length: %d\n", 602 rp.v1.hc_le_data_packet_length); 603 fprintf(stdout, "Number of ACL data packets: %d\n", 604 rp.v1.hc_total_num_le_data_packets); 605 606 if (v == 2) { 607 fprintf(stdout, "ISO data packet length: %d\n", 608 rp.v2.hc_iso_data_packet_length); 609 fprintf(stdout, "Number of ISO data packets: %d\n", 610 rp.v2.hc_total_num_iso_data_packets); 611 } 612 613 return (OK); 614 } 615 616 struct hci_command le_commands[] = { 617 { 618 "le_enable", 619 "le_enable [enable|disable] \n" 620 "Enable LE event ", 621 &le_enable, 622 }, 623 { 624 "le_read_local_supported_features", 625 "le_read_local_supported_features\n" 626 "read local supported features mask", 627 &le_read_local_supported_features, 628 }, 629 { 630 "le_read_supported_states", 631 "le_read_supported_states\n" 632 "read supported status" 633 , 634 &le_read_supported_states, 635 }, 636 { 637 "le_set_scan_response", 638 "le_set_scan_response -n $name -f $flag -u $uuid16,$uuid16 \n" 639 "set LE scan response data" 640 , 641 &le_set_scan_response, 642 }, 643 { 644 "le_set_scan_enable", 645 "le_set_scan_enable [enable|disable] \n" 646 "enable or disable LE device scan", 647 &le_set_scan_enable 648 }, 649 { 650 "le_set_scan_param", 651 "le_set_scan_param [active|passive] interval(ms) window(ms) [public|random] [all|whitelist] \n" 652 "set LE device scan parameter", 653 &le_set_scan_param 654 }, 655 { 656 "le_set_advertising_enable", 657 "le_set_advertising_enable [enable|disable] \n" 658 "start or stop advertising", 659 &le_set_advertising_enable 660 }, 661 { 662 "le_read_advertising_channel_tx_power", 663 "le_read_advertising_channel_tx_power\n" 664 "read host advertising transmit poser level (dBm)", 665 &le_read_advertising_channel_tx_power 666 }, 667 { 668 "le_set_advertising_param", 669 "le_set_advertising_param [-m min_interval(ms)] [-M max_interval(ms)]\n" 670 "[-t advertising_type] [-o own_address_type] [-p peer_address_type]\n" 671 "[-c advertising_channel_map] [-f advertising_filter_policy]\n" 672 "[-a peer_address]\n" 673 "set LE device advertising parameters", 674 &le_set_advertising_param 675 }, 676 { 677 "le_set_advertising_data", 678 "le_set_advertising_data -n $name -f $flag -u $uuid16,$uuid16 \n" 679 "set LE device advertising packed data", 680 &le_set_advertising_data 681 }, 682 { 683 "le_read_buffer_size", 684 "le_read_buffer_size [-v 1|2]\n" 685 "Read the maximum size of ACL and ISO data packets", 686 &le_read_buffer_size 687 }, 688 }; 689