1 /* $NetBSD: device.c,v 1.3 2009/02/09 12:44:32 plunky 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.3 2009/02/09 12:44:32 plunky 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 */
44 struct 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 */
60 static bool
hci_req(uint16_t opcode,void * cptr,size_t clen,int (* func_cc)(void *),uint8_t event,int (* func_ev)(void *))61 hci_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
124 done:
125 if (fd >= 0) close(fd);
126 return rv > 0 ? true : false;
127 }
128
129 /*
130 * List keys on device
131 */
132
133 static int
list_device_cc(void * arg)134 list_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
149 static int
list_device_ev(void * arg)150 list_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
164 bool
list_device(void)165 list_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
181 static int
read_device_cc(void * arg)182 read_device_cc(void *arg)
183 {
184
185 /* if we got here, no key was found */
186 return -1;
187 }
188
189 static int
read_device_ev(void * arg)190 read_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
202 bool
read_device(void)203 read_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 */
218 static int
write_device_cc(void * arg)219 write_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
231 bool
write_device(void)232 write_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 */
248 static int
clear_device_cc(void * arg)249 clear_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
261 bool
clear_device(void)262 clear_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