1/*	$NetBSD: device.c,v 1.2 2007/12/15 16:03:30 perry Exp $	*/
2
3/*-
4 * Copyright (c) 2007 Iain Hibbert
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 * 3. The name of the author may not be used to endorse or promote products
16 *    derived from this software without specific prior written permission.
17 *
18 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
19 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
20 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
21 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
22 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
23 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
27 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 */
29
30#include <sys/cdefs.h>
31__RCSID("$NetBSD: device.c,v 1.2 2007/12/15 16:03:30 perry Exp $");
32
33#include <bluetooth.h>
34#include <errno.h>
35#include <stdbool.h>
36#include <string.h>
37#include <unistd.h>
38
39#include "btkey.h"
40
41/*
42 * read/write stored link keys packet, with space for one key
43 */
44struct stored_link_keys {
45	uint8_t		num_keys;
46	struct {
47		bdaddr_t addr;
48		uint8_t	 key[HCI_KEY_SIZE];
49	} key[1];
50} __packed;
51
52/*
53 * generic request
54 *
55 *	send command 'opcode' with command packet 'cptr' of size 'clen'
56 *	call 'func_cc' on command_complete event
57 *	call 'func_ev' on event 'event'
58 *	callbacks return -1 (failure), 0 (continue) or 1 (success)
59 */
60static bool
61hci_req(uint16_t opcode, void *cptr, size_t clen, int (*func_cc)(void *),
62	uint8_t event, int (*func_ev)(void *))
63{
64	uint8_t buf[sizeof(hci_cmd_hdr_t) + HCI_CMD_PKT_SIZE];
65	struct sockaddr_bt sa;
66	struct hci_filter f;
67	hci_cmd_hdr_t *hdr;
68	hci_event_hdr_t *ep;
69	int fd, rv;
70
71	memset(&f, 0, sizeof(f));
72	hci_filter_set(HCI_EVENT_COMMAND_COMPL, &f);
73	if (event != 0) hci_filter_set(event, &f);
74
75	memset(&sa, 0, sizeof(sa));
76	sa.bt_len = sizeof(sa);
77	sa.bt_family = AF_BLUETOOTH;
78	bdaddr_copy(&sa.bt_bdaddr, &laddr);
79
80	hdr = (hci_cmd_hdr_t *)buf;
81	hdr->type = HCI_CMD_PKT;
82	hdr->opcode = htole16(opcode);
83	hdr->length = clen;
84
85	memcpy(buf + sizeof(hci_cmd_hdr_t), cptr, clen);
86
87	rv = -1;
88
89	if ((fd = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI)) < 0
90	    || setsockopt(fd, BTPROTO_HCI, SO_HCI_EVT_FILTER, &f, sizeof(f)) < 0
91	    || bind(fd, (struct sockaddr *)&sa, sizeof(sa)) < 0
92	    || connect(fd, (struct sockaddr *)&sa, sizeof(sa)) < 0
93	    || send(fd, buf, sizeof(hci_cmd_hdr_t) + clen, 0) < 0)
94		goto done;
95
96	ep = (hci_event_hdr_t *)buf;
97	for (;;) {
98		if (recv(fd, buf, sizeof(buf), 0) < 0)
99			goto done;
100
101		if (ep->event == HCI_EVENT_COMMAND_COMPL) {
102			hci_command_compl_ep *cc;
103
104			cc = (hci_command_compl_ep *)(ep + 1);
105			if (opcode != le16toh(cc->opcode))
106				continue;
107
108			rv = func_cc(cc + 1);
109			if (rv == 0)
110				continue;
111
112			goto done;
113		}
114
115		if (event != 0 && event == ep->event) {
116			rv = func_ev(ep + 1);
117			if (rv == 0)
118				continue;
119
120			goto done;
121		}
122	}
123
124done:
125	if (fd >= 0) close(fd);
126	return rv > 0 ? true : false;
127}
128
129/*
130 * List keys on device
131 */
132
133static int
134list_device_cc(void *arg)
135{
136	hci_read_stored_link_key_rp *rp = arg;
137
138	if (rp->status) {
139		errno = ENODEV;
140		return -1;
141	}
142
143	printf("\n");
144	printf("read %d keys (max %d)\n", rp->num_keys_read, rp->max_num_keys);
145
146	return 1;
147}
148
149static int
150list_device_ev(void *arg)
151{
152	struct stored_link_keys *ep = arg;
153	int i;
154
155	for (i = 0 ; i < ep->num_keys ; i++) {
156		printf("\n");
157		print_addr("bdaddr", &ep->key[i].addr);
158		print_key("device key", ep->key[i].key);
159	}
160
161	return 0;
162}
163
164bool
165list_device(void)
166{
167	hci_read_stored_link_key_cp cp;
168
169	bdaddr_copy(&cp.bdaddr, BDADDR_ANY);
170	cp.read_all = 0x01;
171
172	return hci_req(HCI_CMD_READ_STORED_LINK_KEY,
173			&cp, sizeof(cp), list_device_cc,
174			HCI_EVENT_RETURN_LINK_KEYS, list_device_ev);
175}
176
177/*
178 * Read key from device
179 */
180
181static int
182read_device_cc(void *arg)
183{
184
185	/* if we got here, no key was found */
186	return -1;
187}
188
189static int
190read_device_ev(void *arg)
191{
192	struct stored_link_keys *ep = arg;
193
194	if (ep->num_keys != 1
195	    || !bdaddr_same(&ep->key[0].addr, &raddr))
196		return 0;
197
198	memcpy(key, ep->key[0].key, HCI_KEY_SIZE);
199	return 1;
200}
201
202bool
203read_device(void)
204{
205	hci_read_stored_link_key_cp cp;
206
207	bdaddr_copy(&cp.bdaddr, &raddr);
208	cp.read_all = 0x00;
209
210	return hci_req(HCI_CMD_READ_STORED_LINK_KEY,
211			&cp, sizeof(cp), read_device_cc,
212			HCI_EVENT_RETURN_LINK_KEYS, read_device_ev);
213}
214
215/*
216 * Write key to device
217 */
218static int
219write_device_cc(void *arg)
220{
221	hci_write_stored_link_key_rp *rp = arg;
222
223	if (rp->status || rp->num_keys_written != 1) {
224		errno = ENODEV;
225		return -1;
226	}
227
228	return 1;
229}
230
231bool
232write_device(void)
233{
234	struct stored_link_keys cp;
235
236	cp.num_keys = 1;
237	bdaddr_copy(&cp.key[0].addr, &raddr);
238	memcpy(cp.key[0].key, key, HCI_KEY_SIZE);
239
240	return hci_req(HCI_CMD_WRITE_STORED_LINK_KEY,
241			&cp, sizeof(cp), write_device_cc,
242			0, NULL);
243}
244
245/*
246 * Clear key from device
247 */
248static int
249clear_device_cc(void *arg)
250{
251	hci_delete_stored_link_key_rp *rp = arg;
252
253	if (rp->status || rp->num_keys_deleted != 1) {
254		errno = ENODEV;
255		return -1;
256	}
257
258	return 1;
259}
260
261bool
262clear_device(void)
263{
264	hci_delete_stored_link_key_cp cp;
265
266	cp.delete_all = 0x00;
267	bdaddr_copy(&cp.bdaddr, &raddr);
268
269	return hci_req(HCI_CMD_DELETE_STORED_LINK_KEY,
270			&cp, sizeof(cp), clear_device_cc,
271			0, NULL);
272}
273