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