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