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 61 static int 62 le_set_scan_param(int s, int argc, char *argv[]) 63 { 64 int type; 65 int interval; 66 int window; 67 int adrtype; 68 int policy; 69 int e, n; 70 71 ng_hci_le_set_scan_parameters_cp cp; 72 ng_hci_le_set_scan_parameters_rp rp; 73 74 if (argc != 5) 75 return USAGE; 76 77 if (strcmp(argv[0], "active") == 0) 78 type = 1; 79 else if (strcmp(argv[0], "passive") == 0) 80 type = 0; 81 else 82 return USAGE; 83 84 interval = (int)(atof(argv[1])/0.625); 85 interval = (interval < 4)? 4: interval; 86 window = (int)(atof(argv[2])/0.625); 87 window = (window < 4) ? 4 : interval; 88 89 if (strcmp(argv[3], "public") == 0) 90 adrtype = 0; 91 else if (strcmp(argv[3], "random") == 0) 92 adrtype = 1; 93 else 94 return USAGE; 95 96 if (strcmp(argv[4], "all") == 0) 97 policy = 0; 98 else if (strcmp(argv[4], "whitelist") == 0) 99 policy = 1; 100 else 101 return USAGE; 102 103 cp.le_scan_type = type; 104 cp.le_scan_interval = interval; 105 cp.own_address_type = adrtype; 106 cp.le_scan_window = window; 107 cp.scanning_filter_policy = policy; 108 n = sizeof(rp); 109 e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 110 NG_HCI_OCF_LE_SET_SCAN_PARAMETERS), 111 (void *)&cp, sizeof(cp), (void *)&rp, &n); 112 113 return 0; 114 } 115 116 static int 117 le_set_scan_enable(int s, int argc, char *argv[]) 118 { 119 ng_hci_le_set_scan_enable_cp cp; 120 ng_hci_le_set_scan_enable_rp rp; 121 int e, n, enable = 0; 122 123 if (argc != 1) 124 return USAGE; 125 126 if (strcmp(argv[0], "enable") == 0) 127 enable = 1; 128 else if (strcmp(argv[0], "disable") != 0) 129 return USAGE; 130 131 n = sizeof(rp); 132 cp.le_scan_enable = enable; 133 cp.filter_duplicates = 0; 134 e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 135 NG_HCI_OCF_LE_SET_SCAN_ENABLE), 136 (void *)&cp, sizeof(cp), (void *)&rp, &n); 137 138 if (e != 0 || rp.status != 0) 139 return ERROR; 140 141 return OK; 142 } 143 144 static int 145 parse_param(int argc, char *argv[], char *buf, int *len) 146 { 147 char *buflast = buf + (*len); 148 char *curbuf = buf; 149 char *token,*lenpos; 150 int ch; 151 int datalen; 152 uint16_t value; 153 optreset = 1; 154 optind = 0; 155 while ((ch = getopt(argc, argv , "n:f:u:")) != -1) { 156 switch(ch){ 157 case 'n': 158 datalen = strlen(optarg); 159 if ((curbuf + datalen + 2) >= buflast) 160 goto done; 161 curbuf[0] = datalen + 1; 162 curbuf[1] = 8; 163 curbuf += 2; 164 memcpy(curbuf, optarg, datalen); 165 curbuf += datalen; 166 break; 167 case 'f': 168 if (curbuf+3 > buflast) 169 goto done; 170 curbuf[0] = 2; 171 curbuf[1] = 1; 172 curbuf[2] = atoi(optarg); 173 curbuf += 3; 174 break; 175 case 'u': 176 lenpos = buf; 177 if ((buf+2) >= buflast) 178 goto done; 179 curbuf[1] = 2; 180 *lenpos = 1; 181 curbuf += 2; 182 while ((token = strsep(&optarg, ",")) != NULL) { 183 value = strtol(token, NULL, 16); 184 if ((curbuf+2) >= buflast) 185 break; 186 curbuf[0] = value &0xff; 187 curbuf[1] = (value>>8)&0xff; 188 curbuf += 2; 189 } 190 191 } 192 } 193 done: 194 *len = curbuf - buf; 195 196 return OK; 197 } 198 199 static int 200 le_set_scan_response(int s, int argc, char *argv[]) 201 { 202 ng_hci_le_set_scan_response_data_cp cp; 203 ng_hci_le_set_scan_response_data_rp rp; 204 int n; 205 int e; 206 int len; 207 char buf[NG_HCI_ADVERTISING_DATA_SIZE]; 208 209 len = sizeof(buf); 210 parse_param(argc, argv, buf, &len); 211 memset(cp.scan_response_data, 0, sizeof(cp.scan_response_data)); 212 cp.scan_response_data_length = len; 213 memcpy(cp.scan_response_data, buf, len); 214 n = sizeof(rp); 215 e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 216 NG_HCI_OCF_LE_SET_SCAN_RESPONSE_DATA), 217 (void *)&cp, sizeof(cp), (void *)&rp, &n); 218 219 printf("SET SCAN RESPONSE %d %d %d\n", e, rp.status, n); 220 221 return OK; 222 } 223 224 static int 225 le_read_local_supported_features(int s, int argc ,char *argv[]) 226 { 227 ng_hci_le_read_local_supported_features_rp rp; 228 int n = sizeof(rp); 229 230 union { 231 uint64_t raw; 232 uint8_t octets[8]; 233 } le_features; 234 235 char buffer[2048]; 236 237 if (hci_simple_request(s, 238 NG_HCI_OPCODE(NG_HCI_OGF_LE, 239 NG_HCI_OCF_LE_READ_LOCAL_SUPPORTED_FEATURES), 240 (void *)&rp, &n) == ERROR) 241 return (ERROR); 242 243 if (rp.status != 0x00) { 244 fprintf(stdout, "Status: %s [%#02x]\n", 245 hci_status2str(rp.status), rp.status); 246 return (FAILED); 247 } 248 249 le_features.raw = rp.le_features; 250 251 fprintf(stdout, "LE Features: "); 252 for(int i = 0; i < 8; i++) 253 fprintf(stdout, " %#02x", le_features.octets[i]); 254 fprintf(stdout, "\n%s\n", hci_le_features2str(le_features.octets, 255 buffer, sizeof(buffer))); 256 fprintf(stdout, "\n"); 257 258 return OK; 259 } 260 261 static int 262 le_read_supported_states(int s, int argc, char *argv[]) 263 { 264 ng_hci_le_read_supported_states_rp rp; 265 int n = sizeof(rp); 266 267 if (hci_simple_request(s, NG_HCI_OPCODE( 268 NG_HCI_OGF_LE, 269 NG_HCI_OCF_LE_READ_SUPPORTED_STATES), 270 (void *)&rp, &n) == ERROR) 271 return (ERROR); 272 273 if (rp.status != 0x00) { 274 fprintf(stdout, "Status: %s [%#02x]\n", 275 hci_status2str(rp.status), rp.status); 276 return (FAILED); 277 } 278 279 fprintf(stdout, "LE States: %jx\n", rp.le_states); 280 281 return (OK); 282 } 283 284 static int 285 set_le_event_mask(int s, uint64_t mask) 286 { 287 ng_hci_le_set_event_mask_cp semc; 288 ng_hci_le_set_event_mask_rp rp; 289 int i, n ,e; 290 291 n = sizeof(rp); 292 293 for (i=0; i < NG_HCI_LE_EVENT_MASK_SIZE; i++) { 294 semc.event_mask[i] = mask&0xff; 295 mask >>= 8; 296 } 297 e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 298 NG_HCI_OCF_LE_SET_EVENT_MASK), 299 (void *)&semc, sizeof(semc), (void *)&rp, &n); 300 301 return 0; 302 } 303 304 static int 305 set_event_mask(int s, uint64_t mask) 306 { 307 ng_hci_set_event_mask_cp semc; 308 ng_hci_set_event_mask_rp rp; 309 int i, n, e; 310 311 n = sizeof(rp); 312 313 for (i=0; i < NG_HCI_EVENT_MASK_SIZE; i++) { 314 semc.event_mask[i] = mask&0xff; 315 mask >>= 8; 316 } 317 e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_HC_BASEBAND, 318 NG_HCI_OCF_SET_EVENT_MASK), 319 (void *)&semc, sizeof(semc), (void *)&rp, &n); 320 321 return 0; 322 } 323 324 static 325 int le_enable(int s, int argc, char *argv[]) 326 { 327 if (argc != 1) 328 return USAGE; 329 330 if (strcasecmp(argv[0], "enable") == 0) { 331 set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT | 332 NG_HCI_EVENT_MASK_LE); 333 set_le_event_mask(s, NG_HCI_LE_EVENT_MASK_ALL); 334 } else if (strcasecmp(argv[0], "disable") == 0) 335 set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT); 336 else 337 return USAGE; 338 339 return OK; 340 } 341 342 struct hci_command le_commands[] = { 343 { 344 "le_enable", 345 "le_enable [enable|disable] \n" 346 "Enable LE event ", 347 &le_enable, 348 }, 349 { 350 "le_read_local_supported_features", 351 "le_read_local_supported_features\n" 352 "read local supported features mask", 353 &le_read_local_supported_features, 354 }, 355 { 356 "le_read_supported_states", 357 "le_read_supported_states\n" 358 "read supported status" 359 , 360 &le_read_supported_states, 361 }, 362 { 363 "le_set_scan_response", 364 "le_set_scan_response -n $name -f $flag -u $uuid16,$uuid16 \n" 365 "set LE scan response data" 366 , 367 &le_set_scan_response, 368 }, 369 { 370 "le_set_scan_enable", 371 "le_set_scan_enable [enable|disable] \n" 372 "enable or disable LE device scan", 373 &le_set_scan_enable 374 }, 375 { 376 "le_set_scan_param", 377 "le_set_scan_param [active|passive] interval(ms) window(ms) [public|random] [all|whitelist] \n" 378 "set LE device scan parameter", 379 &le_set_scan_param 380 }, 381 }; 382