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 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[]); 63 64static int 65le_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 e, 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 e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 113 NG_HCI_OCF_LE_SET_SCAN_PARAMETERS), 114 (void *)&cp, sizeof(cp), (void *)&rp, &n); 115 116 return 0; 117} 118 119static int 120le_set_scan_enable(int s, int argc, char *argv[]) 121{ 122 ng_hci_le_set_scan_enable_cp cp; 123 ng_hci_le_set_scan_enable_rp rp; 124 int e, n, enable = 0; 125 126 if (argc != 1) 127 return USAGE; 128 129 if (strcmp(argv[0], "enable") == 0) 130 enable = 1; 131 else if (strcmp(argv[0], "disable") != 0) 132 return USAGE; 133 134 n = sizeof(rp); 135 cp.le_scan_enable = enable; 136 cp.filter_duplicates = 0; 137 e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 138 NG_HCI_OCF_LE_SET_SCAN_ENABLE), 139 (void *)&cp, sizeof(cp), (void *)&rp, &n); 140 141 if (e != 0 || rp.status != 0) 142 return ERROR; 143 144 return OK; 145} 146 147static int 148parse_param(int argc, char *argv[], char *buf, int *len) 149{ 150 char *buflast = buf + (*len); 151 char *curbuf = buf; 152 char *token,*lenpos; 153 int ch; 154 int datalen; 155 uint16_t value; 156 optreset = 1; 157 optind = 0; 158 while ((ch = getopt(argc, argv , "n:f:u:")) != -1) { 159 switch(ch){ 160 case 'n': 161 datalen = strlen(optarg); 162 if ((curbuf + datalen + 2) >= buflast) 163 goto done; 164 curbuf[0] = datalen + 1; 165 curbuf[1] = 8; 166 curbuf += 2; 167 memcpy(curbuf, optarg, datalen); 168 curbuf += datalen; 169 break; 170 case 'f': 171 if (curbuf+3 > buflast) 172 goto done; 173 curbuf[0] = 2; 174 curbuf[1] = 1; 175 curbuf[2] = (uint8_t)strtol(optarg, NULL, 16); 176 curbuf += 3; 177 break; 178 case 'u': 179 if ((buf+2) >= buflast) 180 goto done; 181 lenpos = curbuf; 182 curbuf[1] = 2; 183 *lenpos = 1; 184 curbuf += 2; 185 while ((token = strsep(&optarg, ",")) != NULL) { 186 value = strtol(token, NULL, 16); 187 if ((curbuf+2) >= buflast) 188 break; 189 curbuf[0] = value &0xff; 190 curbuf[1] = (value>>8)&0xff; 191 curbuf += 2; 192 *lenpos += 2; 193 } 194 195 } 196 } 197done: 198 *len = curbuf - buf; 199 200 return OK; 201} 202 203static int 204le_set_scan_response(int s, int argc, char *argv[]) 205{ 206 ng_hci_le_set_scan_response_data_cp cp; 207 ng_hci_le_set_scan_response_data_rp rp; 208 int n; 209 int e; 210 int len; 211 char buf[NG_HCI_ADVERTISING_DATA_SIZE]; 212 213 len = sizeof(buf); 214 parse_param(argc, argv, buf, &len); 215 memset(cp.scan_response_data, 0, sizeof(cp.scan_response_data)); 216 cp.scan_response_data_length = len; 217 memcpy(cp.scan_response_data, buf, len); 218 n = sizeof(rp); 219 e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 220 NG_HCI_OCF_LE_SET_SCAN_RESPONSE_DATA), 221 (void *)&cp, sizeof(cp), (void *)&rp, &n); 222 223 printf("SET SCAN RESPONSE %d %d %d\n", e, rp.status, n); 224 225 return OK; 226} 227 228static int 229le_read_local_supported_features(int s, int argc ,char *argv[]) 230{ 231 ng_hci_le_read_local_supported_features_rp rp; 232 int n = sizeof(rp); 233 234 union { 235 uint64_t raw; 236 uint8_t octets[8]; 237 } le_features; 238 239 char buffer[2048]; 240 241 if (hci_simple_request(s, 242 NG_HCI_OPCODE(NG_HCI_OGF_LE, 243 NG_HCI_OCF_LE_READ_LOCAL_SUPPORTED_FEATURES), 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 le_features.raw = rp.le_features; 254 255 fprintf(stdout, "LE Features: "); 256 for(int i = 0; i < 8; i++) 257 fprintf(stdout, " %#02x", le_features.octets[i]); 258 fprintf(stdout, "\n%s\n", hci_le_features2str(le_features.octets, 259 buffer, sizeof(buffer))); 260 fprintf(stdout, "\n"); 261 262 return OK; 263} 264 265static int 266le_read_supported_states(int s, int argc, char *argv[]) 267{ 268 ng_hci_le_read_supported_states_rp rp; 269 int n = sizeof(rp); 270 271 if (hci_simple_request(s, NG_HCI_OPCODE( 272 NG_HCI_OGF_LE, 273 NG_HCI_OCF_LE_READ_SUPPORTED_STATES), 274 (void *)&rp, &n) == ERROR) 275 return (ERROR); 276 277 if (rp.status != 0x00) { 278 fprintf(stdout, "Status: %s [%#02x]\n", 279 hci_status2str(rp.status), rp.status); 280 return (FAILED); 281 } 282 283 fprintf(stdout, "LE States: %jx\n", rp.le_states); 284 285 return (OK); 286} 287 288static int 289set_le_event_mask(int s, uint64_t mask) 290{ 291 ng_hci_le_set_event_mask_cp semc; 292 ng_hci_le_set_event_mask_rp rp; 293 int i, n ,e; 294 295 n = sizeof(rp); 296 297 for (i=0; i < NG_HCI_LE_EVENT_MASK_SIZE; i++) { 298 semc.event_mask[i] = mask&0xff; 299 mask >>= 8; 300 } 301 e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 302 NG_HCI_OCF_LE_SET_EVENT_MASK), 303 (void *)&semc, sizeof(semc), (void *)&rp, &n); 304 305 return 0; 306} 307 308static int 309set_event_mask(int s, uint64_t mask) 310{ 311 ng_hci_set_event_mask_cp semc; 312 ng_hci_set_event_mask_rp rp; 313 int i, n, e; 314 315 n = sizeof(rp); 316 317 for (i=0; i < NG_HCI_EVENT_MASK_SIZE; i++) { 318 semc.event_mask[i] = mask&0xff; 319 mask >>= 8; 320 } 321 e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_HC_BASEBAND, 322 NG_HCI_OCF_SET_EVENT_MASK), 323 (void *)&semc, sizeof(semc), (void *)&rp, &n); 324 325 return 0; 326} 327 328static 329int le_enable(int s, int argc, char *argv[]) 330{ 331 if (argc != 1) 332 return USAGE; 333 334 if (strcasecmp(argv[0], "enable") == 0) { 335 set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT | 336 NG_HCI_EVENT_MASK_LE); 337 set_le_event_mask(s, NG_HCI_LE_EVENT_MASK_ALL); 338 } else if (strcasecmp(argv[0], "disable") == 0) 339 set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT); 340 else 341 return USAGE; 342 343 return OK; 344} 345 346static int 347le_set_advertising_enable(int s, int argc, char *argv[]) 348{ 349 ng_hci_le_set_advertise_enable_cp cp; 350 ng_hci_le_set_advertise_enable_rp rp; 351 int n, enable = 0; 352 353 if (argc != 1) 354 return USAGE; 355 356 if (strcmp(argv[0], "enable") == 0) 357 enable = 1; 358 else if (strcmp(argv[0], "disable") != 0) 359 return USAGE; 360 361 n = sizeof(rp); 362 cp.advertising_enable = enable; 363 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 364 NG_HCI_OCF_LE_SET_ADVERTISE_ENABLE), 365 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 366 return (ERROR); 367 368 if (rp.status != 0x00) { 369 fprintf(stdout, "Status: %s [%#02x]\n", 370 hci_status2str(rp.status), rp.status); 371 return (FAILED); 372 } 373 fprintf(stdout, "LE Advertising %s\n", (enable ? "enabled" : "disabled")); 374 375 return (OK); 376} 377 378static int 379le_set_advertising_param(int s, int argc, char *argv[]) 380{ 381 ng_hci_le_set_advertising_parameters_cp cp; 382 ng_hci_le_set_advertising_parameters_rp rp; 383 384 int n, ch; 385 386 cp.advertising_interval_min = 0x800; 387 cp.advertising_interval_max = 0x800; 388 cp.advertising_type = 0; 389 cp.own_address_type = 0; 390 cp.direct_address_type = 0; 391 392 cp.advertising_channel_map = 7; 393 cp.advertising_filter_policy = 0; 394 395 optreset = 1; 396 optind = 0; 397 while ((ch = getopt(argc, argv , "m:M:t:o:p:a:c:f:")) != -1) { 398 switch(ch) { 399 case 'm': 400 cp.advertising_interval_min = 401 (uint16_t)(strtod(optarg, NULL)/0.625); 402 break; 403 case 'M': 404 cp.advertising_interval_max = 405 (uint16_t)(strtod(optarg, NULL)/0.625); 406 break; 407 case 't': 408 cp.advertising_type = 409 (uint8_t)strtod(optarg, NULL); 410 break; 411 case 'o': 412 cp.own_address_type = 413 (uint8_t)strtod(optarg, NULL); 414 break; 415 case 'p': 416 cp.direct_address_type = 417 (uint8_t)strtod(optarg, NULL); 418 break; 419 case 'a': 420 if (!bt_aton(optarg, &cp.direct_address)) { 421 struct hostent *he = NULL; 422 423 if ((he = bt_gethostbyname(optarg)) == NULL) 424 return (USAGE); 425 426 memcpy(&cp.direct_address, he->h_addr, sizeof(cp.direct_address)); 427 } 428 break; 429 case 'c': 430 cp.advertising_channel_map = 431 (uint8_t)strtod(optarg, NULL); 432 break; 433 case 'f': 434 cp.advertising_filter_policy = 435 (uint8_t)strtod(optarg, NULL); 436 break; 437 } 438 } 439 440 n = sizeof(rp); 441 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 442 NG_HCI_OCF_LE_SET_ADVERTISING_PARAMETERS), 443 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 444 return (ERROR); 445 446 if (rp.status != 0x00) { 447 fprintf(stdout, "Status: %s [%#02x]\n", 448 hci_status2str(rp.status), rp.status); 449 return (FAILED); 450 } 451 452 return (OK); 453} 454 455static int 456le_read_advertising_channel_tx_power(int s, int argc, char *argv[]) 457{ 458 ng_hci_le_read_advertising_channel_tx_power_rp rp; 459 int n; 460 461 n = sizeof(rp); 462 463 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 464 NG_HCI_OCF_LE_READ_ADVERTISING_CHANNEL_TX_POWER), 465 (void *)&rp, &n) == ERROR) 466 return (ERROR); 467 468 if (rp.status != 0x00) { 469 fprintf(stdout, "Status: %s [%#02x]\n", 470 hci_status2str(rp.status), rp.status); 471 return (FAILED); 472 } 473 474 fprintf(stdout, "Advertising transmit power level: %d dBm\n", 475 (int8_t)rp.transmit_power_level); 476 477 return (OK); 478} 479 480static int 481le_set_advertising_data(int s, int argc, char *argv[]) 482{ 483 ng_hci_le_set_advertising_data_cp cp; 484 ng_hci_le_set_advertising_data_rp rp; 485 int n, len; 486 487 n = sizeof(rp); 488 489 char buf[NG_HCI_ADVERTISING_DATA_SIZE]; 490 491 len = sizeof(buf); 492 parse_param(argc, argv, buf, &len); 493 memset(cp.advertising_data, 0, sizeof(cp.advertising_data)); 494 cp.advertising_data_length = len; 495 memcpy(cp.advertising_data, buf, len); 496 497 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 498 NG_HCI_OCF_LE_SET_ADVERTISING_DATA), 499 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) 500 return (ERROR); 501 502 if (rp.status != 0x00) { 503 fprintf(stdout, "Status: %s [%#02x]\n", 504 hci_status2str(rp.status), rp.status); 505 return (FAILED); 506 } 507 508 return (OK); 509} 510 511struct hci_command le_commands[] = { 512{ 513 "le_enable", 514 "le_enable [enable|disable] \n" 515 "Enable LE event ", 516 &le_enable, 517}, 518 { 519 "le_read_local_supported_features", 520 "le_read_local_supported_features\n" 521 "read local supported features mask", 522 &le_read_local_supported_features, 523 }, 524 { 525 "le_read_supported_states", 526 "le_read_supported_states\n" 527 "read supported status" 528 , 529 &le_read_supported_states, 530 }, 531 { 532 "le_set_scan_response", 533 "le_set_scan_response -n $name -f $flag -u $uuid16,$uuid16 \n" 534 "set LE scan response data" 535 , 536 &le_set_scan_response, 537 }, 538 { 539 "le_set_scan_enable", 540 "le_set_scan_enable [enable|disable] \n" 541 "enable or disable LE device scan", 542 &le_set_scan_enable 543 }, 544 { 545 "le_set_scan_param", 546 "le_set_scan_param [active|passive] interval(ms) window(ms) [public|random] [all|whitelist] \n" 547 "set LE device scan parameter", 548 &le_set_scan_param 549 }, 550 { 551 "le_set_advertising_enable", 552 "le_set_advertising_enable [enable|disable] \n" 553 "start or stop advertising", 554 &le_set_advertising_enable 555 }, 556 { 557 "le_read_advertising_channel_tx_power", 558 "le_read_advertising_channel_tx_power\n" 559 "read host advertising transmit poser level (dBm)", 560 &le_read_advertising_channel_tx_power 561 }, 562 { 563 "le_set_advertising_param", 564 "le_set_advertising_param [-m min_interval(ms)] [-M max_interval(ms)]\n" 565 "[-t advertising_type] [-o own_address_type] [-p peer_address_type]\n" 566 "[-c advertising_channel_map] [-f advertising_filter_policy]\n" 567 "[-a peer_address]\n" 568 "set LE device advertising parameters", 569 &le_set_advertising_param 570 }, 571 { 572 "le_set_advertising_data", 573 "le_set_advertising_data -n $name -f $flag -u $uuid16,$uuid16 \n" 574 "set LE device advertising packed data", 575 &le_set_advertising_data 576 }, 577}; 578