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: releng/11.0/usr.sbin/bluetooth/hccontrol/le.c 299090 2016-05-04 22:34:11Z asomers $ 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_status(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[]); 60 61static int 62le_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 116static int 117le_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 144static int 145parse_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 } 193done: 194 *len = curbuf - buf; 195 196 return OK; 197} 198 199static int 200le_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 224static int 225le_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 242static int 243le_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 259static int 260set_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 279static int 280set_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 299static 300int 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 317struct 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