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/bitstring.h> 36 #include <sys/select.h> 37 #include <assert.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_status(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 e; 229 int n = sizeof(rp); 230 231 e = hci_simple_request(s, 232 NG_HCI_OPCODE(NG_HCI_OGF_LE, 233 NG_HCI_OCF_LE_READ_LOCAL_SUPPORTED_FEATURES), 234 (void *)&rp, &n); 235 236 printf("LOCAL SUPPORTED: %d %d %jx\n", e, rp.status, 237 (uintmax_t) rp.le_features); 238 239 return 0; 240 } 241 242 static int 243 le_read_supported_status(int s, int argc, char *argv[]) 244 { 245 ng_hci_le_read_supported_status_rp rp; 246 int e; 247 int n = sizeof(rp); 248 249 e = hci_simple_request(s, NG_HCI_OPCODE( 250 NG_HCI_OGF_LE, 251 NG_HCI_OCF_LE_READ_SUPPORTED_STATUS), 252 (void *)&rp, &n); 253 254 printf("LE_STATUS: %d %d %jx\n", e, rp.status, (uintmax_t)rp.le_status); 255 256 return 0; 257 } 258 259 static int 260 set_le_event_mask(int s, uint64_t mask) 261 { 262 ng_hci_le_set_event_mask_cp semc; 263 ng_hci_le_set_event_mask_rp rp; 264 int i, n ,e; 265 266 n = sizeof(rp); 267 268 for (i=0; i < NG_HCI_LE_EVENT_MASK_SIZE; i++) { 269 semc.event_mask[i] = mask&0xff; 270 mask >>= 8; 271 } 272 e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, 273 NG_HCI_OCF_LE_SET_EVENT_MASK), 274 (void *)&semc, sizeof(semc), (void *)&rp, &n); 275 276 return 0; 277 } 278 279 static int 280 set_event_mask(int s, uint64_t mask) 281 { 282 ng_hci_set_event_mask_cp semc; 283 ng_hci_set_event_mask_rp rp; 284 int i, n, e; 285 286 n = sizeof(rp); 287 288 for (i=0; i < NG_HCI_EVENT_MASK_SIZE; i++) { 289 semc.event_mask[i] = mask&0xff; 290 mask >>= 8; 291 } 292 e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_HC_BASEBAND, 293 NG_HCI_OCF_SET_EVENT_MASK), 294 (void *)&semc, sizeof(semc), (void *)&rp, &n); 295 296 return 0; 297 } 298 299 static 300 int le_enable(int s, int argc, char *argv[]) 301 { 302 if (argc != 1) 303 return USAGE; 304 305 if (strcasecmp(argv[0], "enable") == 0) { 306 set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT | 307 NG_HCI_EVENT_MASK_LE); 308 set_le_event_mask(s, NG_HCI_LE_EVENT_MASK_ALL); 309 } else if (strcasecmp(argv[0], "disble") == 0) 310 set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT); 311 else 312 return USAGE; 313 314 return OK; 315 } 316 317 struct hci_command le_commands[] = { 318 { 319 "le_enable", 320 "le_enable [enable|disable] \n" 321 "Enable LE event ", 322 &le_enable, 323 }, 324 { 325 "le_read_local_supported_features", 326 "le_read_local_supported_features\n" 327 "read local supported features mask", 328 &le_read_local_supported_features, 329 }, 330 { 331 "le_read_supported_status", 332 "le_read_supported_status\n" 333 "read supported status" 334 , 335 &le_read_supported_status, 336 }, 337 { 338 "le_set_scan_response", 339 "le_set_scan_response -n $name -f $flag -u $uuid16,$uuid16 \n" 340 "set LE scan response data" 341 , 342 &le_set_scan_response, 343 }, 344 { 345 "le_set_scan_enable", 346 "le_set_scan_enable [enable|disable] \n" 347 "enable or disable LE device scan", 348 &le_set_scan_enable 349 }, 350 { 351 "le_set_scan_param", 352 "le_set_scan_param [active|passive] interval(ms) window(ms) [public|random] [all|whitelist] \n" 353 "set LE device scan parameter", 354 &le_set_scan_param 355 }, 356 }; 357