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 */
30
31 #include <sys/types.h>
32 #include <sys/ioctl.h>
33 #include <sys/sysctl.h>
34 #include <sys/select.h>
35 #include <assert.h>
36 #include <bitstring.h>
37 #include <err.h>
38 #include <errno.h>
39 #include <netgraph/ng_message.h>
40 #include <errno.h>
41 #include <stdbool.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 static int le_scan(int s, int argc, char *argv[]);
64 static void handle_le_event(ng_hci_event_pkt_t* e, bool verbose);
65 static int le_read_white_list_size(int s, int argc, char *argv[]);
66 static int le_clear_white_list(int s, int argc, char *argv[]);
67 static int le_add_device_to_white_list(int s, int argc, char *argv[]);
68 static int le_remove_device_from_white_list(int s, int argc, char *argv[]);
69 static int le_connect(int s, int argc, char *argv[]);
70 static void handle_le_connection_event(ng_hci_event_pkt_t* e, bool verbose);
71 static int le_read_channel_map(int s, int argc, char *argv[]);
72 static void handle_le_remote_features_event(ng_hci_event_pkt_t* e);
73 static int le_rand(int s, int argc, char *argv[]);
74
75 static int
le_set_scan_param(int s,int argc,char * argv[])76 le_set_scan_param(int s, int argc, char *argv[])
77 {
78 int type;
79 int interval;
80 int window;
81 int adrtype;
82 int policy;
83 int n;
84
85 ng_hci_le_set_scan_parameters_cp cp;
86 ng_hci_le_set_scan_parameters_rp rp;
87
88 if (argc != 5)
89 return (USAGE);
90
91 if (strcmp(argv[0], "active") == 0)
92 type = 1;
93 else if (strcmp(argv[0], "passive") == 0)
94 type = 0;
95 else
96 return (USAGE);
97
98 interval = (int)(atof(argv[1])/0.625);
99 interval = (interval < 4)? 4: interval;
100 window = (int)(atof(argv[2])/0.625);
101 window = (window < 4) ? 4 : interval;
102
103 if (strcmp(argv[3], "public") == 0)
104 adrtype = 0;
105 else if (strcmp(argv[3], "random") == 0)
106 adrtype = 1;
107 else
108 return (USAGE);
109
110 if (strcmp(argv[4], "all") == 0)
111 policy = 0;
112 else if (strcmp(argv[4], "whitelist") == 0)
113 policy = 1;
114 else
115 return (USAGE);
116
117 cp.le_scan_type = type;
118 cp.le_scan_interval = interval;
119 cp.own_address_type = adrtype;
120 cp.le_scan_window = window;
121 cp.scanning_filter_policy = policy;
122 n = sizeof(rp);
123
124 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
125 NG_HCI_OCF_LE_SET_SCAN_PARAMETERS),
126 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR)
127 return (ERROR);
128
129 if (rp.status != 0x00) {
130 fprintf(stdout, "Status: %s [%#02x]\n",
131 hci_status2str(rp.status), rp.status);
132 return (FAILED);
133 }
134
135 return (OK);
136 }
137
138 static int
le_set_scan_enable(int s,int argc,char * argv[])139 le_set_scan_enable(int s, int argc, char *argv[])
140 {
141 ng_hci_le_set_scan_enable_cp cp;
142 ng_hci_le_set_scan_enable_rp rp;
143 int n, enable = 0;
144
145 if (argc != 1)
146 return (USAGE);
147
148 if (strcmp(argv[0], "enable") == 0)
149 enable = 1;
150 else if (strcmp(argv[0], "disable") != 0)
151 return (USAGE);
152
153 n = sizeof(rp);
154 cp.le_scan_enable = enable;
155 cp.filter_duplicates = 0;
156 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
157 NG_HCI_OCF_LE_SET_SCAN_ENABLE),
158 (void *)&cp, sizeof(cp),
159 (void *)&rp, &n) == ERROR)
160 return (ERROR);
161
162 if (rp.status != 0x00) {
163 fprintf(stdout, "Status: %s [%#02x]\n",
164 hci_status2str(rp.status), rp.status);
165 return (FAILED);
166 }
167
168 fprintf(stdout, "LE Scan: %s\n",
169 enable? "Enabled" : "Disabled");
170
171 return (OK);
172 }
173
174 static int
parse_param(int argc,char * argv[],char * buf,int * len)175 parse_param(int argc, char *argv[], char *buf, int *len)
176 {
177 char *buflast = buf + (*len);
178 char *curbuf = buf;
179 char *token,*lenpos;
180 int ch;
181 int datalen;
182 uint16_t value;
183 optreset = 1;
184 optind = 0;
185 while ((ch = getopt(argc, argv , "n:f:u:")) != -1) {
186 switch(ch){
187 case 'n':
188 datalen = strlen(optarg);
189 if ((curbuf + datalen + 2) >= buflast)
190 goto done;
191 curbuf[0] = datalen + 1;
192 curbuf[1] = 8;
193 curbuf += 2;
194 memcpy(curbuf, optarg, datalen);
195 curbuf += datalen;
196 break;
197 case 'f':
198 if (curbuf+3 > buflast)
199 goto done;
200 curbuf[0] = 2;
201 curbuf[1] = 1;
202 curbuf[2] = (uint8_t)strtol(optarg, NULL, 16);
203 curbuf += 3;
204 break;
205 case 'u':
206 if ((buf+2) >= buflast)
207 goto done;
208 lenpos = curbuf;
209 curbuf[1] = 2;
210 *lenpos = 1;
211 curbuf += 2;
212 while ((token = strsep(&optarg, ",")) != NULL) {
213 value = strtol(token, NULL, 16);
214 if ((curbuf+2) >= buflast)
215 break;
216 curbuf[0] = value &0xff;
217 curbuf[1] = (value>>8)&0xff;
218 curbuf += 2;
219 *lenpos += 2;
220 }
221
222 }
223 }
224 done:
225 *len = curbuf - buf;
226
227 return (OK);
228 }
229
230 static int
le_set_scan_response(int s,int argc,char * argv[])231 le_set_scan_response(int s, int argc, char *argv[])
232 {
233 ng_hci_le_set_scan_response_data_cp cp;
234 ng_hci_le_set_scan_response_data_rp rp;
235 int n;
236 int len;
237 char buf[NG_HCI_ADVERTISING_DATA_SIZE];
238
239 len = sizeof(buf);
240 parse_param(argc, argv, buf, &len);
241 memset(cp.scan_response_data, 0, sizeof(cp.scan_response_data));
242 cp.scan_response_data_length = len;
243 memcpy(cp.scan_response_data, buf, len);
244 n = sizeof(rp);
245 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
246 NG_HCI_OCF_LE_SET_SCAN_RESPONSE_DATA),
247 (void *)&cp, sizeof(cp),
248 (void *)&rp, &n) == ERROR)
249 return (ERROR);
250
251 if (rp.status != 0x00) {
252 fprintf(stdout, "Status: %s [%#02x]\n",
253 hci_status2str(rp.status), rp.status);
254 return (FAILED);
255 }
256
257 return (OK);
258 }
259
260 static int
le_read_local_supported_features(int s,int argc,char * argv[])261 le_read_local_supported_features(int s, int argc ,char *argv[])
262 {
263 ng_hci_le_read_local_supported_features_rp rp;
264 int n = sizeof(rp);
265
266 union {
267 uint64_t raw;
268 uint8_t octets[8];
269 } le_features;
270
271 char buffer[2048];
272
273 if (hci_simple_request(s,
274 NG_HCI_OPCODE(NG_HCI_OGF_LE,
275 NG_HCI_OCF_LE_READ_LOCAL_SUPPORTED_FEATURES),
276 (void *)&rp, &n) == ERROR)
277 return (ERROR);
278
279 if (rp.status != 0x00) {
280 fprintf(stdout, "Status: %s [%#02x]\n",
281 hci_status2str(rp.status), rp.status);
282 return (FAILED);
283 }
284
285 le_features.raw = rp.le_features;
286
287 fprintf(stdout, "LE Features: ");
288 for(int i = 0; i < 8; i++)
289 fprintf(stdout, " %#02x", le_features.octets[i]);
290 fprintf(stdout, "\n%s\n", hci_le_features2str(le_features.octets,
291 buffer, sizeof(buffer)));
292 fprintf(stdout, "\n");
293
294 return (OK);
295 }
296
297 static int
le_read_supported_states(int s,int argc,char * argv[])298 le_read_supported_states(int s, int argc, char *argv[])
299 {
300 ng_hci_le_read_supported_states_rp rp;
301 int n = sizeof(rp);
302
303 if (hci_simple_request(s, NG_HCI_OPCODE(
304 NG_HCI_OGF_LE,
305 NG_HCI_OCF_LE_READ_SUPPORTED_STATES),
306 (void *)&rp, &n) == ERROR)
307 return (ERROR);
308
309 if (rp.status != 0x00) {
310 fprintf(stdout, "Status: %s [%#02x]\n",
311 hci_status2str(rp.status), rp.status);
312 return (FAILED);
313 }
314
315 fprintf(stdout, "LE States: %jx\n", rp.le_states);
316
317 return (OK);
318 }
319
320 static int
set_le_event_mask(int s,uint64_t mask)321 set_le_event_mask(int s, uint64_t mask)
322 {
323 ng_hci_le_set_event_mask_cp semc;
324 ng_hci_le_set_event_mask_rp rp;
325 int i, n;
326
327 n = sizeof(rp);
328
329 for (i=0; i < NG_HCI_LE_EVENT_MASK_SIZE; i++) {
330 semc.event_mask[i] = mask&0xff;
331 mask >>= 8;
332 }
333 if(hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
334 NG_HCI_OCF_LE_SET_EVENT_MASK),
335 (void *)&semc, sizeof(semc), (void *)&rp, &n) == ERROR)
336 return (ERROR);
337
338 if (rp.status != 0x00) {
339 fprintf(stdout, "Status: %s [%#02x]\n",
340 hci_status2str(rp.status), rp.status);
341 return (FAILED);
342 }
343
344 return (OK);
345 }
346
347 static int
set_event_mask(int s,uint64_t mask)348 set_event_mask(int s, uint64_t mask)
349 {
350 ng_hci_set_event_mask_cp semc;
351 ng_hci_set_event_mask_rp rp;
352 int i, n;
353
354 n = sizeof(rp);
355
356 for (i=0; i < NG_HCI_EVENT_MASK_SIZE; i++) {
357 semc.event_mask[i] = mask&0xff;
358 mask >>= 8;
359 }
360 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_HC_BASEBAND,
361 NG_HCI_OCF_SET_EVENT_MASK),
362 (void *)&semc, sizeof(semc), (void *)&rp, &n) == ERROR)
363 return (ERROR);
364
365 if (rp.status != 0x00) {
366 fprintf(stdout, "Status: %s [%#02x]\n",
367 hci_status2str(rp.status), rp.status);
368 return (FAILED);
369 }
370
371 return (OK);
372 }
373
374 static
le_enable(int s,int argc,char * argv[])375 int le_enable(int s, int argc, char *argv[])
376 {
377 int result;
378
379 if (argc != 1)
380 return (USAGE);
381
382 if (strcasecmp(argv[0], "enable") == 0) {
383 result = set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT |
384 NG_HCI_EVENT_MASK_LE);
385 if (result != OK)
386 return result;
387 result = set_le_event_mask(s, NG_HCI_LE_EVENT_MASK_ALL);
388 if (result == OK) {
389 fprintf(stdout, "LE enabled\n");
390 return (OK);
391 } else
392 return result;
393 } else if (strcasecmp(argv[0], "disable") == 0) {
394 result = set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT);
395 if (result == OK) {
396 fprintf(stdout, "LE disabled\n");
397 return (OK);
398 } else
399 return result;
400 } else
401 return (USAGE);
402 }
403
404 static int
le_set_advertising_enable(int s,int argc,char * argv[])405 le_set_advertising_enable(int s, int argc, char *argv[])
406 {
407 ng_hci_le_set_advertise_enable_cp cp;
408 ng_hci_le_set_advertise_enable_rp rp;
409 int n, enable = 0;
410
411 if (argc != 1)
412 return USAGE;
413
414 if (strcmp(argv[0], "enable") == 0)
415 enable = 1;
416 else if (strcmp(argv[0], "disable") != 0)
417 return USAGE;
418
419 n = sizeof(rp);
420 cp.advertising_enable = enable;
421 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
422 NG_HCI_OCF_LE_SET_ADVERTISE_ENABLE),
423 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR)
424 return (ERROR);
425
426 if (rp.status != 0x00) {
427 fprintf(stdout, "Status: %s [%#02x]\n",
428 hci_status2str(rp.status), rp.status);
429 return (FAILED);
430 }
431 fprintf(stdout, "LE Advertising %s\n", (enable ? "enabled" : "disabled"));
432
433 return (OK);
434 }
435
436 static int
le_set_advertising_param(int s,int argc,char * argv[])437 le_set_advertising_param(int s, int argc, char *argv[])
438 {
439 ng_hci_le_set_advertising_parameters_cp cp;
440 ng_hci_le_set_advertising_parameters_rp rp;
441
442 int n, ch;
443
444 cp.advertising_interval_min = 0x800;
445 cp.advertising_interval_max = 0x800;
446 cp.advertising_type = 0;
447 cp.own_address_type = 0;
448 cp.direct_address_type = 0;
449
450 cp.advertising_channel_map = 7;
451 cp.advertising_filter_policy = 0;
452
453 optreset = 1;
454 optind = 0;
455 while ((ch = getopt(argc, argv , "m:M:t:o:p:a:c:f:")) != -1) {
456 switch(ch) {
457 case 'm':
458 cp.advertising_interval_min =
459 (uint16_t)(strtod(optarg, NULL)/0.625);
460 break;
461 case 'M':
462 cp.advertising_interval_max =
463 (uint16_t)(strtod(optarg, NULL)/0.625);
464 break;
465 case 't':
466 cp.advertising_type =
467 (uint8_t)strtod(optarg, NULL);
468 break;
469 case 'o':
470 cp.own_address_type =
471 (uint8_t)strtod(optarg, NULL);
472 break;
473 case 'p':
474 cp.direct_address_type =
475 (uint8_t)strtod(optarg, NULL);
476 break;
477 case 'a':
478 if (!bt_aton(optarg, &cp.direct_address)) {
479 struct hostent *he = NULL;
480
481 if ((he = bt_gethostbyname(optarg)) == NULL)
482 return (USAGE);
483
484 memcpy(&cp.direct_address, he->h_addr, sizeof(cp.direct_address));
485 }
486 break;
487 case 'c':
488 cp.advertising_channel_map =
489 (uint8_t)strtod(optarg, NULL);
490 break;
491 case 'f':
492 cp.advertising_filter_policy =
493 (uint8_t)strtod(optarg, NULL);
494 break;
495 }
496 }
497
498 n = sizeof(rp);
499 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
500 NG_HCI_OCF_LE_SET_ADVERTISING_PARAMETERS),
501 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR)
502 return (ERROR);
503
504 if (rp.status != 0x00) {
505 fprintf(stdout, "Status: %s [%#02x]\n",
506 hci_status2str(rp.status), rp.status);
507 return (FAILED);
508 }
509
510 return (OK);
511 }
512
513 static int
le_read_advertising_channel_tx_power(int s,int argc,char * argv[])514 le_read_advertising_channel_tx_power(int s, int argc, char *argv[])
515 {
516 ng_hci_le_read_advertising_channel_tx_power_rp rp;
517 int n;
518
519 n = sizeof(rp);
520
521 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
522 NG_HCI_OCF_LE_READ_ADVERTISING_CHANNEL_TX_POWER),
523 (void *)&rp, &n) == ERROR)
524 return (ERROR);
525
526 if (rp.status != 0x00) {
527 fprintf(stdout, "Status: %s [%#02x]\n",
528 hci_status2str(rp.status), rp.status);
529 return (FAILED);
530 }
531
532 fprintf(stdout, "Advertising transmit power level: %d dBm\n",
533 (int8_t)rp.transmit_power_level);
534
535 return (OK);
536 }
537
538 static int
le_set_advertising_data(int s,int argc,char * argv[])539 le_set_advertising_data(int s, int argc, char *argv[])
540 {
541 ng_hci_le_set_advertising_data_cp cp;
542 ng_hci_le_set_advertising_data_rp rp;
543 int n, len;
544
545 n = sizeof(rp);
546
547 char buf[NG_HCI_ADVERTISING_DATA_SIZE];
548
549 len = sizeof(buf);
550 parse_param(argc, argv, buf, &len);
551 memset(cp.advertising_data, 0, sizeof(cp.advertising_data));
552 cp.advertising_data_length = len;
553 memcpy(cp.advertising_data, buf, len);
554
555 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
556 NG_HCI_OCF_LE_SET_ADVERTISING_DATA),
557 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR)
558 return (ERROR);
559
560 if (rp.status != 0x00) {
561 fprintf(stdout, "Status: %s [%#02x]\n",
562 hci_status2str(rp.status), rp.status);
563 return (FAILED);
564 }
565
566 return (OK);
567 }
568 static int
le_read_buffer_size(int s,int argc,char * argv[])569 le_read_buffer_size(int s, int argc, char *argv[])
570 {
571 union {
572 ng_hci_le_read_buffer_size_rp v1;
573 ng_hci_le_read_buffer_size_rp_v2 v2;
574 } rp;
575
576 int n, ch;
577 uint8_t v;
578 uint16_t cmd;
579
580 optreset = 1;
581 optind = 0;
582
583 /* Default to version 1*/
584 v = 1;
585 cmd = NG_HCI_OCF_LE_READ_BUFFER_SIZE;
586
587 while ((ch = getopt(argc, argv , "v:")) != -1) {
588 switch(ch) {
589 case 'v':
590 v = (uint8_t)strtol(optarg, NULL, 16);
591 if (v == 2)
592 cmd = NG_HCI_OCF_LE_READ_BUFFER_SIZE_V2;
593 else if (v > 2)
594 return (USAGE);
595 break;
596 default:
597 v = 1;
598 }
599 }
600
601 n = sizeof(rp);
602 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, cmd),
603 (void *)&rp, &n) == ERROR)
604 return (ERROR);
605
606 if (rp.v1.status != 0x00) {
607 fprintf(stdout, "Status: %s [%#02x]\n",
608 hci_status2str(rp.v1.status), rp.v1.status);
609 return (FAILED);
610 }
611
612 fprintf(stdout, "ACL data packet length: %d\n",
613 rp.v1.hc_le_data_packet_length);
614 fprintf(stdout, "Number of ACL data packets: %d\n",
615 rp.v1.hc_total_num_le_data_packets);
616
617 if (v == 2) {
618 fprintf(stdout, "ISO data packet length: %d\n",
619 rp.v2.hc_iso_data_packet_length);
620 fprintf(stdout, "Number of ISO data packets: %d\n",
621 rp.v2.hc_total_num_iso_data_packets);
622 }
623
624 return (OK);
625 }
626
627 static int
le_scan(int s,int argc,char * argv[])628 le_scan(int s, int argc, char *argv[])
629 {
630 int n, bufsize, scancount, numscans;
631 bool verbose;
632 uint8_t active = 0;
633 char ch;
634
635 char b[512];
636 ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b;
637
638 ng_hci_le_set_scan_parameters_cp scan_param_cp;
639 ng_hci_le_set_scan_parameters_rp scan_param_rp;
640
641 ng_hci_le_set_scan_enable_cp scan_enable_cp;
642 ng_hci_le_set_scan_enable_rp scan_enable_rp;
643
644 optreset = 1;
645 optind = 0;
646 verbose = false;
647 numscans = 1;
648
649 while ((ch = getopt(argc, argv , "an:v")) != -1) {
650 switch(ch) {
651 case 'a':
652 active = 1;
653 break;
654 case 'n':
655 numscans = (uint8_t)strtol(optarg, NULL, 10);
656 break;
657 case 'v':
658 verbose = true;
659 break;
660 }
661 }
662
663 scan_param_cp.le_scan_type = active;
664 scan_param_cp.le_scan_interval = (uint16_t)(100/0.625);
665 scan_param_cp.le_scan_window = (uint16_t)(50/0.625);
666 /* Address type public */
667 scan_param_cp.own_address_type = 0;
668 /* 'All' filter policy */
669 scan_param_cp.scanning_filter_policy = 0;
670 n = sizeof(scan_param_rp);
671
672 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
673 NG_HCI_OCF_LE_SET_SCAN_PARAMETERS),
674 (void *)&scan_param_cp, sizeof(scan_param_cp),
675 (void *)&scan_param_rp, &n) == ERROR)
676 return (ERROR);
677
678 if (scan_param_rp.status != 0x00) {
679 fprintf(stdout, "LE_Set_Scan_Parameters failed. Status: %s [%#02x]\n",
680 hci_status2str(scan_param_rp.status),
681 scan_param_rp.status);
682 return (FAILED);
683 }
684
685 /* Enable scanning */
686 n = sizeof(scan_enable_rp);
687 scan_enable_cp.le_scan_enable = 1;
688 scan_enable_cp.filter_duplicates = 1;
689 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
690 NG_HCI_OCF_LE_SET_SCAN_ENABLE),
691 (void *)&scan_enable_cp, sizeof(scan_enable_cp),
692 (void *)&scan_enable_rp, &n) == ERROR)
693 return (ERROR);
694
695 if (scan_enable_rp.status != 0x00) {
696 fprintf(stdout, "LE_Scan_Enable enable failed. Status: %s [%#02x]\n",
697 hci_status2str(scan_enable_rp.status),
698 scan_enable_rp.status);
699 return (FAILED);
700 }
701
702 scancount = 0;
703 while (scancount < numscans) {
704 /* wait for scan events */
705 bufsize = sizeof(b);
706 if (hci_recv(s, b, &bufsize) == ERROR) {
707 return (ERROR);
708 }
709
710 if (bufsize < sizeof(*e)) {
711 errno = EIO;
712 return (ERROR);
713 }
714 scancount++;
715 if (e->event == NG_HCI_EVENT_LE) {
716 fprintf(stdout, "Scan %d\n", scancount);
717 handle_le_event(e, verbose);
718 }
719 }
720
721 fprintf(stdout, "Scan complete\n");
722
723 /* Disable scanning */
724 n = sizeof(scan_enable_rp);
725 scan_enable_cp.le_scan_enable = 0;
726 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
727 NG_HCI_OCF_LE_SET_SCAN_ENABLE),
728 (void *)&scan_enable_cp, sizeof(scan_enable_cp),
729 (void *)&scan_enable_rp, &n) == ERROR)
730 return (ERROR);
731
732 if (scan_enable_rp.status != 0x00) {
733 fprintf(stdout, "LE_Scan_Enable disable failed. Status: %s [%#02x]\n",
734 hci_status2str(scan_enable_rp.status),
735 scan_enable_rp.status);
736 return (FAILED);
737 }
738
739 return (OK);
740 }
741
handle_le_event(ng_hci_event_pkt_t * e,bool verbose)742 static void handle_le_event(ng_hci_event_pkt_t* e, bool verbose)
743 {
744 int rc;
745 ng_hci_le_ep *leer =
746 (ng_hci_le_ep *)(e + 1);
747 ng_hci_le_advertising_report_ep *advrep =
748 (ng_hci_le_advertising_report_ep *)(leer + 1);
749 ng_hci_le_advreport *reports =
750 (ng_hci_le_advreport *)(advrep + 1);
751
752 if (leer->subevent_code == NG_HCI_LEEV_ADVREP) {
753 fprintf(stdout, "Scan result, num_reports: %d\n",
754 advrep->num_reports);
755 for(rc = 0; rc < advrep->num_reports; rc++) {
756 uint8_t length = (uint8_t)reports[rc].length_data;
757 fprintf(stdout, "\tBD_ADDR %s \n",
758 hci_bdaddr2str(&reports[rc].bdaddr));
759 fprintf(stdout, "\tAddress type: %s\n",
760 hci_addrtype2str(reports[rc].addr_type));
761 if (length > 0 && verbose) {
762 dump_adv_data(length, reports[rc].data);
763 print_adv_data(length, reports[rc].data);
764 fprintf(stdout,
765 "\tRSSI: %d dBm\n",
766 (int8_t)reports[rc].data[length]);
767 fprintf(stdout, "\n");
768 }
769 }
770 }
771 }
772
773 static int
le_read_white_list_size(int s,int argc,char * argv[])774 le_read_white_list_size(int s, int argc, char *argv[])
775 {
776 ng_hci_le_read_white_list_size_rp rp;
777 int n;
778
779 n = sizeof(rp);
780
781 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
782 NG_HCI_OCF_LE_READ_WHITE_LIST_SIZE),
783 (void *)&rp, &n) == ERROR)
784 return (ERROR);
785
786 if (rp.status != 0x00) {
787 fprintf(stdout, "Status: %s [%#02x]\n",
788 hci_status2str(rp.status), rp.status);
789 return (FAILED);
790 }
791
792 fprintf(stdout, "White list size: %d\n",
793 (uint8_t)rp.white_list_size);
794
795 return (OK);
796 }
797
798 static int
le_clear_white_list(int s,int argc,char * argv[])799 le_clear_white_list(int s, int argc, char *argv[])
800 {
801 ng_hci_le_clear_white_list_rp rp;
802 int n;
803
804 n = sizeof(rp);
805
806 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
807 NG_HCI_OCF_LE_CLEAR_WHITE_LIST),
808 (void *)&rp, &n) == ERROR)
809 return (ERROR);
810
811 if (rp.status != 0x00) {
812 fprintf(stdout, "Status: %s [%#02x]\n",
813 hci_status2str(rp.status), rp.status);
814 return (FAILED);
815 }
816
817 fprintf(stdout, "White list cleared\n");
818
819 return (OK);
820 }
821
822 static int
le_add_device_to_white_list(int s,int argc,char * argv[])823 le_add_device_to_white_list(int s, int argc, char *argv[])
824 {
825 ng_hci_le_add_device_to_white_list_cp cp;
826 ng_hci_le_add_device_to_white_list_rp rp;
827 int n;
828 char ch;
829 optreset = 1;
830 optind = 0;
831 bool addr_set = false;
832
833 n = sizeof(rp);
834
835 cp.address_type = 0x00;
836
837 while ((ch = getopt(argc, argv , "t:a:")) != -1) {
838 switch(ch) {
839 case 't':
840 if (strcmp(optarg, "public") == 0)
841 cp.address_type = 0x00;
842 else if (strcmp(optarg, "random") == 0)
843 cp.address_type = 0x01;
844 else
845 return (USAGE);
846 break;
847 case 'a':
848 addr_set = true;
849 if (!bt_aton(optarg, &cp.address)) {
850 struct hostent *he = NULL;
851
852 if ((he = bt_gethostbyname(optarg)) == NULL)
853 return (USAGE);
854
855 memcpy(&cp.address, he->h_addr,
856 sizeof(cp.address));
857 }
858 break;
859 }
860 }
861
862 if (addr_set == false)
863 return (USAGE);
864
865 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
866 NG_HCI_OCF_LE_ADD_DEVICE_TO_WHITE_LIST),
867 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR)
868 return (ERROR);
869
870 if (rp.status != 0x00) {
871 fprintf(stdout, "Status: %s [%#02x]\n",
872 hci_status2str(rp.status), rp.status);
873 return (FAILED);
874 }
875
876 fprintf(stdout, "Address added to white list\n");
877
878 return (OK);
879 }
880
881 static int
le_remove_device_from_white_list(int s,int argc,char * argv[])882 le_remove_device_from_white_list(int s, int argc, char *argv[])
883 {
884 ng_hci_le_remove_device_from_white_list_cp cp;
885 ng_hci_le_remove_device_from_white_list_rp rp;
886 int n;
887 char ch;
888 optreset = 1;
889 optind = 0;
890 bool addr_set = false;
891
892 n = sizeof(rp);
893
894 cp.address_type = 0x00;
895
896 while ((ch = getopt(argc, argv , "t:a:")) != -1) {
897 switch(ch) {
898 case 't':
899 if (strcmp(optarg, "public") == 0)
900 cp.address_type = 0x00;
901 else if (strcmp(optarg, "random") == 0)
902 cp.address_type = 0x01;
903 else
904 return (USAGE);
905 break;
906 case 'a':
907 addr_set = true;
908 if (!bt_aton(optarg, &cp.address)) {
909 struct hostent *he = NULL;
910
911 if ((he = bt_gethostbyname(optarg)) == NULL)
912 return (USAGE);
913
914 memcpy(&cp.address, he->h_addr,
915 sizeof(cp.address));
916 }
917 break;
918 }
919 }
920
921 if (addr_set == false)
922 return (USAGE);
923
924 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
925 NG_HCI_OCF_LE_ADD_DEVICE_TO_WHITE_LIST),
926 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR)
927 return (ERROR);
928
929 if (rp.status != 0x00) {
930 fprintf(stdout, "Status: %s [%#02x]\n",
931 hci_status2str(rp.status), rp.status);
932 return (FAILED);
933 }
934
935 fprintf(stdout, "Address removed from white list\n");
936
937 return (OK);
938 }
939
940 static int
le_connect(int s,int argc,char * argv[])941 le_connect(int s, int argc, char *argv[])
942 {
943 ng_hci_le_create_connection_cp cp;
944 ng_hci_status_rp rp;
945 char b[512];
946 ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b;
947
948 int n, scancount, bufsize;
949 char ch;
950 bool addr_set = false;
951 bool verbose = false;
952
953 optreset = 1;
954 optind = 0;
955
956 /* minimal scan interval (2.5ms) */
957 cp.scan_interval = htole16(4);
958 cp.scan_window = htole16(4);
959
960 /* Don't use the whitelist */
961 cp.filter_policy = 0x00;
962
963 /* Default to public peer address */
964 cp.peer_addr_type = 0x00;
965
966 /* Own address type public */
967 cp.own_address_type = 0x00;
968
969 /* 18.75ms min connection interval */
970 cp.conn_interval_min = htole16(0x000F);
971 /* 18.75ms max connection interval */
972 cp.conn_interval_max = htole16(0x000F);
973
974 /* 0 events connection latency */
975 cp.conn_latency = htole16(0x0000);
976
977 /* 32s supervision timeout */
978 cp.supervision_timeout = htole16(0x0C80);
979
980 /* Min CE Length 0.625 ms */
981 cp.min_ce_length = htole16(1);
982 /* Max CE Length 0.625 ms */
983 cp.max_ce_length = htole16(1);
984
985 while ((ch = getopt(argc, argv , "a:t:v")) != -1) {
986 switch(ch) {
987 case 't':
988 if (strcmp(optarg, "public") == 0)
989 cp.peer_addr_type = 0x00;
990 else if (strcmp(optarg, "random") == 0)
991 cp.peer_addr_type = 0x01;
992 else
993 return (USAGE);
994 break;
995 case 'a':
996 addr_set = true;
997 if (!bt_aton(optarg, &cp.peer_addr)) {
998 struct hostent *he = NULL;
999
1000 if ((he = bt_gethostbyname(optarg)) == NULL)
1001 return (USAGE);
1002
1003 memcpy(&cp.peer_addr, he->h_addr,
1004 sizeof(cp.peer_addr));
1005 }
1006 break;
1007 case 'v':
1008 verbose = true;
1009 break;
1010 }
1011 }
1012
1013 if (addr_set == false)
1014 return (USAGE);
1015
1016 n = sizeof(rp);
1017 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
1018 NG_HCI_OCF_LE_CREATE_CONNECTION),
1019 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR)
1020 return (ERROR);
1021
1022 if (rp.status != 0x00) {
1023 fprintf(stdout,
1024 "Create connection failed. Status: %s [%#02x]\n",
1025 hci_status2str(rp.status), rp.status);
1026 return (FAILED);
1027 }
1028
1029 scancount = 0;
1030 while (scancount < 3) {
1031 /* wait for connection events */
1032 bufsize = sizeof(b);
1033 if (hci_recv(s, b, &bufsize) == ERROR) {
1034 return (ERROR);
1035 }
1036
1037 if (bufsize < sizeof(*e)) {
1038 errno = EIO;
1039 return (ERROR);
1040 }
1041 scancount++;
1042 if (e->event == NG_HCI_EVENT_LE) {
1043 handle_le_connection_event(e, verbose);
1044 break;
1045 }
1046 }
1047
1048 return (OK);
1049 }
1050
handle_le_connection_event(ng_hci_event_pkt_t * e,bool verbose)1051 static void handle_le_connection_event(ng_hci_event_pkt_t* e, bool verbose)
1052 {
1053 ng_hci_le_ep *ev_pkt;
1054 ng_hci_le_connection_complete_ep *conn_event;
1055
1056 ev_pkt = (ng_hci_le_ep *)(e + 1);
1057
1058 if (ev_pkt->subevent_code == NG_HCI_LEEV_CON_COMPL) {
1059 conn_event =(ng_hci_le_connection_complete_ep *)(ev_pkt + 1);
1060 fprintf(stdout, "Handle: %d\n", le16toh(conn_event->handle));
1061 if (verbose) {
1062 fprintf(stdout,
1063 "Status: %s\n",
1064 hci_status2str(conn_event->status));
1065 fprintf(stdout,
1066 "Role: %s\n",
1067 hci_role2str(conn_event->role));
1068 fprintf(stdout,
1069 "Address Type: %s\n",
1070 hci_addrtype2str(conn_event->address_type));
1071 fprintf(stdout,
1072 "Address: %s\n",
1073 hci_bdaddr2str(&conn_event->address));
1074 fprintf(stdout,
1075 "Interval: %.2fms\n",
1076 6.25 * le16toh(conn_event->interval));
1077 fprintf(stdout,
1078 "Latency: %d events\n", conn_event->latency);
1079 fprintf(stdout,
1080 "Supervision timeout: %dms\n",
1081 10 * le16toh(conn_event->supervision_timeout));
1082 fprintf(stdout,
1083 "Master clock accuracy: %s\n",
1084 hci_mc_accuracy2str(
1085 conn_event->master_clock_accuracy));
1086 }
1087 }
1088 }
1089
1090 static int
le_read_channel_map(int s,int argc,char * argv[])1091 le_read_channel_map(int s, int argc, char *argv[])
1092 {
1093 ng_hci_le_read_channel_map_cp cp;
1094 ng_hci_le_read_channel_map_rp rp;
1095 int n;
1096 char buffer[2048];
1097
1098 /* parse command parameters */
1099 switch (argc) {
1100 case 1:
1101 /* connection handle */
1102 if (sscanf(argv[0], "%d", &n) != 1 || n <= 0 || n > 0x0eff)
1103 return (USAGE);
1104
1105 cp.connection_handle = (uint16_t) (n & 0x0fff);
1106 cp.connection_handle = htole16(cp.connection_handle);
1107 break;
1108
1109 default:
1110 return (USAGE);
1111 }
1112
1113 n = sizeof(rp);
1114 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
1115 NG_HCI_OCF_LE_READ_CHANNEL_MAP),
1116 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR)
1117 return (ERROR);
1118
1119 if (rp.status != 0x00) {
1120 fprintf(stdout,
1121 "Read channel map failed. Status: %s [%#02x]\n",
1122 hci_status2str(rp.status), rp.status);
1123 return (FAILED);
1124 }
1125
1126 fprintf(stdout, "Connection handle: %d\n",
1127 le16toh(rp.connection_handle));
1128 fprintf(stdout, "Used channels:\n");
1129 fprintf(stdout, "\n%s\n", hci_le_chanmap2str(rp.le_channel_map,
1130 buffer, sizeof(buffer)));
1131
1132 return (OK);
1133 } /* le_read_channel_map */
1134
1135 static int
le_read_remote_features(int s,int argc,char * argv[])1136 le_read_remote_features(int s, int argc, char *argv[])
1137 {
1138 ng_hci_le_read_remote_used_features_cp cp;
1139 ng_hci_status_rp rp;
1140 int n, bufsize;
1141 char b[512];
1142
1143 ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b;
1144
1145 /* parse command parameters */
1146 switch (argc) {
1147 case 1:
1148 /* connection handle */
1149 if (sscanf(argv[0], "%d", &n) != 1 || n <= 0 || n > 0x0eff)
1150 return (USAGE);
1151
1152 cp.connection_handle = (uint16_t) (n & 0x0fff);
1153 cp.connection_handle = htole16(cp.connection_handle);
1154 break;
1155
1156 default:
1157 return (USAGE);
1158 }
1159
1160 n = sizeof(rp);
1161 if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
1162 NG_HCI_OCF_LE_READ_REMOTE_USED_FEATURES),
1163 (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR)
1164 return (ERROR);
1165
1166 if (rp.status != 0x00) {
1167 fprintf(stdout,
1168 "Read remote features failed. Status: %s [%#02x]\n",
1169 hci_status2str(rp.status), rp.status);
1170 return (FAILED);
1171 }
1172
1173 /* wait for connection events */
1174 bufsize = sizeof(b);
1175 if (hci_recv(s, b, &bufsize) == ERROR) {
1176 return (ERROR);
1177 }
1178
1179 if (bufsize < sizeof(*e)) {
1180 errno = EIO;
1181 return (ERROR);
1182 }
1183 if (e->event == NG_HCI_EVENT_LE) {
1184 handle_le_remote_features_event(e);
1185 }
1186
1187 return (OK);
1188 } /* le_read_remote_features */
1189
handle_le_remote_features_event(ng_hci_event_pkt_t * e)1190 static void handle_le_remote_features_event(ng_hci_event_pkt_t* e)
1191 {
1192 ng_hci_le_ep *ev_pkt;
1193 ng_hci_le_read_remote_features_ep *feat_event;
1194 char buffer[2048];
1195
1196 ev_pkt = (ng_hci_le_ep *)(e + 1);
1197
1198 if (ev_pkt->subevent_code == NG_HCI_LEEV_READ_REMOTE_FEATURES_COMPL) {
1199 feat_event =(ng_hci_le_read_remote_features_ep *)(ev_pkt + 1);
1200 fprintf(stdout, "Handle: %d\n",
1201 le16toh(feat_event->connection_handle));
1202 fprintf(stdout,
1203 "Status: %s\n",
1204 hci_status2str(feat_event->status));
1205 fprintf(stdout, "Features:\n%s\n",
1206 hci_le_features2str(feat_event->features,
1207 buffer, sizeof(buffer)));
1208 }
1209 } /* handle_le_remote_features_event */
1210
le_rand(int s,int argc,char * argv[])1211 static int le_rand(int s, int argc, char *argv[])
1212 {
1213 ng_hci_le_rand_rp rp;
1214 int n;
1215
1216 n = sizeof(rp);
1217
1218 if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
1219 NG_HCI_OCF_LE_RAND),
1220 (void *)&rp, &n) == ERROR)
1221 return (ERROR);
1222
1223 if (rp.status != 0x00) {
1224 fprintf(stdout, "Status: %s [%#02x]\n",
1225 hci_status2str(rp.status), rp.status);
1226 return (FAILED);
1227 }
1228
1229 fprintf(stdout,
1230 "Random number : %08llx\n",
1231 (unsigned long long)le64toh(rp.random_number));
1232
1233 return (OK);
1234 }
1235
1236
1237
1238 struct hci_command le_commands[] = {
1239 {
1240 "le_enable",
1241 "le_enable [enable|disable] \n"
1242 "Enable LE event ",
1243 &le_enable,
1244 },
1245 {
1246 "le_read_local_supported_features",
1247 "le_read_local_supported_features\n"
1248 "read local supported features mask",
1249 &le_read_local_supported_features,
1250 },
1251 {
1252 "le_read_supported_states",
1253 "le_read_supported_states\n"
1254 "read supported status"
1255 ,
1256 &le_read_supported_states,
1257 },
1258 {
1259 "le_set_scan_response",
1260 "le_set_scan_response -n $name -f $flag -u $uuid16,$uuid16 \n"
1261 "set LE scan response data"
1262 ,
1263 &le_set_scan_response,
1264 },
1265 {
1266 "le_set_scan_enable",
1267 "le_set_scan_enable [enable|disable] \n"
1268 "enable or disable LE device scan",
1269 &le_set_scan_enable
1270 },
1271 {
1272 "le_set_scan_param",
1273 "le_set_scan_param [active|passive] interval(ms) window(ms) [public|random] [all|whitelist] \n"
1274 "set LE device scan parameter",
1275 &le_set_scan_param
1276 },
1277 {
1278 "le_set_advertising_enable",
1279 "le_set_advertising_enable [enable|disable] \n"
1280 "start or stop advertising",
1281 &le_set_advertising_enable
1282 },
1283 {
1284 "le_read_advertising_channel_tx_power",
1285 "le_read_advertising_channel_tx_power\n"
1286 "read host advertising transmit poser level (dBm)",
1287 &le_read_advertising_channel_tx_power
1288 },
1289 {
1290 "le_set_advertising_param",
1291 "le_set_advertising_param [-m min_interval(ms)] [-M max_interval(ms)]\n"
1292 "[-t advertising_type] [-o own_address_type] [-p peer_address_type]\n"
1293 "[-c advertising_channel_map] [-f advertising_filter_policy]\n"
1294 "[-a peer_address]\n"
1295 "set LE device advertising parameters",
1296 &le_set_advertising_param
1297 },
1298 {
1299 "le_set_advertising_data",
1300 "le_set_advertising_data -n $name -f $flag -u $uuid16,$uuid16 \n"
1301 "set LE device advertising packed data",
1302 &le_set_advertising_data
1303 },
1304 {
1305 "le_read_buffer_size",
1306 "le_read_buffer_size [-v 1|2]\n"
1307 "Read the maximum size of ACL and ISO data packets",
1308 &le_read_buffer_size
1309 },
1310 {
1311 "le_scan",
1312 "le_scan [-a] [-v] [-n number_of_scans]\n"
1313 "Do an LE scan",
1314 &le_scan
1315 },
1316 {
1317 "le_read_white_list_size",
1318 "le_read_white_list_size\n"
1319 "Read total number of white list entries that can be stored",
1320 &le_read_white_list_size
1321 },
1322 {
1323 "le_clear_white_list",
1324 "le_clear_white_list\n"
1325 "Clear the white list in the controller",
1326 &le_clear_white_list
1327 },
1328 {
1329 "le_add_device_to_white_list",
1330 "le_add_device_to_white_list\n"
1331 "[-t public|random] -a address\n"
1332 "Add device to the white list",
1333 &le_add_device_to_white_list
1334 },
1335 {
1336 "le_remove_device_from_white_list",
1337 "le_remove_device_from_white_list\n"
1338 "[-t public|random] -a address\n"
1339 "Remove device from the white list",
1340 &le_remove_device_from_white_list
1341 },
1342 {
1343 "le_connect",
1344 "le_connect -a address [-t public|random] [-v]\n"
1345 "Connect to an LE device",
1346 &le_connect
1347 },
1348 {
1349 "le_read_channel_map",
1350 "le_read_channel_map <connection_handle>\n"
1351 "Read the channel map for a connection",
1352 &le_read_channel_map
1353 },
1354 {
1355 "le_read_remote_features",
1356 "le_read_remote_features <connection_handle>\n"
1357 "Read supported features for the device\n"
1358 "identified by the connection handle",
1359 &le_read_remote_features
1360 },
1361 {
1362 "le_rand",
1363 "le_rand\n"
1364 "Generate 64 bits of random data",
1365 &le_rand
1366 },
1367 {
1368 NULL,
1369 }
1370 };
1371