emulator/le.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2011-2012  Intel Corporation
   6  *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.org>
   7  *
   8  *
   9  *  This library is free software; you can redistribute it and/or
  10  *  modify it under the terms of the GNU Lesser General Public
  11  *  License as published by the Free Software Foundation; either
  12  *  version 2.1 of the License, or (at your option) any later version.
  13  *
  14  *  This library is distributed in the hope that it will be useful,
  15  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17  *  Lesser General Public License for more details.
  18  *
  19  *  You should have received a copy of the GNU Lesser General Public
  20  *  License along with this library; if not, write to the Free Software
  21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  22  *
  23  */
  24
  25 #ifdef HAVE_CONFIG_H
  26 #include <config.h>
  27 #endif
  28
  29 #include <fcntl.h>
  30 #include <unistd.h>
  31 #include <stdlib.h>
  32 #include <string.h>
  33 #include <sys/socket.h>
  34 #include <sys/un.h>
  35
  36 #include <bluetooth/bluetooth.h>
  37 #include <bluetooth/hci.h>
  38
  39 #include "src/shared/util.h"
  40 #include "src/shared/crypto.h"
  41 #include "monitor/mainloop.h"
  42 #include "monitor/bt.h"
  43
  44 #include "le.h"
  45
  46 #define WHITE_LIST_SIZE  16
  47
  48 struct bt_le {
  49         volatile int ref_count;
  50         int vhci_fd;
  51         struct bt_crypto *crypto;
  52
  53         uint8_t  event_mask[16];
  54         uint16_t manufacturer;
  55         uint8_t  commands[64];
  56         uint8_t  features[8];
  57         uint8_t  bdaddr[6];
  58
  59         uint8_t  le_event_mask[8];
  60         uint16_t le_mtu;
  61         uint8_t  le_max_pkt;
  62         uint8_t  le_features[8];
  63         uint8_t  le_random_addr[6];
  64         uint16_t le_adv_min_interval;
  65         uint16_t le_adv_max_interval;
  66         uint8_t  le_adv_type;
  67         uint8_t  le_adv_own_addr_type;
  68         uint8_t  le_adv_direct_addr_type;
  69         uint8_t  le_adv_direct_addr[6];
  70         uint8_t  le_adv_channel_map;
  71         uint8_t  le_adv_filter_policy;
  72         int8_t   le_adv_tx_power;
  73         uint8_t  le_adv_data_len;
  74         uint8_t  le_adv_data[31];
  75         uint8_t  le_scan_rsp_data_len;
  76         uint8_t  le_scan_rsp_data[31];
  77         uint8_t  le_adv_enable;
  78
  79         uint8_t  le_white_list_size;
  80         uint8_t  le_states[8];
  81 };
  82
  83 static void reset_defaults(struct bt_le *hci)
  84 {
  85         memset(hci->event_mask, 0, sizeof(hci->event_mask));
  86         hci->event_mask[0] |= 0x10;     /* Disconnection Complete */
  87         hci->event_mask[0] |= 0x80;     /* Encryption Change */
  88         hci->event_mask[1] |= 0x08;     /* Read Remote Version Information Complete */
  89         hci->event_mask[1] |= 0x20;     /* Command Complete */
  90         hci->event_mask[1] |= 0x40;     /* Command Status */
  91         hci->event_mask[1] |= 0x80;     /* Hardware Error */
  92         hci->event_mask[2] |= 0x04;     /* Number of Completed Packets */
  93         hci->event_mask[3] |= 0x02;     /* Data Buffer Overflow */
  94         hci->event_mask[5] |= 0x80;     /* Encryption Key Refresh Complete */
  95
  96         hci->manufacturer = 0x003f;     /* Bluetooth SIG (63) */
  97
  98         memset(hci->commands, 0, sizeof(hci->commands));
  99         //hci->commands[0]  |= 0x20;    /* Disconnect */
 100         //hci->commands[2]  |= 0x80;    /* Read Remote Version Information */
 101         hci->commands[5]  |= 0x40;      /* Set Event Mask */
 102         hci->commands[5]  |= 0x80;      /* Reset */
 103         //hci->commands[10] |= 0x04;    /* Read Transmit Power Level */
 104         hci->commands[14] |= 0x08;      /* Read Local Version Information */
 105         hci->commands[14] |= 0x10;      /* Read Local Supported Commands */
 106         hci->commands[14] |= 0x20;      /* Read Local Supported Features */
 107         hci->commands[14] |= 0x80;      /* Read Buffer Size */
 108         hci->commands[15] |= 0x02;      /* Read BD ADDR */
 109         //hci->commands[15] |= 0x20;    /* Read RSSI */
 110         hci->commands[25] |= 0x01;      /* LE Set Event Mask */
 111         hci->commands[25] |= 0x02;      /* LE Read Buffer Size */
 112         hci->commands[25] |= 0x04;      /* LE Read Local Supported Features */
 113         hci->commands[25] |= 0x10;      /* LE Set Random Address */
 114         hci->commands[25] |= 0x20;      /* LE Set Advertising Parameters */
 115         hci->commands[25] |= 0x40;      /* LE Read Advertising Channel TX Power */
 116         hci->commands[25] |= 0x80;      /* LE Set Advertising Data */
 117         hci->commands[26] |= 0x01;      /* LE Set Scan Response Data */
 118         hci->commands[26] |= 0x02;      /* LE Set Advertise Enable */
 119         //hci->commands[26] |= 0x04;    /* LE Set Scan Parameters */
 120         //hci->commands[26] |= 0x08;    /* LE Set Scan Enable */
 121         //hci->commands[26] |= 0x10;    /* LE Create Connection */
 122         //hci->commands[26] |= 0x20;    /* LE Create Connection Cancel */
 123         hci->commands[26] |= 0x40;      /* LE Read White List Size */
 124         hci->commands[26] |= 0x80;      /* LE Clear White List */
 125         //hci->commands[27] |= 0x01;    /* LE Add Device To White List */
 126         //hci->commands[27] |= 0x02;    /* LE Remove Device From White List */
 127         //hci->commands[27] |= 0x04;    /* LE Connection Update */
 128         //hci->commands[27] |= 0x08;    /* LE Set Host Channel Classification */
 129         //hci->commands[27] |= 0x10;    /* LE Read Channel Map */
 130         //hci->commands[27] |= 0x20;    /* LE Read Remote Used Features */
 131         hci->commands[27] |= 0x40;      /* LE Encrypt */
 132         hci->commands[27] |= 0x80;      /* LE Rand */
 133         //hci->commands[28] |= 0x01;    /* LE Start Encryption */
 134         //hci->commands[28] |= 0x02;    /* LE Long Term Key Request Reply */
 135         //hci->commands[28] |= 0x04;    /* LE Long Term Key Request Negative Reply */
 136         hci->commands[28] |= 0x08;      /* LE Read Supported States */
 137         //hci->commands[28] |= 0x10;    /* LE Receiver Test */
 138         //hci->commands[28] |= 0x20;    /* LE Transmitter Test */
 139         //hci->commands[28] |= 0x40;    /* LE Test End */
 140         //hci->commands[33] |= 0x10;    /* LE Remote Connection Parameter Request Reply */
 141         //hci->commands[33] |= 0x20;    /* LE Remote Connection Parameter Request Negative Reply */
 142
 143         memset(hci->features, 0, sizeof(hci->features));
 144         hci->features[4] |= 0x20;       /* BR/EDR Not Supported */
 145         hci->features[4] |= 0x40;       /* LE Supported */
 146
 147         memset(hci->bdaddr, 0, sizeof(hci->bdaddr));
 148
 149         memset(hci->le_event_mask, 0, sizeof(hci->le_event_mask));
 150         hci->le_event_mask[0] |= 0x01;  /* LE Connection Complete */
 151         hci->le_event_mask[0] |= 0x02;  /* LE Advertising Report */
 152         hci->le_event_mask[0] |= 0x04;  /* LE Connection Update Complete */
 153         hci->le_event_mask[0] |= 0x08;  /* LE Read Remote Used Features Complete */
 154         hci->le_event_mask[0] |= 0x10;  /* LE Long Term Key Request */
 155         //hci->le_event_mask[0] |= 0x20;        /* LE Remote Connection Parameter Request */
 156
 157         hci->le_mtu = 64;
 158         hci->le_max_pkt = 1;
 159
 160         memset(hci->le_features, 0, sizeof(hci->le_features));
 161         hci->le_features[0] |= 0x01;    /* LE Encryption */
 162         //hci->le_features[0] |= 0x02;  /* Connection Parameter Request Procedure */
 163         //hci->le_features[0] |= 0x04;  /* Extended Reject Indication */
 164         //hci->le_features[0] |= 0x08;  /* Slave-initiated Features Exchange */
 165         //hci->le_features[0] |= 0x10;  /* LE Ping */
 166
 167         memset(hci->le_random_addr, 0, sizeof(hci->le_random_addr));
 168
 169         hci->le_adv_min_interval = 0x0800;
 170         hci->le_adv_max_interval = 0x0800;
 171         hci->le_adv_type = 0x00;
 172         hci->le_adv_own_addr_type = 0x00;
 173         hci->le_adv_direct_addr_type = 0x00;
 174         memset(hci->le_adv_direct_addr, 0, 6);
 175         hci->le_adv_channel_map = 0x07;
 176         hci->le_adv_filter_policy = 0x00;
 177
 178         hci->le_adv_tx_power = 0;
 179
 180         memset(hci->le_adv_data, 0, sizeof(hci->le_adv_data));
 181         hci->le_adv_data_len = 0;
 182
 183         memset(hci->le_scan_rsp_data, 0, sizeof(hci->le_scan_rsp_data));
 184         hci->le_scan_rsp_data_len = 0;
 185
 186         hci->le_adv_enable = 0x00;
 187
 188         hci->le_white_list_size = WHITE_LIST_SIZE;
 189
 190         memset(hci->le_states, 0, sizeof(hci->le_states));
 191         hci->le_states[0] |= 0x01;      /* Non-connectable Advertising */
 192         hci->le_states[0] |= 0x02;      /* Scannable Advertising */
 193         hci->le_states[0] |= 0x04;      /* Connectable Advertising */
 194         hci->le_states[0] |= 0x08;      /* Directed Advertising */
 195         hci->le_states[0] |= 0x10;      /* Passive Scanning */
 196         hci->le_states[0] |= 0x20;      /* Active Scanning */
 197         hci->le_states[0] |= 0x40;      /* Initiating */
 198         hci->le_states[0] |= 0x80;      /* Connection */
 199 }
 200
 201 static void send_event(struct bt_le *hci, uint8_t event,
 202                                                 void *data, uint8_t size)
 203 {
 204         uint8_t type = BT_H4_EVT_PKT;
 205         struct bt_hci_evt_hdr hdr;
 206         struct iovec iov[3];
 207         int iovcnt;
 208
 209         hdr.evt  = event;
 210         hdr.plen = size;
 211
 212         iov[0].iov_base = &type;
 213         iov[0].iov_len  = 1;
 214         iov[1].iov_base = &hdr;
 215         iov[1].iov_len  = sizeof(hdr);
 216
 217         if (size > 0) {
 218                 iov[2].iov_base = data;
 219                 iov[2].iov_len  = size;
 220                 iovcnt = 3;
 221         } else
 222                 iovcnt = 2;
 223
 224         if (writev(hci->vhci_fd, iov, iovcnt) < 0)
 225                 fprintf(stderr, "Write to /dev/vhci failed (%m)\n");
 226 }
 227
 228 static void cmd_complete(struct bt_le *hci, uint16_t opcode,
 229                                                 const void *data, uint8_t len)
 230 {
 231         struct bt_hci_evt_cmd_complete *cc;
 232         void *pkt_data;
 233
 234         pkt_data = alloca(sizeof(*cc) + len);
 235         if (!pkt_data)
 236                 return;
 237
 238         cc = pkt_data;
 239         cc->ncmd = 0x01;
 240         cc->opcode = cpu_to_le16(opcode);
 241
 242         if (len > 0)
 243                 memcpy(pkt_data + sizeof(*cc), data, len);
 244
 245         send_event(hci, BT_HCI_EVT_CMD_COMPLETE, pkt_data, sizeof(*cc) + len);
 246 }
 247
 248 static void cmd_status(struct bt_le *hci, uint8_t status, uint16_t opcode)
 249 {
 250         struct bt_hci_evt_cmd_status cs;
 251
 252         cs.status = status;
 253         cs.ncmd = 0x01;
 254         cs.opcode = cpu_to_le16(opcode);
 255
 256         send_event(hci, BT_HCI_EVT_CMD_STATUS, &cs, sizeof(cs));
 257 }
 258
 259 static void cmd_set_event_mask(struct bt_le *hci,
 260                                                 const void *data, uint8_t size)
 261 {
 262         const struct bt_hci_cmd_set_event_mask *cmd = data;
 263         uint8_t status;
 264
 265         memcpy(hci->event_mask, cmd->mask, 8);
 266
 267         status = BT_HCI_ERR_SUCCESS;
 268         cmd_complete(hci, BT_HCI_CMD_SET_EVENT_MASK, &status, sizeof(status));
 269 }
 270
 271 static void cmd_reset(struct bt_le *hci, const void *data, uint8_t size)
 272 {
 273         uint8_t status;
 274
 275         reset_defaults(hci);
 276
 277         status = BT_HCI_ERR_SUCCESS;
 278         cmd_complete(hci, BT_HCI_CMD_RESET, &status, sizeof(status));
 279 }
 280
 281 static void cmd_read_local_version(struct bt_le *hci,
 282                                                 const void *data, uint8_t size)
 283 {
 284         struct bt_hci_rsp_read_local_version rsp;
 285
 286         rsp.status = BT_HCI_ERR_SUCCESS;
 287         rsp.hci_ver = 0x06;
 288         rsp.hci_rev = cpu_to_le16(0x0000);
 289         rsp.lmp_ver = 0x06;
 290         rsp.manufacturer = cpu_to_le16(hci->manufacturer);
 291         rsp.lmp_subver = cpu_to_le16(0x0000);
 292
 293         cmd_complete(hci, BT_HCI_CMD_READ_LOCAL_VERSION, &rsp, sizeof(rsp));
 294 }
 295
 296 static void cmd_read_local_commands(struct bt_le *hci,
 297                                                 const void *data, uint8_t size)
 298 {
 299         struct bt_hci_rsp_read_local_commands rsp;
 300
 301         rsp.status = BT_HCI_ERR_SUCCESS;
 302         memcpy(rsp.commands, hci->commands, 64);
 303
 304         cmd_complete(hci, BT_HCI_CMD_READ_LOCAL_COMMANDS, &rsp, sizeof(rsp));
 305 }
 306
 307 static void cmd_read_local_features(struct bt_le *hci,
 308                                                 const void *data, uint8_t size)
 309 {
 310         struct bt_hci_rsp_read_local_features rsp;
 311
 312         rsp.status = BT_HCI_ERR_SUCCESS;
 313         memcpy(rsp.features, hci->features, 8);
 314
 315         cmd_complete(hci, BT_HCI_CMD_READ_LOCAL_FEATURES, &rsp, sizeof(rsp));
 316 }
 317
 318 static void cmd_read_buffer_size(struct bt_le *hci,
 319                                                 const void *data, uint8_t size)
 320 {
 321         struct bt_hci_rsp_read_buffer_size rsp;
 322
 323         rsp.status = BT_HCI_ERR_SUCCESS;
 324         rsp.acl_mtu = cpu_to_le16(0x0000);
 325         rsp.sco_mtu = 0x00;
 326         rsp.acl_max_pkt = cpu_to_le16(0x0000);
 327         rsp.sco_max_pkt = cpu_to_le16(0x0000);
 328
 329         cmd_complete(hci, BT_HCI_CMD_READ_BUFFER_SIZE, &rsp, sizeof(rsp));
 330 }
 331
 332 static void cmd_read_bd_addr(struct bt_le *hci, const void *data, uint8_t size)
 333 {
 334         struct bt_hci_rsp_read_bd_addr rsp;
 335
 336         rsp.status = BT_HCI_ERR_SUCCESS;
 337         memcpy(rsp.bdaddr, hci->bdaddr, 6);
 338
 339         cmd_complete(hci, BT_HCI_CMD_READ_BD_ADDR, &rsp, sizeof(rsp));
 340 }
 341
 342 static void cmd_le_set_event_mask(struct bt_le *hci,
 343                                                 const void *data, uint8_t size)
 344 {
 345         const struct bt_hci_cmd_le_set_event_mask *cmd = data;
 346         uint8_t status;
 347
 348         memcpy(hci->le_event_mask, cmd->mask, 8);
 349
 350         status = BT_HCI_ERR_SUCCESS;
 351         cmd_complete(hci, BT_HCI_CMD_LE_SET_EVENT_MASK,
 352                                                 &status, sizeof(status));
 353 }
 354
 355 static void cmd_le_read_buffer_size(struct bt_le *hci,
 356                                                 const void *data, uint8_t size)
 357 {
 358         struct bt_hci_rsp_le_read_buffer_size rsp;
 359
 360         rsp.status = BT_HCI_ERR_SUCCESS;
 361         rsp.le_mtu = cpu_to_le16(hci->le_mtu);
 362         rsp.le_max_pkt = hci->le_max_pkt;
 363
 364         cmd_complete(hci, BT_HCI_CMD_LE_READ_BUFFER_SIZE, &rsp, sizeof(rsp));
 365 }
 366
 367 static void cmd_le_read_local_features(struct bt_le *hci,
 368                                                 const void *data, uint8_t size)
 369 {
 370         struct bt_hci_rsp_le_read_local_features rsp;
 371
 372         rsp.status = BT_HCI_ERR_SUCCESS;
 373         memcpy(rsp.features, hci->le_features, 8);
 374
 375         cmd_complete(hci, BT_HCI_CMD_LE_READ_LOCAL_FEATURES,
 376                                                         &rsp, sizeof(rsp));
 377 }
 378
 379 static void cmd_le_set_random_address(struct bt_le *hci,
 380                                                 const void *data, uint8_t size)
 381 {
 382         const struct bt_hci_cmd_le_set_random_address *cmd = data;
 383         uint8_t status;
 384
 385         memcpy(hci->le_random_addr, cmd->addr, 6);
 386
 387         status = BT_HCI_ERR_SUCCESS;
 388         cmd_complete(hci, BT_HCI_CMD_LE_SET_RANDOM_ADDRESS,
 389                                                 &status, sizeof(status));
 390 }
 391
 392 static void cmd_le_set_adv_parameters(struct bt_le *hci,
 393                                                 const void *data, uint8_t size)
 394 {
 395         const struct bt_hci_cmd_le_set_adv_parameters *cmd = data;
 396         uint16_t min_interval, max_interval;
 397         uint8_t status;
 398
 399         if (hci->le_adv_enable == 0x01) {
 400                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
 401                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 402                 return;
 403         }
 404
 405         min_interval = le16_to_cpu(cmd->min_interval);
 406         max_interval = le16_to_cpu(cmd->max_interval);
 407
 408         /* Valid range for advertising type is 0x00 to 0x03 */
 409         switch (cmd->type) {
 410         case 0x00:      /* ADV_IND */
 411                 /* Range for advertising interval min is 0x0020 to 0x4000 */
 412                 if (min_interval < 0x0020 || min_interval > 0x4000) {
 413                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 414                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 415                         return;
 416                 }
 417                 /* Range for advertising interval max is 0x0020 to 0x4000 */
 418                 if (max_interval < 0x0020 || max_interval > 0x4000) {
 419                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 420                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 421                         return;
 422                 }
 423                 /* Advertising interval max shall be less or equal */
 424                 if (min_interval > max_interval) {
 425                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 426                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 427                         return;
 428                 }
 429                 break;
 430
 431         case 0x01:      /* ADV_DIRECT_IND */
 432                 /* Range for direct address type is 0x00 to 0x01 */
 433                 if (cmd->direct_addr_type > 0x01) {
 434                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 435                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 436                         return;
 437                 }
 438                 break;
 439
 440         case 0x02:      /* ADV_SCAN_IND */
 441         case 0x03:      /* ADV_NONCONN_IND */
 442                 /* Range for advertising interval min is 0x00a0 to 0x4000 */
 443                 if (min_interval < 0x00a0 || min_interval > 0x4000) {
 444                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 445                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 446                         return;
 447                 }
 448                 /* Range for advertising interval max is 0x00a0 to 0x4000 */
 449                 if (max_interval < 0x00a0 || max_interval > 0x4000) {
 450                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 451                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 452                         return;
 453                 }
 454                 /* Advertising interval min shall be less or equal */
 455                 if (min_interval > max_interval) {
 456                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 457                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 458                         return;
 459                 }
 460                 break;
 461
 462         default:
 463                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 464                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 465                 return;
 466         }
 467
 468         /* Valid range for own address type is 0x00 to 0x01 */
 469         if (cmd->own_addr_type > 0x01) {
 470                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 471                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 472                 return;
 473         }
 474
 475         /* Valid range for advertising channel map is 0x01 to 0x07 */
 476         if (cmd->channel_map < 0x01 || cmd->channel_map > 0x07) {
 477                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 478                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 479                 return;
 480         }
 481
 482         /* Valid range for advertising filter policy is 0x00 to 0x03 */
 483         if (cmd->filter_policy > 0x03) {
 484                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 485                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 486                 return;
 487         }
 488
 489         hci->le_adv_min_interval = min_interval;
 490         hci->le_adv_max_interval = max_interval;
 491         hci->le_adv_type = cmd->type;
 492         hci->le_adv_own_addr_type = cmd->own_addr_type;
 493         hci->le_adv_direct_addr_type = cmd->direct_addr_type;
 494         memcpy(hci->le_adv_direct_addr, cmd->direct_addr, 6);
 495         hci->le_adv_channel_map = cmd->channel_map;
 496         hci->le_adv_filter_policy = cmd->filter_policy;
 497
 498         status = BT_HCI_ERR_SUCCESS;
 499         cmd_complete(hci, BT_HCI_CMD_LE_SET_ADV_PARAMETERS,
 500                                                 &status, sizeof(status));
 501 }
 502
 503 static void cmd_le_read_adv_tx_power(struct bt_le *hci,
 504                                                 const void *data, uint8_t size)
 505 {
 506         struct bt_hci_rsp_le_read_adv_tx_power rsp;
 507
 508         rsp.status = BT_HCI_ERR_SUCCESS;
 509         rsp.level = hci->le_adv_tx_power;
 510
 511         cmd_complete(hci, BT_HCI_CMD_LE_READ_ADV_TX_POWER, &rsp, sizeof(rsp));
 512 }
 513
 514 static void cmd_le_set_adv_data(struct bt_le *hci,
 515                                                 const void *data, uint8_t size)
 516 {
 517         const struct bt_hci_cmd_le_set_adv_data *cmd = data;
 518         uint8_t status;
 519
 520         /* Valid range for advertising data length is 0x00 to 0x1f */
 521         if (cmd->len > 0x1f) {
 522                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 523                                         BT_HCI_CMD_LE_SET_ADV_DATA);
 524                 return;
 525         }
 526
 527         hci->le_adv_data_len = cmd->len;
 528         memcpy(hci->le_adv_data, cmd->data, 31);
 529
 530         status = BT_HCI_ERR_SUCCESS;
 531         cmd_complete(hci, BT_HCI_CMD_LE_SET_ADV_DATA, &status, sizeof(status));
 532 }
 533
 534 static void cmd_le_set_scan_rsp_data(struct bt_le *hci,
 535                                                 const void *data, uint8_t size)
 536 {
 537         const struct bt_hci_cmd_le_set_scan_rsp_data *cmd = data;
 538         uint8_t status;
 539
 540         /* Valid range for scan response data length is 0x00 to 0x1f */
 541         if (cmd->len > 0x1f) {
 542                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 543                                         BT_HCI_CMD_LE_SET_SCAN_RSP_DATA);
 544                 return;
 545         }
 546
 547         hci->le_scan_rsp_data_len = cmd->len;
 548         memcpy(hci->le_scan_rsp_data, cmd->data, 31);
 549
 550         status = BT_HCI_ERR_SUCCESS;
 551         cmd_complete(hci, BT_HCI_CMD_LE_SET_SCAN_RSP_DATA,
 552                                                 &status, sizeof(status));
 553 }
 554
 555 static void cmd_le_set_adv_enable(struct bt_le *hci,
 556                                                 const void *data, uint8_t size)
 557 {
 558         const struct bt_hci_cmd_le_set_adv_enable *cmd = data;
 559         uint8_t status;
 560
 561         /* Valid range for advertising enable is 0x00 to 0x01 */
 562         if (cmd->enable > 0x01) {
 563                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 564                                         BT_HCI_CMD_LE_SET_ADV_ENABLE);
 565                 return;
 566         }
 567
 568         if (cmd->enable == hci->le_adv_enable) {
 569                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
 570                                         BT_HCI_CMD_LE_SET_ADV_ENABLE);
 571                 return;
 572         }
 573
 574         hci->le_adv_enable = cmd->enable;
 575
 576         status = BT_HCI_ERR_SUCCESS;
 577         cmd_complete(hci, BT_HCI_CMD_LE_SET_ADV_ENABLE,
 578                                                 &status, sizeof(status));
 579 }
 580
 581 static void cmd_le_read_white_list_size(struct bt_le *hci,
 582                                                 const void *data, uint8_t size)
 583 {
 584         struct bt_hci_rsp_le_read_white_list_size rsp;
 585
 586         rsp.status = BT_HCI_ERR_SUCCESS;
 587         rsp.size = hci->le_white_list_size;
 588
 589         cmd_complete(hci, BT_HCI_CMD_LE_READ_WHITE_LIST_SIZE,
 590                                                         &rsp, sizeof(rsp));
 591 }
 592
 593 static void cmd_le_clear_white_list(struct bt_le *hci,
 594                                                 const void *data, uint8_t size)
 595 {
 596         uint8_t status;
 597
 598         status = BT_HCI_ERR_SUCCESS;
 599         cmd_complete(hci, BT_HCI_CMD_LE_CLEAR_WHITE_LIST,
 600                                                 &status, sizeof(status));
 601 }
 602
 603 static void cmd_le_encrypt(struct bt_le *hci, const void *data, uint8_t size)
 604 {
 605         const struct bt_hci_cmd_le_encrypt *cmd = data;
 606         struct bt_hci_rsp_le_encrypt rsp;
 607
 608         if (!bt_crypto_e(hci->crypto, cmd->key, cmd->plaintext, rsp.data)) {
 609                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
 610                                         BT_HCI_CMD_LE_ENCRYPT);
 611                 return;
 612         }
 613
 614         rsp.status = BT_HCI_ERR_SUCCESS;
 615
 616         cmd_complete(hci, BT_HCI_CMD_LE_ENCRYPT, &rsp, sizeof(rsp));
 617 }
 618
 619 static void cmd_le_rand(struct bt_le *hci, const void *data, uint8_t size)
 620 {
 621         struct bt_hci_rsp_le_rand rsp;
 622         uint8_t value[8];
 623
 624         if (!bt_crypto_random_bytes(hci->crypto, value, 8)) {
 625                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
 626                                         BT_HCI_CMD_LE_RAND);
 627                 return;
 628         }
 629
 630         rsp.status = BT_HCI_ERR_SUCCESS;
 631         memcpy(&rsp.number, value, 8);
 632
 633         cmd_complete(hci, BT_HCI_CMD_LE_RAND, &rsp, sizeof(rsp));
 634 }
 635
 636 static void cmd_le_read_supported_states(struct bt_le *hci,
 637                                                 const void *data, uint8_t size)
 638 {
 639         struct bt_hci_rsp_le_read_supported_states rsp;
 640
 641         rsp.status = BT_HCI_ERR_SUCCESS;
 642         memcpy(rsp.states, hci->le_states, 8);
 643
 644         cmd_complete(hci, BT_HCI_CMD_LE_READ_SUPPORTED_STATES,
 645                                                         &rsp, sizeof(rsp));
 646 }
 647
 648 static const struct {
 649         uint16_t opcode;
 650         void (*func) (struct bt_le *hci, const void *data, uint8_t size);
 651         uint8_t size;
 652         bool fixed;
 653 } cmd_table[] = {
 654         { BT_HCI_CMD_SET_EVENT_MASK,       cmd_set_event_mask,      8, true },
 655         { BT_HCI_CMD_RESET,                cmd_reset,               0, true },
 656         { BT_HCI_CMD_READ_LOCAL_VERSION,   cmd_read_local_version,  0, true },
 657         { BT_HCI_CMD_READ_LOCAL_COMMANDS,  cmd_read_local_commands, 0, true },
 658         { BT_HCI_CMD_READ_LOCAL_FEATURES,  cmd_read_local_features, 0, true },
 659         { BT_HCI_CMD_READ_BUFFER_SIZE,     cmd_read_buffer_size,    0, true },
 660         { BT_HCI_CMD_READ_BD_ADDR,         cmd_read_bd_addr,        0, true },
 661
 662         { BT_HCI_CMD_LE_SET_EVENT_MASK,
 663                                 cmd_le_set_event_mask, 8, true },
 664         { BT_HCI_CMD_LE_READ_BUFFER_SIZE,
 665                                 cmd_le_read_buffer_size, 0, true },
 666         { BT_HCI_CMD_LE_READ_LOCAL_FEATURES,
 667                                 cmd_le_read_local_features, 0, true },
 668         { BT_HCI_CMD_LE_SET_RANDOM_ADDRESS,
 669                                 cmd_le_set_random_address, 6, true },
 670         { BT_HCI_CMD_LE_SET_ADV_PARAMETERS,
 671                                 cmd_le_set_adv_parameters, 15, true },
 672         { BT_HCI_CMD_LE_READ_ADV_TX_POWER,
 673                                 cmd_le_read_adv_tx_power, 0, true },
 674         { BT_HCI_CMD_LE_SET_ADV_DATA,
 675                                 cmd_le_set_adv_data, 32, true },
 676         { BT_HCI_CMD_LE_SET_SCAN_RSP_DATA,
 677                                 cmd_le_set_scan_rsp_data, 32, true },
 678         { BT_HCI_CMD_LE_SET_ADV_ENABLE,
 679                                 cmd_le_set_adv_enable, 1, true },
 680
 681         { BT_HCI_CMD_LE_READ_WHITE_LIST_SIZE,
 682                                 cmd_le_read_white_list_size, 0, true },
 683         { BT_HCI_CMD_LE_CLEAR_WHITE_LIST,
 684                                 cmd_le_clear_white_list, 0, true },
 685
 686         { BT_HCI_CMD_LE_ENCRYPT, cmd_le_encrypt, 32, true },
 687         { BT_HCI_CMD_LE_RAND, cmd_le_rand, 0, true },
 688
 689         { BT_HCI_CMD_LE_READ_SUPPORTED_STATES,
 690                                 cmd_le_read_supported_states, 0, true },
 691
 692         { }
 693 };
 694
 695 static void process_command(struct bt_le *hci, const void *data, size_t size)
 696 {
 697         const struct bt_hci_cmd_hdr *hdr = data;
 698         uint16_t opcode;
 699         unsigned int i;
 700
 701         if (size < sizeof(*hdr))
 702                 return;
 703
 704         data += sizeof(*hdr);
 705         size -= sizeof(*hdr);
 706
 707         opcode = le16_to_cpu(hdr->opcode);
 708
 709         if (hdr->plen != size) {
 710                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS, opcode);
 711                 return;
 712         }
 713
 714         for (i = 0; cmd_table[i].func; i++) {
 715                 if (cmd_table[i].opcode != opcode)
 716                         continue;
 717
 718                 if ((cmd_table[i].fixed && size != cmd_table[i].size) ||
 719                                                 size < cmd_table[i].size) {
 720                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS, opcode);
 721                         return;
 722                 }
 723
 724                 cmd_table[i].func(hci, data, size);
 725                 return;
 726         }
 727
 728         cmd_status(hci, BT_HCI_ERR_UNKNOWN_COMMAND, opcode);
 729 }
 730
 731 static void vhci_read_callback(int fd, uint32_t events, void *user_data)
 732 {
 733         struct bt_le *hci = user_data;
 734         unsigned char buf[4096];
 735         ssize_t len;
 736
 737         if (events & (EPOLLERR | EPOLLHUP))
 738                 return;
 739
 740         len = read(hci->vhci_fd, buf, sizeof(buf));
 741         if (len < 1)
 742                 return;
 743
 744         switch (buf[0]) {
 745         case BT_H4_CMD_PKT:
 746                 process_command(hci, buf + 1, len - 1);
 747                 break;
 748         }
 749 }
 750
 751 struct bt_le *bt_le_new(void)
 752 {
 753         unsigned char setup_cmd[2];
 754         struct bt_le *hci;
 755
 756         hci = calloc(1, sizeof(*hci));
 757         if (!hci)
 758                 return NULL;
 759
 760         reset_defaults(hci);
 761
 762         hci->vhci_fd = open("/dev/vhci", O_RDWR);
 763         if (hci->vhci_fd < 0) {
 764                 free(hci);
 765                 return NULL;
 766         }
 767
 768         setup_cmd[0] = HCI_VENDOR_PKT;
 769         setup_cmd[1] = HCI_BREDR;
 770
 771         if (write(hci->vhci_fd, setup_cmd, sizeof(setup_cmd)) < 0) {
 772                 close(hci->vhci_fd);
 773                 free(hci);
 774                 return NULL;
 775         }
 776
 777         mainloop_add_fd(hci->vhci_fd, EPOLLIN, vhci_read_callback, hci, NULL);
 778
 779         hci->crypto = bt_crypto_new();
 780
 781         return bt_le_ref(hci);
 782 }
 783
 784 struct bt_le *bt_le_ref(struct bt_le *hci)
 785 {
 786         if (!hci)
 787                 return NULL;
 788
 789         __sync_fetch_and_add(&hci->ref_count, 1);
 790
 791         return hci;
 792 }
 793
 794 void bt_le_unref(struct bt_le *hci)
 795 {
 796         if (!hci)
 797                 return;
 798
 799         if (__sync_sub_and_fetch(&hci->ref_count, 1))
 800                 return;
 801
 802         bt_crypto_unref(hci->crypto);
 803
 804         mainloop_remove_fd(hci->vhci_fd);
 805
 806         close(hci->vhci_fd);
 807
 808         free(hci);
 809 }