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