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 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 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 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 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 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 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 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 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 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 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 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