usr.sbin/bluetooth/hccontrol/le.c

   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_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 };