usr.bin/btkey/device.c

   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  */
  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 }