1 /* $NetBSD: device.c,v 1.1 2007/11/09 21:18:25 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.1 2007/11/09 21:18:25 plunky Exp $"); 32 33 #include <bluetooth.h> 34 #include <stdbool.h> 35 #include <string.h> 36 #include <unistd.h> 37 38 #include "btkey.h" 39 40 /* 41 * read/write stored link keys packet, with space for one key 42 */ 43 struct stored_link_keys { 44 uint8_t num_keys; 45 struct { 46 bdaddr_t addr; 47 uint8_t key[HCI_KEY_SIZE]; 48 } key[1]; 49 } __attribute__ ((__packed__)); 50 51 /* 52 * generic request 53 * 54 * send command 'opcode' with command packet 'cptr' of size 'clen' 55 * call 'func_cc' on command_complete event 56 * call 'func_ev' on event 'event' 57 * callbacks return -1 (failure), 0 (continue) or 1 (success) 58 */ 59 static bool 60 hci_req(uint16_t opcode, void *cptr, size_t clen, int (*func_cc)(void *), 61 uint8_t event, int (*func_ev)(void *)) 62 { 63 uint8_t buf[sizeof(hci_cmd_hdr_t) + HCI_CMD_PKT_SIZE]; 64 struct sockaddr_bt sa; 65 struct hci_filter f; 66 hci_cmd_hdr_t *hdr; 67 hci_event_hdr_t *ep; 68 int fd, rv; 69 70 memset(&f, 0, sizeof(f)); 71 hci_filter_set(HCI_EVENT_COMMAND_COMPL, &f); 72 if (event != 0) hci_filter_set(event, &f); 73 74 memset(&sa, 0, sizeof(sa)); 75 sa.bt_len = sizeof(sa); 76 sa.bt_family = AF_BLUETOOTH; 77 bdaddr_copy(&sa.bt_bdaddr, &laddr); 78 79 hdr = (hci_cmd_hdr_t *)buf; 80 hdr->type = HCI_CMD_PKT; 81 hdr->opcode = htole16(opcode); 82 hdr->length = clen; 83 84 memcpy(buf + sizeof(hci_cmd_hdr_t), cptr, clen); 85 86 rv = -1; 87 88 if ((fd = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI)) < 0 89 || setsockopt(fd, BTPROTO_HCI, SO_HCI_EVT_FILTER, &f, sizeof(f)) < 0 90 || bind(fd, (struct sockaddr *)&sa, sizeof(sa)) < 0 91 || connect(fd, (struct sockaddr *)&sa, sizeof(sa)) < 0 92 || send(fd, buf, sizeof(hci_cmd_hdr_t) + clen, 0) < 0) 93 goto done; 94 95 ep = (hci_event_hdr_t *)buf; 96 for (;;) { 97 if (recv(fd, buf, sizeof(buf), 0) < 0) 98 goto done; 99 100 if (ep->event == HCI_EVENT_COMMAND_COMPL) { 101 hci_command_compl_ep *cc; 102 103 cc = (hci_command_compl_ep *)(ep + 1); 104 if (opcode != le16toh(cc->opcode)) 105 continue; 106 107 rv = func_cc(cc + 1); 108 if (rv == 0) 109 continue; 110 111 goto done; 112 } 113 114 if (event != 0 && event == ep->event) { 115 rv = func_ev(ep + 1); 116 if (rv == 0) 117 continue; 118 119 goto done; 120 } 121 } 122 123 done: 124 if (fd >= 0) close(fd); 125 return rv > 0 ? true : false; 126 } 127 128 /* 129 * List keys on device 130 */ 131 132 static int 133 list_device_cc(void *arg) 134 { 135 hci_read_stored_link_key_rp *rp = arg; 136 137 if (rp->status) 138 return -1; 139 140 printf("\n"); 141 printf("read %d keys (max %d)\n", rp->num_keys_read, rp->max_num_keys); 142 143 return 1; 144 } 145 146 static int 147 list_device_ev(void *arg) 148 { 149 struct stored_link_keys *ep = arg; 150 int i; 151 152 for (i = 0 ; i < ep->num_keys ; i++) { 153 printf("\n"); 154 print_addr("bdaddr", &ep->key[i].addr); 155 print_key("device key", ep->key[i].key); 156 } 157 158 return 0; 159 } 160 161 bool 162 list_device(void) 163 { 164 hci_read_stored_link_key_cp cp; 165 166 bdaddr_copy(&cp.bdaddr, BDADDR_ANY); 167 cp.read_all = 0x01; 168 169 return hci_req(HCI_CMD_READ_STORED_LINK_KEY, 170 &cp, sizeof(cp), list_device_cc, 171 HCI_EVENT_RETURN_LINK_KEYS, list_device_ev); 172 } 173 174 /* 175 * Read key from device 176 */ 177 178 static int 179 read_device_cc(void *arg) 180 { 181 182 /* if we got here, no key was found */ 183 return -1; 184 } 185 186 static int 187 read_device_ev(void *arg) 188 { 189 struct stored_link_keys *ep = arg; 190 191 if (ep->num_keys != 1 192 || !bdaddr_same(&ep->key[0].addr, &raddr)) 193 return 0; 194 195 memcpy(key, ep->key[0].key, HCI_KEY_SIZE); 196 return 1; 197 } 198 199 bool 200 read_device(void) 201 { 202 hci_read_stored_link_key_cp cp; 203 204 bdaddr_copy(&cp.bdaddr, &raddr); 205 cp.read_all = 0x00; 206 207 return hci_req(HCI_CMD_READ_STORED_LINK_KEY, 208 &cp, sizeof(cp), read_device_cc, 209 HCI_EVENT_RETURN_LINK_KEYS, read_device_ev); 210 } 211 212 /* 213 * Write key to device 214 */ 215 static int 216 write_device_cc(void *arg) 217 { 218 hci_write_stored_link_key_rp *rp = arg; 219 220 if (rp->status || rp->num_keys_written != 1) 221 return -1; 222 223 return 1; 224 } 225 226 bool 227 write_device(void) 228 { 229 struct stored_link_keys cp; 230 231 cp.num_keys = 1; 232 bdaddr_copy(&cp.key[0].addr, &raddr); 233 memcpy(cp.key[0].key, key, HCI_KEY_SIZE); 234 235 return hci_req(HCI_CMD_WRITE_STORED_LINK_KEY, 236 &cp, sizeof(cp), write_device_cc, 237 0, NULL); 238 } 239 240 /* 241 * Clear key from device 242 */ 243 static int 244 clear_device_cc(void *arg) 245 { 246 hci_delete_stored_link_key_rp *rp = arg; 247 248 if (rp->status || rp->num_keys_deleted != 1) 249 return -1; 250 251 return 1; 252 } 253 254 bool 255 clear_device(void) 256 { 257 hci_delete_stored_link_key_cp cp; 258 259 cp.delete_all = 0x00; 260 bdaddr_copy(&cp.bdaddr, &raddr); 261 262 return hci_req(HCI_CMD_DELETE_STORED_LINK_KEY, 263 &cp, sizeof(cp), clear_device_cc, 264 0, NULL); 265 } 266