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 "lib/bluetooth.h"
  37 #include "lib/hci.h"
  38
  39 #include "src/shared/util.h"
  40 #include "src/shared/crypto.h"
  41 #include "src/shared/ecc.h"
  42 #include "src/shared/mainloop.h"
  43 #include "monitor/bt.h"
  44
  45 #include "phy.h"
  46 #include "le.h"
  47
  48 #define WHITE_LIST_SIZE         16
  49 #define RESOLV_LIST_SIZE        16
  50 #define SCAN_CACHE_SIZE         64
  51
  52 #define DEFAULT_TX_LEN          0x001b
  53 #define DEFAULT_TX_TIME         0x0148
  54 #define MAX_TX_LEN              0x00fb
  55 #define MAX_TX_TIME             0x0848
  56 #define MAX_RX_LEN              0x00fb
  57 #define MAX_RX_TIME             0x0848
  58
  59 struct bt_peer {
  60         uint8_t  addr_type;
  61         uint8_t  addr[6];
  62 };
  63
  64 struct bt_le {
  65         volatile int ref_count;
  66         int vhci_fd;
  67         struct bt_phy *phy;
  68         struct bt_crypto *crypto;
  69         int adv_timeout_id;
  70
  71         uint8_t  event_mask[16];
  72         uint16_t manufacturer;
  73         uint8_t  commands[64];
  74         uint8_t  features[8];
  75         uint8_t  bdaddr[6];
  76
  77         uint8_t  le_event_mask[8];
  78         uint16_t le_mtu;
  79         uint8_t  le_max_pkt;
  80         uint8_t  le_features[8];
  81         uint8_t  le_random_addr[6];
  82         uint16_t le_adv_min_interval;
  83         uint16_t le_adv_max_interval;
  84         uint8_t  le_adv_type;
  85         uint8_t  le_adv_own_addr_type;
  86         uint8_t  le_adv_direct_addr_type;
  87         uint8_t  le_adv_direct_addr[6];
  88         uint8_t  le_adv_channel_map;
  89         uint8_t  le_adv_filter_policy;
  90         int8_t   le_adv_tx_power;
  91         uint8_t  le_adv_data_len;
  92         uint8_t  le_adv_data[31];
  93         uint8_t  le_scan_rsp_data_len;
  94         uint8_t  le_scan_rsp_data[31];
  95         uint8_t  le_adv_enable;
  96         uint8_t  le_scan_type;
  97         uint16_t le_scan_interval;
  98         uint16_t le_scan_window;
  99         uint8_t  le_scan_own_addr_type;
 100         uint8_t  le_scan_filter_policy;
 101         uint8_t  le_scan_enable;
 102         uint8_t  le_scan_filter_dup;
 103
 104         uint8_t  le_conn_peer_addr_type;
 105         uint8_t  le_conn_peer_addr[6];
 106         uint8_t  le_conn_own_addr_type;
 107         uint8_t  le_conn_enable;
 108
 109         uint8_t  le_white_list_size;
 110         uint8_t  le_white_list[WHITE_LIST_SIZE][7];
 111         uint8_t  le_states[8];
 112
 113         uint16_t le_default_tx_len;
 114         uint16_t le_default_tx_time;
 115         uint8_t  le_local_sk256[32];
 116         uint8_t  le_resolv_list[RESOLV_LIST_SIZE][39];
 117         uint8_t  le_resolv_list_size;
 118         uint8_t  le_resolv_enable;
 119         uint16_t le_resolv_timeout;
 120
 121         struct bt_peer scan_cache[SCAN_CACHE_SIZE];
 122         uint8_t scan_cache_count;
 123 };
 124
 125 static bool is_in_white_list(struct bt_le *hci, uint8_t addr_type,
 126                                                         const uint8_t addr[6])
 127 {
 128         int i;
 129
 130         for (i = 0; i < hci->le_white_list_size; i++) {
 131                 if (hci->le_white_list[i][0] == addr_type &&
 132                                 !memcmp(&hci->le_white_list[i][1], addr, 6))
 133                         return true;
 134         }
 135
 136         return false;
 137 }
 138
 139 static void clear_white_list(struct bt_le *hci)
 140 {
 141         int i;
 142
 143         for (i = 0; i < hci->le_white_list_size; i++) {
 144                 hci->le_white_list[i][0] = 0xff;
 145                 memset(&hci->le_white_list[i][1], 0, 6);
 146         }
 147 }
 148
 149 static void resolve_peer_addr(struct bt_le *hci, uint8_t peer_addr_type,
 150                                         const uint8_t peer_addr[6],
 151                                         uint8_t *addr_type, uint8_t addr[6])
 152 {
 153         int i;
 154
 155         if (!hci->le_resolv_enable)
 156                 goto done;
 157
 158         if (peer_addr_type != 0x01)
 159                 goto done;
 160
 161         if ((peer_addr[5] & 0xc0) != 0x40)
 162                 goto done;
 163
 164         for (i = 0; i < hci->le_resolv_list_size; i++) {
 165                 uint8_t local_hash[3];
 166
 167                 if (hci->le_resolv_list[i][0] == 0xff)
 168                         continue;
 169
 170                 bt_crypto_ah(hci->crypto, &hci->le_resolv_list[i][7],
 171                                                 peer_addr + 3, local_hash);
 172
 173                 if (!memcmp(peer_addr, local_hash, 3)) {
 174                         switch (hci->le_resolv_list[i][0]) {
 175                         case 0x00:
 176                                 *addr_type = 0x02;
 177                                 break;
 178                         case 0x01:
 179                                 *addr_type = 0x03;
 180                                 break;
 181                         default:
 182                                 continue;
 183                         }
 184                         memcpy(addr, &hci->le_resolv_list[i][1], 6);
 185                         return;
 186                 }
 187         }
 188
 189 done:
 190         *addr_type = peer_addr_type;
 191         memcpy(addr, peer_addr, 6);
 192 }
 193
 194 static void clear_resolv_list(struct bt_le *hci)
 195 {
 196         int i;
 197
 198         for (i = 0; i < hci->le_resolv_list_size; i++) {
 199                 hci->le_resolv_list[i][0] = 0xff;
 200                 memset(&hci->le_resolv_list[i][1], 0, 38);
 201         }
 202 }
 203
 204 static void reset_defaults(struct bt_le *hci)
 205 {
 206         memset(hci->event_mask, 0, sizeof(hci->event_mask));
 207         hci->event_mask[0] |= 0x10;     /* Disconnection Complete */
 208         hci->event_mask[0] |= 0x80;     /* Encryption Change */
 209         hci->event_mask[1] |= 0x08;     /* Read Remote Version Information Complete */
 210         hci->event_mask[1] |= 0x20;     /* Command Complete */
 211         hci->event_mask[1] |= 0x40;     /* Command Status */
 212         hci->event_mask[1] |= 0x80;     /* Hardware Error */
 213         hci->event_mask[2] |= 0x04;     /* Number of Completed Packets */
 214         hci->event_mask[3] |= 0x02;     /* Data Buffer Overflow */
 215         hci->event_mask[5] |= 0x80;     /* Encryption Key Refresh Complete */
 216         //hci->event_mask[7] |= 0x20;   /* LE Meta Event */
 217
 218         hci->manufacturer = 0x003f;     /* Bluetooth SIG (63) */
 219
 220         memset(hci->commands, 0, sizeof(hci->commands));
 221         hci->commands[0]  |= 0x20;      /* Disconnect */
 222         //hci->commands[2]  |= 0x80;    /* Read Remote Version Information */
 223         hci->commands[5]  |= 0x40;      /* Set Event Mask */
 224         hci->commands[5]  |= 0x80;      /* Reset */
 225         //hci->commands[10] |= 0x04;    /* Read Transmit Power Level */
 226         hci->commands[14] |= 0x08;      /* Read Local Version Information */
 227         hci->commands[14] |= 0x10;      /* Read Local Supported Commands */
 228         hci->commands[14] |= 0x20;      /* Read Local Supported Features */
 229         hci->commands[14] |= 0x80;      /* Read Buffer Size */
 230         hci->commands[15] |= 0x02;      /* Read BD ADDR */
 231         //hci->commands[15] |= 0x20;    /* Read RSSI */
 232         hci->commands[22] |= 0x04;      /* Set Event Mask Page 2 */
 233         hci->commands[25] |= 0x01;      /* LE Set Event Mask */
 234         hci->commands[25] |= 0x02;      /* LE Read Buffer Size */
 235         hci->commands[25] |= 0x04;      /* LE Read Local Supported Features */
 236         hci->commands[25] |= 0x10;      /* LE Set Random Address */
 237         hci->commands[25] |= 0x20;      /* LE Set Advertising Parameters */
 238         hci->commands[25] |= 0x40;      /* LE Read Advertising Channel TX Power */
 239         hci->commands[25] |= 0x80;      /* LE Set Advertising Data */
 240         hci->commands[26] |= 0x01;      /* LE Set Scan Response Data */
 241         hci->commands[26] |= 0x02;      /* LE Set Advertise Enable */
 242         hci->commands[26] |= 0x04;      /* LE Set Scan Parameters */
 243         hci->commands[26] |= 0x08;      /* LE Set Scan Enable */
 244         hci->commands[26] |= 0x10;      /* LE Create Connection */
 245         hci->commands[26] |= 0x20;      /* LE Create Connection Cancel */
 246         hci->commands[26] |= 0x40;      /* LE Read White List Size */
 247         hci->commands[26] |= 0x80;      /* LE Clear White List */
 248         hci->commands[27] |= 0x01;      /* LE Add Device To White List */
 249         hci->commands[27] |= 0x02;      /* LE Remove Device From White List */
 250         //hci->commands[27] |= 0x04;    /* LE Connection Update */
 251         //hci->commands[27] |= 0x08;    /* LE Set Host Channel Classification */
 252         //hci->commands[27] |= 0x10;    /* LE Read Channel Map */
 253         //hci->commands[27] |= 0x20;    /* LE Read Remote Used Features */
 254         hci->commands[27] |= 0x40;      /* LE Encrypt */
 255         hci->commands[27] |= 0x80;      /* LE Rand */
 256         //hci->commands[28] |= 0x01;    /* LE Start Encryption */
 257         //hci->commands[28] |= 0x02;    /* LE Long Term Key Request Reply */
 258         //hci->commands[28] |= 0x04;    /* LE Long Term Key Request Negative Reply */
 259         hci->commands[28] |= 0x08;      /* LE Read Supported States */
 260         //hci->commands[28] |= 0x10;    /* LE Receiver Test */
 261         //hci->commands[28] |= 0x20;    /* LE Transmitter Test */
 262         //hci->commands[28] |= 0x40;    /* LE Test End */
 263         //hci->commands[33] |= 0x10;    /* LE Remote Connection Parameter Request Reply */
 264         //hci->commands[33] |= 0x20;    /* LE Remote Connection Parameter Request Negative Reply */
 265         hci->commands[33] |= 0x40;      /* LE Set Data Length */
 266         hci->commands[33] |= 0x80;      /* LE Read Suggested Default Data Length */
 267         hci->commands[34] |= 0x01;      /* LE Write Suggested Default Data Length */
 268         hci->commands[34] |= 0x02;      /* LE Read Local P-256 Public Key */
 269         hci->commands[34] |= 0x04;      /* LE Generate DHKey */
 270         hci->commands[34] |= 0x08;      /* LE Add Device To Resolving List */
 271         hci->commands[34] |= 0x10;      /* LE Remove Device From Resolving List */
 272         hci->commands[34] |= 0x20;      /* LE Clear Resolving List */
 273         hci->commands[34] |= 0x40;      /* LE Read Resolving List Size */
 274         hci->commands[34] |= 0x80;      /* LE Read Peer Resolvable Address */
 275         hci->commands[35] |= 0x01;      /* LE Read Local Resolvable Address */
 276         hci->commands[35] |= 0x02;      /* LE Set Address Resolution Enable */
 277         hci->commands[35] |= 0x04;      /* LE Set Resolvable Private Address Timeout */
 278         hci->commands[35] |= 0x08;      /* LE Read Maximum Data Length */
 279
 280         memset(hci->features, 0, sizeof(hci->features));
 281         hci->features[4] |= 0x20;       /* BR/EDR Not Supported */
 282         hci->features[4] |= 0x40;       /* LE Supported */
 283
 284         memset(hci->bdaddr, 0, sizeof(hci->bdaddr));
 285
 286         memset(hci->le_event_mask, 0, sizeof(hci->le_event_mask));
 287         hci->le_event_mask[0] |= 0x01;  /* LE Connection Complete */
 288         hci->le_event_mask[0] |= 0x02;  /* LE Advertising Report */
 289         hci->le_event_mask[0] |= 0x04;  /* LE Connection Update Complete */
 290         hci->le_event_mask[0] |= 0x08;  /* LE Read Remote Used Features Complete */
 291         hci->le_event_mask[0] |= 0x10;  /* LE Long Term Key Request */
 292         //hci->le_event_mask[0] |= 0x20;        /* LE Remote Connection Parameter Request */
 293         //hci->le_event_mask[0] |= 0x40;        /* LE Data Length Change */
 294         //hci->le_event_mask[0] |= 0x80;        /* LE Read Local P-256 Public Key Complete */
 295         //hci->le_event_mask[1] |= 0x01;        /* LE Generate DHKey Complete */
 296         //hci->le_event_mask[1] |= 0x02;        /* LE Enhanced Connection Complete */
 297         //hci->le_event_mask[1] |= 0x04;        /* LE Direct Advertising Report */
 298
 299         hci->le_mtu = 64;
 300         hci->le_max_pkt = 1;
 301
 302         memset(hci->le_features, 0, sizeof(hci->le_features));
 303         hci->le_features[0] |= 0x01;    /* LE Encryption */
 304         //hci->le_features[0] |= 0x02;  /* Connection Parameter Request Procedure */
 305         //hci->le_features[0] |= 0x04;  /* Extended Reject Indication */
 306         //hci->le_features[0] |= 0x08;  /* Slave-initiated Features Exchange */
 307         hci->le_features[0] |= 0x10;    /* LE Ping */
 308         hci->le_features[0] |= 0x20;    /* LE Data Packet Length Extension */
 309         hci->le_features[0] |= 0x40;    /* LL Privacy */
 310         hci->le_features[0] |= 0x80;    /* Extended Scanner Filter Policies */
 311
 312         memset(hci->le_random_addr, 0, sizeof(hci->le_random_addr));
 313
 314         hci->le_adv_min_interval = 0x0800;
 315         hci->le_adv_max_interval = 0x0800;
 316         hci->le_adv_type = 0x00;
 317         hci->le_adv_own_addr_type = 0x00;
 318         hci->le_adv_direct_addr_type = 0x00;
 319         memset(hci->le_adv_direct_addr, 0, 6);
 320         hci->le_adv_channel_map = 0x07;
 321         hci->le_adv_filter_policy = 0x00;
 322
 323         hci->le_adv_tx_power = 0;
 324
 325         memset(hci->le_adv_data, 0, sizeof(hci->le_adv_data));
 326         hci->le_adv_data_len = 0;
 327
 328         memset(hci->le_scan_rsp_data, 0, sizeof(hci->le_scan_rsp_data));
 329         hci->le_scan_rsp_data_len = 0;
 330
 331         hci->le_adv_enable = 0x00;
 332
 333         hci->le_scan_type = 0x00;               /* Passive Scanning */
 334         hci->le_scan_interval = 0x0010;         /* 10 ms */
 335         hci->le_scan_window = 0x0010;           /* 10 ms */
 336         hci->le_scan_own_addr_type = 0x00;      /* Public Device Address */
 337         hci->le_scan_filter_policy = 0x00;
 338         hci->le_scan_enable = 0x00;
 339         hci->le_scan_filter_dup = 0x00;
 340
 341         hci->le_conn_enable = 0x00;
 342
 343         hci->le_white_list_size = WHITE_LIST_SIZE;
 344         clear_white_list(hci);
 345
 346         memset(hci->le_states, 0, sizeof(hci->le_states));
 347         hci->le_states[0] |= 0x01;      /* Non-connectable Advertising */
 348         hci->le_states[0] |= 0x02;      /* Scannable Advertising */
 349         hci->le_states[0] |= 0x04;      /* Connectable Advertising */
 350         hci->le_states[0] |= 0x08;      /* High Duty Cycle Directed Advertising */
 351         hci->le_states[0] |= 0x10;      /* Passive Scanning */
 352         hci->le_states[0] |= 0x20;      /* Active Scanning */
 353         hci->le_states[0] |= 0x40;      /* Initiating + Connection (Master Role) */
 354         hci->le_states[0] |= 0x80;      /* Connection (Slave Role) */
 355         hci->le_states[1] |= 0x01;      /* Passive Scanning +
 356                                          * Non-connectable Advertising */
 357
 358         hci->le_default_tx_len = DEFAULT_TX_LEN;
 359         hci->le_default_tx_time = DEFAULT_TX_TIME;
 360
 361         memset(hci->le_local_sk256, 0, sizeof(hci->le_local_sk256));
 362
 363         hci->le_resolv_list_size = RESOLV_LIST_SIZE;
 364         clear_resolv_list(hci);
 365         hci->le_resolv_enable = 0x00;
 366         hci->le_resolv_timeout = 0x0384;        /* 900 secs or 15 minutes */
 367 }
 368
 369 static void clear_scan_cache(struct bt_le *hci)
 370 {
 371         memset(hci->scan_cache, 0, sizeof(hci->scan_cache));
 372         hci->scan_cache_count = 0;
 373 }
 374
 375 static bool add_to_scan_cache(struct bt_le *hci, uint8_t addr_type,
 376                                                         const uint8_t addr[6])
 377 {
 378         int i;
 379
 380         for (i = 0; i < hci->scan_cache_count; i++) {
 381                 if (hci->scan_cache[i].addr_type == addr_type &&
 382                                 !memcmp(hci->scan_cache[i].addr, addr, 6))
 383                         return false;
 384         }
 385
 386         if (hci->scan_cache_count >= SCAN_CACHE_SIZE)
 387                 return true;
 388
 389         hci->scan_cache[hci->scan_cache_count].addr_type = addr_type;
 390         memcpy(hci->scan_cache[hci->scan_cache_count].addr, addr, 6);
 391         hci->scan_cache_count++;
 392
 393         return true;
 394 }
 395
 396 static void send_event(struct bt_le *hci, uint8_t event,
 397                                                 void *data, uint8_t size)
 398 {
 399         uint8_t type = BT_H4_EVT_PKT;
 400         struct bt_hci_evt_hdr hdr;
 401         struct iovec iov[3];
 402         int iovcnt;
 403
 404         hdr.evt  = event;
 405         hdr.plen = size;
 406
 407         iov[0].iov_base = &type;
 408         iov[0].iov_len  = 1;
 409         iov[1].iov_base = &hdr;
 410         iov[1].iov_len  = sizeof(hdr);
 411
 412         if (size > 0) {
 413                 iov[2].iov_base = data;
 414                 iov[2].iov_len  = size;
 415                 iovcnt = 3;
 416         } else
 417                 iovcnt = 2;
 418
 419         if (writev(hci->vhci_fd, iov, iovcnt) < 0)
 420                 fprintf(stderr, "Write to /dev/vhci failed (%m)\n");
 421 }
 422
 423 static void send_adv_pkt(struct bt_le *hci)
 424 {
 425         struct bt_phy_pkt_adv pkt;
 426
 427         memset(&pkt, 0, sizeof(pkt));
 428         pkt.pdu_type = hci->le_adv_type;
 429         pkt.tx_addr_type = hci->le_adv_own_addr_type;
 430         switch (hci->le_adv_own_addr_type) {
 431         case 0x00:
 432         case 0x02:
 433                 memcpy(pkt.tx_addr, hci->bdaddr, 6);
 434                 break;
 435         case 0x01:
 436         case 0x03:
 437                 memcpy(pkt.tx_addr, hci->le_random_addr, 6);
 438                 break;
 439         }
 440         pkt.rx_addr_type = hci->le_adv_direct_addr_type;
 441         memcpy(pkt.rx_addr, hci->le_adv_direct_addr, 6);
 442         pkt.adv_data_len = hci->le_adv_data_len;
 443         pkt.scan_rsp_len = hci->le_scan_rsp_data_len;
 444
 445         bt_phy_send_vector(hci->phy, BT_PHY_PKT_ADV, &pkt, sizeof(pkt),
 446                                 hci->le_adv_data, pkt.adv_data_len,
 447                                 hci->le_scan_rsp_data, pkt.scan_rsp_len);
 448 }
 449
 450 static void adv_timeout_callback(int id, void *user_data)
 451 {
 452         struct bt_le *hci = user_data;
 453         unsigned int min_msec, max_msec;
 454
 455         send_adv_pkt(hci);
 456
 457         min_msec = (hci->le_adv_min_interval * 625) / 1000;
 458         max_msec = (hci->le_adv_max_interval * 625) / 1000;
 459
 460         if (mainloop_modify_timeout(id, (min_msec + max_msec) / 2) < 0) {
 461                 fprintf(stderr, "Setting advertising timeout failed\n");
 462                 hci->le_adv_enable = 0x00;
 463         }
 464 }
 465
 466 static bool start_adv(struct bt_le *hci)
 467 {
 468         unsigned int msec;
 469
 470         if (hci->adv_timeout_id >= 0)
 471                 return false;
 472
 473         msec = (hci->le_adv_min_interval * 625) / 1000;
 474
 475         hci->adv_timeout_id = mainloop_add_timeout(msec, adv_timeout_callback,
 476                                                                 hci, NULL);
 477         if (hci->adv_timeout_id < 0)
 478                 return false;
 479
 480         return true;
 481 }
 482
 483 static bool stop_adv(struct bt_le *hci)
 484 {
 485         if (hci->adv_timeout_id < 0)
 486                 return false;
 487
 488         mainloop_remove_timeout(hci->adv_timeout_id);
 489         hci->adv_timeout_id = -1;
 490
 491         return true;
 492 }
 493
 494 static void cmd_complete(struct bt_le *hci, uint16_t opcode,
 495                                                 const void *data, uint8_t len)
 496 {
 497         struct bt_hci_evt_cmd_complete *cc;
 498         void *pkt_data;
 499
 500         pkt_data = alloca(sizeof(*cc) + len);
 501         if (!pkt_data)
 502                 return;
 503
 504         cc = pkt_data;
 505         cc->ncmd = 0x01;
 506         cc->opcode = cpu_to_le16(opcode);
 507
 508         if (len > 0)
 509                 memcpy(pkt_data + sizeof(*cc), data, len);
 510
 511         send_event(hci, BT_HCI_EVT_CMD_COMPLETE, pkt_data, sizeof(*cc) + len);
 512 }
 513
 514 static void cmd_status(struct bt_le *hci, uint8_t status, uint16_t opcode)
 515 {
 516         struct bt_hci_evt_cmd_status cs;
 517
 518         cs.status = status;
 519         cs.ncmd = 0x01;
 520         cs.opcode = cpu_to_le16(opcode);
 521
 522         send_event(hci, BT_HCI_EVT_CMD_STATUS, &cs, sizeof(cs));
 523 }
 524
 525 static void le_meta_event(struct bt_le *hci, uint8_t event,
 526                                                 void *data, uint8_t len)
 527 {
 528         void *pkt_data;
 529
 530         if (!(hci->event_mask[7] & 0x20))
 531                 return;
 532
 533         pkt_data = alloca(1 + len);
 534         if (!pkt_data)
 535                 return;
 536
 537         ((uint8_t *) pkt_data)[0] = event;
 538
 539         if (len > 0)
 540                 memcpy(pkt_data + 1, data, len);
 541
 542         send_event(hci, BT_HCI_EVT_LE_META_EVENT, pkt_data, 1 + len);
 543 }
 544
 545 static void cmd_disconnect(struct bt_le *hci, const void *data, uint8_t size)
 546 {
 547         cmd_status(hci, BT_HCI_ERR_UNKNOWN_CONN_ID, BT_HCI_CMD_DISCONNECT);
 548 }
 549
 550 static void cmd_set_event_mask(struct bt_le *hci,
 551                                                 const void *data, uint8_t size)
 552 {
 553         const struct bt_hci_cmd_set_event_mask *cmd = data;
 554         uint8_t status;
 555
 556         memcpy(hci->event_mask, cmd->mask, 8);
 557
 558         status = BT_HCI_ERR_SUCCESS;
 559         cmd_complete(hci, BT_HCI_CMD_SET_EVENT_MASK, &status, sizeof(status));
 560 }
 561
 562 static void cmd_reset(struct bt_le *hci, const void *data, uint8_t size)
 563 {
 564         uint8_t status;
 565
 566         stop_adv(hci);
 567         reset_defaults(hci);
 568
 569         status = BT_HCI_ERR_SUCCESS;
 570         cmd_complete(hci, BT_HCI_CMD_RESET, &status, sizeof(status));
 571 }
 572
 573 static void cmd_set_event_mask_page2(struct bt_le *hci,
 574                                                 const void *data, uint8_t size)
 575 {
 576         const struct bt_hci_cmd_set_event_mask_page2 *cmd = data;
 577         uint8_t status;
 578
 579         memcpy(hci->event_mask + 8, cmd->mask, 8);
 580
 581         status = BT_HCI_ERR_SUCCESS;
 582         cmd_complete(hci, BT_HCI_CMD_SET_EVENT_MASK_PAGE2,
 583                                                 &status, sizeof(status));
 584 }
 585
 586 static void cmd_read_local_version(struct bt_le *hci,
 587                                                 const void *data, uint8_t size)
 588 {
 589         struct bt_hci_rsp_read_local_version rsp;
 590
 591         rsp.status = BT_HCI_ERR_SUCCESS;
 592         rsp.hci_ver = 0x08;
 593         rsp.hci_rev = cpu_to_le16(0x0000);
 594         rsp.lmp_ver = 0x08;
 595         rsp.manufacturer = cpu_to_le16(hci->manufacturer);
 596         rsp.lmp_subver = cpu_to_le16(0x0000);
 597
 598         cmd_complete(hci, BT_HCI_CMD_READ_LOCAL_VERSION, &rsp, sizeof(rsp));
 599 }
 600
 601 static void cmd_read_local_commands(struct bt_le *hci,
 602                                                 const void *data, uint8_t size)
 603 {
 604         struct bt_hci_rsp_read_local_commands rsp;
 605
 606         rsp.status = BT_HCI_ERR_SUCCESS;
 607         memcpy(rsp.commands, hci->commands, 64);
 608
 609         cmd_complete(hci, BT_HCI_CMD_READ_LOCAL_COMMANDS, &rsp, sizeof(rsp));
 610 }
 611
 612 static void cmd_read_local_features(struct bt_le *hci,
 613                                                 const void *data, uint8_t size)
 614 {
 615         struct bt_hci_rsp_read_local_features rsp;
 616
 617         rsp.status = BT_HCI_ERR_SUCCESS;
 618         memcpy(rsp.features, hci->features, 8);
 619
 620         cmd_complete(hci, BT_HCI_CMD_READ_LOCAL_FEATURES, &rsp, sizeof(rsp));
 621 }
 622
 623 static void cmd_read_buffer_size(struct bt_le *hci,
 624                                                 const void *data, uint8_t size)
 625 {
 626         struct bt_hci_rsp_read_buffer_size rsp;
 627
 628         rsp.status = BT_HCI_ERR_SUCCESS;
 629         rsp.acl_mtu = cpu_to_le16(0x0000);
 630         rsp.sco_mtu = 0x00;
 631         rsp.acl_max_pkt = cpu_to_le16(0x0000);
 632         rsp.sco_max_pkt = cpu_to_le16(0x0000);
 633
 634         cmd_complete(hci, BT_HCI_CMD_READ_BUFFER_SIZE, &rsp, sizeof(rsp));
 635 }
 636
 637 static void cmd_read_bd_addr(struct bt_le *hci, const void *data, uint8_t size)
 638 {
 639         struct bt_hci_rsp_read_bd_addr rsp;
 640
 641         rsp.status = BT_HCI_ERR_SUCCESS;
 642         memcpy(rsp.bdaddr, hci->bdaddr, 6);
 643
 644         cmd_complete(hci, BT_HCI_CMD_READ_BD_ADDR, &rsp, sizeof(rsp));
 645 }
 646
 647 static void cmd_le_set_event_mask(struct bt_le *hci,
 648                                                 const void *data, uint8_t size)
 649 {
 650         const struct bt_hci_cmd_le_set_event_mask *cmd = data;
 651         uint8_t status;
 652
 653         memcpy(hci->le_event_mask, cmd->mask, 8);
 654
 655         status = BT_HCI_ERR_SUCCESS;
 656         cmd_complete(hci, BT_HCI_CMD_LE_SET_EVENT_MASK,
 657                                                 &status, sizeof(status));
 658 }
 659
 660 static void cmd_le_read_buffer_size(struct bt_le *hci,
 661                                                 const void *data, uint8_t size)
 662 {
 663         struct bt_hci_rsp_le_read_buffer_size rsp;
 664
 665         rsp.status = BT_HCI_ERR_SUCCESS;
 666         rsp.le_mtu = cpu_to_le16(hci->le_mtu);
 667         rsp.le_max_pkt = hci->le_max_pkt;
 668
 669         cmd_complete(hci, BT_HCI_CMD_LE_READ_BUFFER_SIZE, &rsp, sizeof(rsp));
 670 }
 671
 672 static void cmd_le_read_local_features(struct bt_le *hci,
 673                                                 const void *data, uint8_t size)
 674 {
 675         struct bt_hci_rsp_le_read_local_features rsp;
 676
 677         rsp.status = BT_HCI_ERR_SUCCESS;
 678         memcpy(rsp.features, hci->le_features, 8);
 679
 680         cmd_complete(hci, BT_HCI_CMD_LE_READ_LOCAL_FEATURES,
 681                                                         &rsp, sizeof(rsp));
 682 }
 683
 684 static void cmd_le_set_random_address(struct bt_le *hci,
 685                                                 const void *data, uint8_t size)
 686 {
 687         const struct bt_hci_cmd_le_set_random_address *cmd = data;
 688         uint8_t status;
 689
 690         memcpy(hci->le_random_addr, cmd->addr, 6);
 691
 692         status = BT_HCI_ERR_SUCCESS;
 693         cmd_complete(hci, BT_HCI_CMD_LE_SET_RANDOM_ADDRESS,
 694                                                 &status, sizeof(status));
 695 }
 696
 697 static void cmd_le_set_adv_parameters(struct bt_le *hci,
 698                                                 const void *data, uint8_t size)
 699 {
 700         const struct bt_hci_cmd_le_set_adv_parameters *cmd = data;
 701         uint16_t min_interval, max_interval;
 702         uint8_t status;
 703
 704         if (hci->le_adv_enable == 0x01) {
 705                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
 706                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 707                 return;
 708         }
 709
 710         min_interval = le16_to_cpu(cmd->min_interval);
 711         max_interval = le16_to_cpu(cmd->max_interval);
 712
 713         /* Valid range for advertising type is 0x00 to 0x03 */
 714         switch (cmd->type) {
 715         case 0x00:      /* ADV_IND */
 716                 /* Range for advertising interval min is 0x0020 to 0x4000 */
 717                 if (min_interval < 0x0020 || min_interval > 0x4000) {
 718                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 719                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 720                         return;
 721                 }
 722                 /* Range for advertising interval max is 0x0020 to 0x4000 */
 723                 if (max_interval < 0x0020 || max_interval > 0x4000) {
 724                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 725                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 726                         return;
 727                 }
 728                 /* Advertising interval max shall be less or equal */
 729                 if (min_interval > max_interval) {
 730                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 731                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 732                         return;
 733                 }
 734                 break;
 735
 736         case 0x01:      /* ADV_DIRECT_IND */
 737                 /* Range for direct address type is 0x00 to 0x01 */
 738                 if (cmd->direct_addr_type > 0x01) {
 739                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 740                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 741                         return;
 742                 }
 743                 break;
 744
 745         case 0x02:      /* ADV_SCAN_IND */
 746         case 0x03:      /* ADV_NONCONN_IND */
 747                 /* Range for advertising interval min is 0x00a0 to 0x4000 */
 748                 if (min_interval < 0x00a0 || min_interval > 0x4000) {
 749                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 750                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 751                         return;
 752                 }
 753                 /* Range for advertising interval max is 0x00a0 to 0x4000 */
 754                 if (max_interval < 0x00a0 || max_interval > 0x4000) {
 755                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 756                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 757                         return;
 758                 }
 759                 /* Advertising interval min shall be less or equal */
 760                 if (min_interval > max_interval) {
 761                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 762                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 763                         return;
 764                 }
 765                 break;
 766
 767         default:
 768                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 769                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 770                 return;
 771         }
 772
 773         /* Valid range for own address type is 0x00 to 0x03 */
 774         if (cmd->own_addr_type > 0x03) {
 775                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 776                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 777                 return;
 778         }
 779
 780         /* Valid range for advertising channel map is 0x01 to 0x07 */
 781         if (cmd->channel_map < 0x01 || cmd->channel_map > 0x07) {
 782                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 783                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 784                 return;
 785         }
 786
 787         /* Valid range for advertising filter policy is 0x00 to 0x03 */
 788         if (cmd->filter_policy > 0x03) {
 789                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 790                                         BT_HCI_CMD_LE_SET_ADV_PARAMETERS);
 791                 return;
 792         }
 793
 794         hci->le_adv_min_interval = min_interval;
 795         hci->le_adv_max_interval = max_interval;
 796         hci->le_adv_type = cmd->type;
 797         hci->le_adv_own_addr_type = cmd->own_addr_type;
 798         hci->le_adv_direct_addr_type = cmd->direct_addr_type;
 799         memcpy(hci->le_adv_direct_addr, cmd->direct_addr, 6);
 800         hci->le_adv_channel_map = cmd->channel_map;
 801         hci->le_adv_filter_policy = cmd->filter_policy;
 802
 803         status = BT_HCI_ERR_SUCCESS;
 804         cmd_complete(hci, BT_HCI_CMD_LE_SET_ADV_PARAMETERS,
 805                                                 &status, sizeof(status));
 806 }
 807
 808 static void cmd_le_read_adv_tx_power(struct bt_le *hci,
 809                                                 const void *data, uint8_t size)
 810 {
 811         struct bt_hci_rsp_le_read_adv_tx_power rsp;
 812
 813         rsp.status = BT_HCI_ERR_SUCCESS;
 814         rsp.level = hci->le_adv_tx_power;
 815
 816         cmd_complete(hci, BT_HCI_CMD_LE_READ_ADV_TX_POWER, &rsp, sizeof(rsp));
 817 }
 818
 819 static void cmd_le_set_adv_data(struct bt_le *hci,
 820                                                 const void *data, uint8_t size)
 821 {
 822         const struct bt_hci_cmd_le_set_adv_data *cmd = data;
 823         uint8_t status;
 824
 825         /* Valid range for advertising data length is 0x00 to 0x1f */
 826         if (cmd->len > 0x1f) {
 827                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 828                                         BT_HCI_CMD_LE_SET_ADV_DATA);
 829                 return;
 830         }
 831
 832         hci->le_adv_data_len = cmd->len;
 833         memcpy(hci->le_adv_data, cmd->data, 31);
 834
 835         status = BT_HCI_ERR_SUCCESS;
 836         cmd_complete(hci, BT_HCI_CMD_LE_SET_ADV_DATA, &status, sizeof(status));
 837 }
 838
 839 static void cmd_le_set_scan_rsp_data(struct bt_le *hci,
 840                                                 const void *data, uint8_t size)
 841 {
 842         const struct bt_hci_cmd_le_set_scan_rsp_data *cmd = data;
 843         uint8_t status;
 844
 845         /* Valid range for scan response data length is 0x00 to 0x1f */
 846         if (cmd->len > 0x1f) {
 847                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 848                                         BT_HCI_CMD_LE_SET_SCAN_RSP_DATA);
 849                 return;
 850         }
 851
 852         hci->le_scan_rsp_data_len = cmd->len;
 853         memcpy(hci->le_scan_rsp_data, cmd->data, 31);
 854
 855         status = BT_HCI_ERR_SUCCESS;
 856         cmd_complete(hci, BT_HCI_CMD_LE_SET_SCAN_RSP_DATA,
 857                                                 &status, sizeof(status));
 858 }
 859
 860 static void cmd_le_set_adv_enable(struct bt_le *hci,
 861                                                 const void *data, uint8_t size)
 862 {
 863         const struct bt_hci_cmd_le_set_adv_enable *cmd = data;
 864         uint8_t status;
 865         bool result;
 866
 867         /* Valid range for advertising enable is 0x00 to 0x01 */
 868         if (cmd->enable > 0x01) {
 869                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 870                                         BT_HCI_CMD_LE_SET_ADV_ENABLE);
 871                 return;
 872         }
 873
 874         if (cmd->enable == hci->le_adv_enable) {
 875                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
 876                                         BT_HCI_CMD_LE_SET_ADV_ENABLE);
 877                 return;
 878         }
 879
 880         if (cmd->enable == 0x01)
 881                 result = start_adv(hci);
 882         else
 883                 result = stop_adv(hci);
 884
 885         if (!result) {
 886                 cmd_status(hci, BT_HCI_ERR_UNSPECIFIED_ERROR,
 887                                         BT_HCI_CMD_LE_SET_ADV_ENABLE);
 888                 return;
 889         }
 890
 891         hci->le_adv_enable = cmd->enable;
 892
 893         status = BT_HCI_ERR_SUCCESS;
 894         cmd_complete(hci, BT_HCI_CMD_LE_SET_ADV_ENABLE,
 895                                                 &status, sizeof(status));
 896 }
 897
 898 static void cmd_le_set_scan_parameters(struct bt_le *hci,
 899                                                 const void *data, uint8_t size)
 900 {
 901         const struct bt_hci_cmd_le_set_scan_parameters *cmd = data;
 902         uint16_t interval, window;
 903         uint8_t status;
 904
 905         if (hci->le_scan_enable == 0x01) {
 906                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
 907                                         BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
 908                 return;
 909         }
 910
 911         interval = le16_to_cpu(cmd->interval);
 912         window = le16_to_cpu(cmd->window);
 913
 914         /* Valid range for scan type is 0x00 to 0x01 */
 915         if (cmd->type > 0x01) {
 916                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 917                                         BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
 918                 return;
 919         }
 920
 921         /* Valid range for scan interval is 0x0004 to 0x4000 */
 922         if (interval < 0x0004 || interval > 0x4000) {
 923                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 924                                         BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
 925                 return;
 926         }
 927
 928         /* Valid range for scan window is 0x0004 to 0x4000 */
 929         if (window < 0x0004 || window > 0x4000) {
 930                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 931                                         BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
 932                 return;
 933         }
 934
 935         /* Scan window shall be less or equal than scan interval */
 936         if (window > interval) {
 937                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 938                                         BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
 939                 return;
 940         }
 941
 942         /* Valid range for own address type is 0x00 to 0x03 */
 943         if (cmd->own_addr_type > 0x03) {
 944                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 945                                         BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
 946                 return;
 947         }
 948
 949         /* Valid range for scanning filter policy is 0x00 to 0x03 */
 950         if (cmd->filter_policy > 0x03) {
 951                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 952                                         BT_HCI_CMD_LE_SET_SCAN_PARAMETERS);
 953                 return;
 954         }
 955
 956         hci->le_scan_type = cmd->type;
 957         hci->le_scan_interval = interval;
 958         hci->le_scan_window = window;
 959         hci->le_scan_own_addr_type = cmd->own_addr_type;
 960         hci->le_scan_filter_policy = cmd->filter_policy;
 961
 962         status = BT_HCI_ERR_SUCCESS;
 963         cmd_complete(hci, BT_HCI_CMD_LE_SET_SCAN_PARAMETERS,
 964                                                 &status, sizeof(status));
 965 }
 966
 967 static void cmd_le_set_scan_enable(struct bt_le *hci,
 968                                                 const void *data, uint8_t size)
 969 {
 970         const struct bt_hci_cmd_le_set_scan_enable *cmd = data;
 971         uint8_t status;
 972
 973         /* Valid range for scan enable is 0x00 to 0x01 */
 974         if (cmd->enable > 0x01) {
 975                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 976                                         BT_HCI_CMD_LE_SET_SCAN_ENABLE);
 977                 return;
 978         }
 979
 980         /* Valid range for filter duplicates is 0x00 to 0x01 */
 981         if (cmd->filter_dup > 0x01) {
 982                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
 983                                         BT_HCI_CMD_LE_SET_SCAN_ENABLE);
 984                 return;
 985         }
 986
 987         if (cmd->enable == hci->le_scan_enable) {
 988                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
 989                                         BT_HCI_CMD_LE_SET_SCAN_ENABLE);
 990                 return;
 991         }
 992
 993         clear_scan_cache(hci);
 994
 995         hci->le_scan_enable = cmd->enable;
 996         hci->le_scan_filter_dup = cmd->filter_dup;
 997
 998         status = BT_HCI_ERR_SUCCESS;
 999         cmd_complete(hci, BT_HCI_CMD_LE_SET_SCAN_ENABLE,
1000                                                 &status, sizeof(status));
1001 }
1002
1003 static void cmd_le_create_conn(struct bt_le *hci,
1004                                                 const void *data, uint8_t size)
1005 {
1006         const struct bt_hci_cmd_le_create_conn *cmd = data;
1007
1008         if (hci->le_conn_enable == 0x01) {
1009                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1010                                         BT_HCI_CMD_LE_CREATE_CONN);
1011                 return;
1012         }
1013
1014         /* Valid range for peer address type is 0x00 to 0x03 */
1015         if (cmd->peer_addr_type > 0x03) {
1016                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1017                                         BT_HCI_CMD_LE_CREATE_CONN);
1018                 return;
1019         }
1020
1021         /* Valid range for own address type is 0x00 to 0x03 */
1022         if (cmd->own_addr_type > 0x03) {
1023                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1024                                         BT_HCI_CMD_LE_CREATE_CONN);
1025                 return;
1026         }
1027
1028         hci->le_conn_peer_addr_type = cmd->peer_addr_type;
1029         memcpy(hci->le_conn_peer_addr, cmd->peer_addr, 6);
1030         hci->le_conn_own_addr_type = cmd->own_addr_type;
1031         hci->le_conn_enable = 0x01;
1032
1033         cmd_status(hci, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_CREATE_CONN);
1034 }
1035
1036 static void cmd_le_create_conn_cancel(struct bt_le *hci,
1037                                                 const void *data, uint8_t size)
1038 {
1039         struct bt_hci_evt_le_conn_complete evt;
1040         uint8_t status;
1041
1042         if (hci->le_conn_enable == 0x00) {
1043                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1044                                         BT_HCI_CMD_LE_CREATE_CONN_CANCEL);
1045                 return;
1046         }
1047
1048         hci->le_conn_enable = 0x00;
1049
1050         status = BT_HCI_ERR_SUCCESS;
1051         cmd_complete(hci, BT_HCI_CMD_LE_CREATE_CONN_CANCEL,
1052                                                 &status, sizeof(status));
1053
1054         evt.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1055         evt.handle = cpu_to_le16(0x0000);
1056         evt.role = 0x00;
1057         evt.peer_addr_type = 0x00;
1058         memset(evt.peer_addr, 0, 6);
1059         evt.interval = cpu_to_le16(0x0000);
1060         evt.latency = cpu_to_le16(0x0000);
1061         evt.supv_timeout = cpu_to_le16(0x0000);
1062         evt.clock_accuracy = 0x00;
1063
1064         if (hci->le_event_mask[0] & 0x01)
1065                 le_meta_event(hci, BT_HCI_EVT_LE_CONN_COMPLETE,
1066                                                         &evt, sizeof(evt));
1067 }
1068
1069 static void cmd_le_read_white_list_size(struct bt_le *hci,
1070                                                 const void *data, uint8_t size)
1071 {
1072         struct bt_hci_rsp_le_read_white_list_size rsp;
1073
1074         rsp.status = BT_HCI_ERR_SUCCESS;
1075         rsp.size = hci->le_white_list_size;
1076
1077         cmd_complete(hci, BT_HCI_CMD_LE_READ_WHITE_LIST_SIZE,
1078                                                         &rsp, sizeof(rsp));
1079 }
1080
1081 static void cmd_le_clear_white_list(struct bt_le *hci,
1082                                                 const void *data, uint8_t size)
1083 {
1084         uint8_t status;
1085
1086         clear_white_list(hci);
1087
1088         status = BT_HCI_ERR_SUCCESS;
1089         cmd_complete(hci, BT_HCI_CMD_LE_CLEAR_WHITE_LIST,
1090                                                 &status, sizeof(status));
1091 }
1092
1093 static void cmd_le_add_to_white_list(struct bt_le *hci,
1094                                                 const void *data, uint8_t size)
1095 {
1096         const struct bt_hci_cmd_le_add_to_white_list *cmd = data;
1097         uint8_t status;
1098         bool exists = false;
1099         int i, pos = -1;
1100
1101         /* Valid range for address type is 0x00 to 0x01 */
1102         if (cmd->addr_type > 0x01) {
1103                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1104                                         BT_HCI_CMD_LE_ADD_TO_WHITE_LIST);
1105                 return;
1106         }
1107
1108         for (i = 0; i < hci->le_white_list_size; i++) {
1109                 if (hci->le_white_list[i][0] == cmd->addr_type &&
1110                                 !memcmp(&hci->le_white_list[i][1],
1111                                                         cmd->addr, 6)) {
1112                         exists = true;
1113                         break;
1114                 } else if (pos < 0 && hci->le_white_list[i][0] == 0xff)
1115                         pos = i;
1116         }
1117
1118         if (exists) {
1119                 cmd_status(hci, BT_HCI_ERR_UNSPECIFIED_ERROR,
1120                                         BT_HCI_CMD_LE_ADD_TO_WHITE_LIST);
1121                 return;
1122         }
1123
1124         if (pos < 0) {
1125                 cmd_status(hci, BT_HCI_ERR_MEM_CAPACITY_EXCEEDED,
1126                                         BT_HCI_CMD_LE_ADD_TO_WHITE_LIST);
1127                 return;
1128         }
1129
1130         hci->le_white_list[pos][0] = cmd->addr_type;
1131         memcpy(&hci->le_white_list[pos][1], cmd->addr, 6);
1132
1133         status = BT_HCI_ERR_SUCCESS;
1134         cmd_complete(hci, BT_HCI_CMD_LE_ADD_TO_WHITE_LIST,
1135                                                 &status, sizeof(status));
1136 }
1137
1138 static void cmd_le_remove_from_white_list(struct bt_le *hci,
1139                                                 const void *data, uint8_t size)
1140 {
1141         const struct bt_hci_cmd_le_remove_from_white_list *cmd = data;
1142         uint8_t status;
1143         int i, pos = -1;
1144
1145         /* Valid range for address type is 0x00 to 0x01 */
1146         if (cmd->addr_type > 0x01) {
1147                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1148                                         BT_HCI_CMD_LE_REMOVE_FROM_WHITE_LIST);
1149                 return;
1150         }
1151
1152         for (i = 0; i < hci->le_white_list_size; i++) {
1153                 if (hci->le_white_list[i][0] == cmd->addr_type &&
1154                                 !memcmp(&hci->le_white_list[i][1],
1155                                                         cmd->addr, 6)) {
1156                         pos = i;
1157                         break;
1158                 }
1159         }
1160
1161         if (pos < 0) {
1162                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1163                                         BT_HCI_CMD_LE_REMOVE_FROM_WHITE_LIST);
1164                 return;
1165         }
1166
1167         hci->le_white_list[pos][0] = 0xff;
1168         memset(&hci->le_white_list[pos][1], 0, 6);
1169
1170         status = BT_HCI_ERR_SUCCESS;
1171         cmd_complete(hci, BT_HCI_CMD_LE_REMOVE_FROM_WHITE_LIST,
1172                                                 &status, sizeof(status));
1173 }
1174
1175 static void cmd_le_encrypt(struct bt_le *hci, const void *data, uint8_t size)
1176 {
1177         const struct bt_hci_cmd_le_encrypt *cmd = data;
1178         struct bt_hci_rsp_le_encrypt rsp;
1179
1180         if (!bt_crypto_e(hci->crypto, cmd->key, cmd->plaintext, rsp.data)) {
1181                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1182                                         BT_HCI_CMD_LE_ENCRYPT);
1183                 return;
1184         }
1185
1186         rsp.status = BT_HCI_ERR_SUCCESS;
1187
1188         cmd_complete(hci, BT_HCI_CMD_LE_ENCRYPT, &rsp, sizeof(rsp));
1189 }
1190
1191 static void cmd_le_rand(struct bt_le *hci, const void *data, uint8_t size)
1192 {
1193         struct bt_hci_rsp_le_rand rsp;
1194         uint8_t value[8];
1195
1196         if (!bt_crypto_random_bytes(hci->crypto, value, 8)) {
1197                 cmd_status(hci, BT_HCI_ERR_COMMAND_DISALLOWED,
1198                                         BT_HCI_CMD_LE_RAND);
1199                 return;
1200         }
1201
1202         rsp.status = BT_HCI_ERR_SUCCESS;
1203         memcpy(&rsp.number, value, 8);
1204
1205         cmd_complete(hci, BT_HCI_CMD_LE_RAND, &rsp, sizeof(rsp));
1206 }
1207
1208 static void cmd_le_read_supported_states(struct bt_le *hci,
1209                                                 const void *data, uint8_t size)
1210 {
1211         struct bt_hci_rsp_le_read_supported_states rsp;
1212
1213         rsp.status = BT_HCI_ERR_SUCCESS;
1214         memcpy(rsp.states, hci->le_states, 8);
1215
1216         cmd_complete(hci, BT_HCI_CMD_LE_READ_SUPPORTED_STATES,
1217                                                         &rsp, sizeof(rsp));
1218 }
1219
1220 static void cmd_le_set_data_length(struct bt_le *hci,
1221                                                 const void *data, uint8_t size)
1222 {
1223         const struct bt_hci_cmd_le_set_data_length *cmd = data;
1224         struct bt_hci_rsp_le_set_data_length rsp;
1225         uint16_t handle, tx_len, tx_time;
1226
1227         handle = le16_to_cpu(cmd->handle);
1228         tx_len = le16_to_cpu(cmd->tx_len);
1229         tx_time = le16_to_cpu(cmd->tx_time);
1230
1231         /* Valid range for connection handle is 0x0000 to 0x0eff */
1232         if (handle > 0x0eff) {
1233                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1234                                         BT_HCI_CMD_LE_SET_DATA_LENGTH);
1235                 return;
1236         }
1237
1238         /* Valid range for suggested max TX octets is 0x001b to 0x00fb */
1239         if (tx_len < 0x001b || tx_len > 0x00fb) {
1240                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1241                                         BT_HCI_CMD_LE_SET_DATA_LENGTH);
1242                 return;
1243         }
1244
1245         /* Valid range for suggested max TX time is 0x0148 to 0x0848 */
1246         if (tx_time < 0x0148 || tx_time > 0x0848) {
1247                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1248                                         BT_HCI_CMD_LE_SET_DATA_LENGTH);
1249                 return;
1250         }
1251
1252         /* Max TX len and time shall be less or equal supported */
1253         if (tx_len > MAX_TX_LEN || tx_time > MAX_TX_TIME) {
1254                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1255                                         BT_HCI_CMD_LE_SET_DATA_LENGTH);
1256                 return;
1257         }
1258
1259         rsp.status = BT_HCI_ERR_SUCCESS;
1260         rsp.handle = cpu_to_le16(handle);
1261
1262         cmd_complete(hci, BT_HCI_CMD_LE_SET_DATA_LENGTH, &rsp, sizeof(rsp));
1263 }
1264
1265 static void cmd_le_read_default_data_length(struct bt_le *hci,
1266                                                 const void *data, uint8_t size)
1267 {
1268         struct bt_hci_rsp_le_read_default_data_length rsp;
1269
1270         rsp.status = BT_HCI_ERR_SUCCESS;
1271         rsp.tx_len = cpu_to_le16(hci->le_default_tx_len);
1272         rsp.tx_time = cpu_to_le16(hci->le_default_tx_time);
1273
1274         cmd_complete(hci, BT_HCI_CMD_LE_READ_DEFAULT_DATA_LENGTH,
1275                                                         &rsp, sizeof(rsp));
1276 }
1277
1278 static void cmd_le_write_default_data_length(struct bt_le *hci,
1279                                                 const void *data, uint8_t size)
1280 {
1281         const struct bt_hci_cmd_le_write_default_data_length *cmd = data;
1282         uint16_t tx_len, tx_time;
1283         uint8_t status;
1284
1285         tx_len = le16_to_cpu(cmd->tx_len);
1286         tx_time = le16_to_cpu(cmd->tx_time);
1287
1288         /* Valid range for suggested max TX octets is 0x001b to 0x00fb */
1289         if (tx_len < 0x001b || tx_len > 0x00fb) {
1290                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1291                                 BT_HCI_CMD_LE_WRITE_DEFAULT_DATA_LENGTH);
1292                 return;
1293         }
1294
1295         /* Valid range for suggested max TX time is 0x0148 to 0x0848 */
1296         if (tx_time < 0x0148 || tx_time > 0x0848) {
1297                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1298                                 BT_HCI_CMD_LE_WRITE_DEFAULT_DATA_LENGTH);
1299                 return;
1300         }
1301
1302         /* Suggested max TX len and time shall be less or equal supported */
1303         if (tx_len > MAX_TX_LEN || tx_time > MAX_TX_TIME) {
1304                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1305                                 BT_HCI_CMD_LE_WRITE_DEFAULT_DATA_LENGTH);
1306                 return;
1307         }
1308
1309         hci->le_default_tx_len = tx_len;
1310         hci->le_default_tx_time = tx_time;
1311
1312         status = BT_HCI_ERR_SUCCESS;
1313         cmd_complete(hci, BT_HCI_CMD_LE_WRITE_DEFAULT_DATA_LENGTH,
1314                                                 &status, sizeof(status));
1315 }
1316
1317 static void cmd_le_read_local_pk256(struct bt_le *hci,
1318                                                 const void *data, uint8_t size)
1319 {
1320         struct bt_hci_evt_le_read_local_pk256_complete evt;
1321
1322         cmd_status(hci, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_READ_LOCAL_PK256);
1323
1324         evt.status = BT_HCI_ERR_SUCCESS;
1325         ecc_make_key(evt.local_pk256, hci->le_local_sk256);
1326
1327         if (hci->le_event_mask[0] & 0x80)
1328                 le_meta_event(hci, BT_HCI_EVT_LE_READ_LOCAL_PK256_COMPLETE,
1329                                                         &evt, sizeof(evt));
1330 }
1331
1332 static void cmd_le_generate_dhkey(struct bt_le *hci,
1333                                                 const void *data, uint8_t size)
1334 {
1335         const struct bt_hci_cmd_le_generate_dhkey *cmd = data;
1336         struct bt_hci_evt_le_generate_dhkey_complete evt;
1337
1338         cmd_status(hci, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_GENERATE_DHKEY);
1339
1340         evt.status = BT_HCI_ERR_SUCCESS;
1341         ecdh_shared_secret(cmd->remote_pk256, hci->le_local_sk256, evt.dhkey);
1342
1343         if (hci->le_event_mask[1] & 0x01)
1344                 le_meta_event(hci, BT_HCI_EVT_LE_GENERATE_DHKEY_COMPLETE,
1345                                                         &evt, sizeof(evt));
1346 }
1347
1348 static void cmd_le_add_to_resolv_list(struct bt_le *hci,
1349                                                 const void *data, uint8_t size)
1350 {
1351         const struct bt_hci_cmd_le_add_to_resolv_list *cmd = data;
1352         uint8_t status;
1353         bool exists = false;
1354         int i, pos = -1;
1355
1356         /* Valid range for address type is 0x00 to 0x01 */
1357         if (cmd->addr_type > 0x01) {
1358                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1359                                         BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST);
1360                 return;
1361         }
1362
1363         for (i = 0; i < hci->le_resolv_list_size; i++) {
1364                 if (hci->le_resolv_list[i][0] == cmd->addr_type &&
1365                                 !memcmp(&hci->le_resolv_list[i][1],
1366                                                         cmd->addr, 6)) {
1367                         exists = true;
1368                         break;
1369                 } else if (pos < 0 && hci->le_resolv_list[i][0] == 0xff)
1370                         pos = i;
1371         }
1372
1373         if (exists) {
1374                 cmd_status(hci, BT_HCI_ERR_UNSPECIFIED_ERROR,
1375                                         BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST);
1376                 return;
1377         }
1378
1379         if (pos < 0) {
1380                 cmd_status(hci, BT_HCI_ERR_MEM_CAPACITY_EXCEEDED,
1381                                         BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST);
1382                 return;
1383         }
1384
1385         hci->le_resolv_list[pos][0] = cmd->addr_type;
1386         memcpy(&hci->le_resolv_list[pos][1], cmd->addr, 6);
1387         memcpy(&hci->le_resolv_list[pos][7], cmd->peer_irk, 16);
1388         memcpy(&hci->le_resolv_list[pos][23], cmd->local_irk, 16);
1389
1390         status = BT_HCI_ERR_SUCCESS;
1391         cmd_complete(hci, BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST,
1392                                                 &status, sizeof(status));
1393 }
1394
1395 static void cmd_le_remove_from_resolv_list(struct bt_le *hci,
1396                                                 const void *data, uint8_t size)
1397 {
1398         const struct bt_hci_cmd_le_remove_from_resolv_list *cmd = data;
1399         uint8_t status;
1400         int i, pos = -1;
1401
1402         /* Valid range for address type is 0x00 to 0x01 */
1403         if (cmd->addr_type > 0x01) {
1404                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1405                                         BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST);
1406                 return;
1407         }
1408
1409         for (i = 0; i < hci->le_resolv_list_size; i++) {
1410                 if (hci->le_resolv_list[i][0] == cmd->addr_type &&
1411                                 !memcmp(&hci->le_resolv_list[i][1],
1412                                                         cmd->addr, 6)) {
1413                         pos = i;
1414                         break;
1415                 }
1416         }
1417
1418         if (pos < 0) {
1419                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1420                                         BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST);
1421                 return;
1422         }
1423
1424         hci->le_resolv_list[pos][0] = 0xff;
1425         memset(&hci->le_resolv_list[pos][1], 0, 38);
1426
1427         status = BT_HCI_ERR_SUCCESS;
1428         cmd_complete(hci, BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST,
1429                                                 &status, sizeof(status));
1430 }
1431
1432 static void cmd_le_clear_resolv_list(struct bt_le *hci,
1433                                                 const void *data, uint8_t size)
1434 {
1435         uint8_t status;
1436
1437         clear_resolv_list(hci);
1438
1439         status = BT_HCI_ERR_SUCCESS;
1440         cmd_complete(hci, BT_HCI_CMD_LE_CLEAR_RESOLV_LIST,
1441                                                 &status, sizeof(status));
1442 }
1443
1444 static void cmd_le_read_resolv_list_size(struct bt_le *hci,
1445                                                 const void *data, uint8_t size)
1446 {
1447         struct bt_hci_rsp_le_read_resolv_list_size rsp;
1448
1449         rsp.status = BT_HCI_ERR_SUCCESS;
1450         rsp.size = hci->le_resolv_list_size;
1451
1452         cmd_complete(hci, BT_HCI_CMD_LE_READ_RESOLV_LIST_SIZE,
1453                                                         &rsp, sizeof(rsp));
1454 }
1455
1456 static void cmd_le_read_peer_resolv_addr(struct bt_le *hci,
1457                                                 const void *data, uint8_t size)
1458 {
1459         const struct bt_hci_cmd_le_read_peer_resolv_addr *cmd = data;
1460         struct bt_hci_rsp_le_read_peer_resolv_addr rsp;
1461
1462         /* Valid range for address type is 0x00 to 0x01 */
1463         if (cmd->addr_type > 0x01) {
1464                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1465                                         BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR);
1466                 return;
1467         }
1468
1469         rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1470         memset(rsp.addr, 0, 6);
1471
1472         cmd_complete(hci, BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR,
1473                                                         &rsp, sizeof(rsp));
1474 }
1475
1476 static void cmd_le_read_local_resolv_addr(struct bt_le *hci,
1477                                                 const void *data, uint8_t size)
1478 {
1479         const struct bt_hci_cmd_le_read_local_resolv_addr *cmd = data;
1480         struct bt_hci_rsp_le_read_local_resolv_addr rsp;
1481
1482         /* Valid range for address type is 0x00 to 0x01 */
1483         if (cmd->addr_type > 0x01) {
1484                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1485                                         BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR);
1486                 return;
1487         }
1488
1489         rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
1490         memset(rsp.addr, 0, 6);
1491
1492         cmd_complete(hci, BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR,
1493                                                         &rsp, sizeof(rsp));
1494 }
1495
1496 static void cmd_le_set_resolv_enable(struct bt_le *hci,
1497                                                 const void *data, uint8_t size)
1498 {
1499         const struct bt_hci_cmd_le_set_resolv_enable *cmd = data;
1500         uint8_t status;
1501
1502         /* Valid range for address resolution enable is 0x00 to 0x01 */
1503         if (cmd->enable > 0x01) {
1504                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1505                                         BT_HCI_CMD_LE_SET_RESOLV_ENABLE);
1506                 return;
1507         }
1508
1509         hci->le_resolv_enable = cmd->enable;
1510
1511         status = BT_HCI_ERR_SUCCESS;
1512         cmd_complete(hci, BT_HCI_CMD_LE_SET_RESOLV_ENABLE,
1513                                                 &status, sizeof(status));
1514 }
1515
1516 static void cmd_le_set_resolv_timeout(struct bt_le *hci,
1517                                                 const void *data, uint8_t size)
1518 {
1519         const struct bt_hci_cmd_le_set_resolv_timeout *cmd = data;
1520         uint16_t timeout;
1521         uint8_t status;
1522
1523         timeout = le16_to_cpu(cmd->timeout);
1524
1525         /* Valid range for RPA timeout is 0x0001 to 0xa1b8 */
1526         if (timeout < 0x0001 || timeout > 0xa1b8) {
1527                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
1528                                         BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT);
1529                 return;
1530         }
1531
1532         hci->le_resolv_timeout = timeout;
1533
1534         status = BT_HCI_ERR_SUCCESS;
1535         cmd_complete(hci, BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT,
1536                                                 &status, sizeof(status));
1537 }
1538
1539 static void cmd_le_read_max_data_length(struct bt_le *hci,
1540                                                 const void *data, uint8_t size)
1541 {
1542         struct bt_hci_rsp_le_read_max_data_length rsp;
1543
1544         rsp.status = BT_HCI_ERR_SUCCESS;
1545         rsp.max_tx_len = cpu_to_le16(MAX_TX_LEN);
1546         rsp.max_tx_time = cpu_to_le16(MAX_TX_TIME);
1547         rsp.max_rx_len = cpu_to_le16(MAX_RX_LEN);
1548         rsp.max_rx_time = cpu_to_le16(MAX_RX_TIME);
1549
1550         cmd_complete(hci, BT_HCI_CMD_LE_READ_MAX_DATA_LENGTH,
1551                                                         &rsp, sizeof(rsp));
1552 }
1553
1554 static const struct {
1555         uint16_t opcode;
1556         void (*func) (struct bt_le *hci, const void *data, uint8_t size);
1557         uint8_t size;
1558         bool fixed;
1559 } cmd_table[] = {
1560         { BT_HCI_CMD_DISCONNECT,           cmd_disconnect,           3, true },
1561
1562         { BT_HCI_CMD_SET_EVENT_MASK,       cmd_set_event_mask,       8, true },
1563         { BT_HCI_CMD_RESET,                cmd_reset,                0, true },
1564         { BT_HCI_CMD_SET_EVENT_MASK_PAGE2, cmd_set_event_mask_page2, 8, true },
1565
1566         { BT_HCI_CMD_READ_LOCAL_VERSION,   cmd_read_local_version,   0, true },
1567         { BT_HCI_CMD_READ_LOCAL_COMMANDS,  cmd_read_local_commands,  0, true },
1568         { BT_HCI_CMD_READ_LOCAL_FEATURES,  cmd_read_local_features,  0, true },
1569         { BT_HCI_CMD_READ_BUFFER_SIZE,     cmd_read_buffer_size,     0, true },
1570         { BT_HCI_CMD_READ_BD_ADDR,         cmd_read_bd_addr,         0, true },
1571
1572         { BT_HCI_CMD_LE_SET_EVENT_MASK,
1573                                 cmd_le_set_event_mask, 8, true },
1574         { BT_HCI_CMD_LE_READ_BUFFER_SIZE,
1575                                 cmd_le_read_buffer_size, 0, true },
1576         { BT_HCI_CMD_LE_READ_LOCAL_FEATURES,
1577                                 cmd_le_read_local_features, 0, true },
1578         { BT_HCI_CMD_LE_SET_RANDOM_ADDRESS,
1579                                 cmd_le_set_random_address, 6, true },
1580         { BT_HCI_CMD_LE_SET_ADV_PARAMETERS,
1581                                 cmd_le_set_adv_parameters, 15, true },
1582         { BT_HCI_CMD_LE_READ_ADV_TX_POWER,
1583                                 cmd_le_read_adv_tx_power, 0, true },
1584         { BT_HCI_CMD_LE_SET_ADV_DATA,
1585                                 cmd_le_set_adv_data, 32, true },
1586         { BT_HCI_CMD_LE_SET_SCAN_RSP_DATA,
1587                                 cmd_le_set_scan_rsp_data, 32, true },
1588         { BT_HCI_CMD_LE_SET_ADV_ENABLE,
1589                                 cmd_le_set_adv_enable, 1, true },
1590         { BT_HCI_CMD_LE_SET_SCAN_PARAMETERS,
1591                                 cmd_le_set_scan_parameters, 7, true },
1592         { BT_HCI_CMD_LE_SET_SCAN_ENABLE,
1593                                 cmd_le_set_scan_enable, 2, true },
1594         { BT_HCI_CMD_LE_CREATE_CONN,
1595                                 cmd_le_create_conn, 25, true },
1596         { BT_HCI_CMD_LE_CREATE_CONN_CANCEL,
1597                                 cmd_le_create_conn_cancel, 0, true },
1598         { BT_HCI_CMD_LE_READ_WHITE_LIST_SIZE,
1599                                 cmd_le_read_white_list_size, 0, true },
1600         { BT_HCI_CMD_LE_CLEAR_WHITE_LIST,
1601                                 cmd_le_clear_white_list, 0, true },
1602         { BT_HCI_CMD_LE_ADD_TO_WHITE_LIST,
1603                                 cmd_le_add_to_white_list,  7, true },
1604         { BT_HCI_CMD_LE_REMOVE_FROM_WHITE_LIST,
1605                                 cmd_le_remove_from_white_list, 7, true },
1606
1607         { BT_HCI_CMD_LE_ENCRYPT, cmd_le_encrypt, 32, true },
1608         { BT_HCI_CMD_LE_RAND, cmd_le_rand, 0, true },
1609
1610         { BT_HCI_CMD_LE_READ_SUPPORTED_STATES,
1611                                 cmd_le_read_supported_states, 0, true },
1612
1613         { BT_HCI_CMD_LE_SET_DATA_LENGTH,
1614                                 cmd_le_set_data_length, 6, true },
1615         { BT_HCI_CMD_LE_READ_DEFAULT_DATA_LENGTH,
1616                                 cmd_le_read_default_data_length, 0, true },
1617         { BT_HCI_CMD_LE_WRITE_DEFAULT_DATA_LENGTH,
1618                                 cmd_le_write_default_data_length, 4, true },
1619         { BT_HCI_CMD_LE_READ_LOCAL_PK256,
1620                                 cmd_le_read_local_pk256, 0, true },
1621         { BT_HCI_CMD_LE_GENERATE_DHKEY,
1622                                 cmd_le_generate_dhkey, 64, true },
1623         { BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST,
1624                                 cmd_le_add_to_resolv_list,  39, true },
1625         { BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST,
1626                                 cmd_le_remove_from_resolv_list, 7, true },
1627         { BT_HCI_CMD_LE_CLEAR_RESOLV_LIST,
1628                                 cmd_le_clear_resolv_list, 0, true },
1629         { BT_HCI_CMD_LE_READ_RESOLV_LIST_SIZE,
1630                                 cmd_le_read_resolv_list_size, 0, true },
1631         { BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR,
1632                                 cmd_le_read_peer_resolv_addr, 7, true },
1633         { BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR,
1634                                 cmd_le_read_local_resolv_addr, 7, true },
1635         { BT_HCI_CMD_LE_SET_RESOLV_ENABLE,
1636                                 cmd_le_set_resolv_enable, 1, true },
1637         { BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT,
1638                                 cmd_le_set_resolv_timeout, 2, true },
1639         { BT_HCI_CMD_LE_READ_MAX_DATA_LENGTH,
1640                                 cmd_le_read_max_data_length, 0, true },
1641
1642         { }
1643 };
1644
1645 static void process_command(struct bt_le *hci, const void *data, size_t size)
1646 {
1647         const struct bt_hci_cmd_hdr *hdr = data;
1648         uint16_t opcode;
1649         unsigned int i;
1650
1651         if (size < sizeof(*hdr))
1652                 return;
1653
1654         data += sizeof(*hdr);
1655         size -= sizeof(*hdr);
1656
1657         opcode = le16_to_cpu(hdr->opcode);
1658
1659         if (hdr->plen != size) {
1660                 cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS, opcode);
1661                 return;
1662         }
1663
1664         for (i = 0; cmd_table[i].func; i++) {
1665                 if (cmd_table[i].opcode != opcode)
1666                         continue;
1667
1668                 if ((cmd_table[i].fixed && size != cmd_table[i].size) ||
1669                                                 size < cmd_table[i].size) {
1670                         cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS, opcode);
1671                         return;
1672                 }
1673
1674                 cmd_table[i].func(hci, data, size);
1675                 return;
1676         }
1677
1678         cmd_status(hci, BT_HCI_ERR_UNKNOWN_COMMAND, opcode);
1679 }
1680
1681 static void vhci_read_callback(int fd, uint32_t events, void *user_data)
1682 {
1683         struct bt_le *hci = user_data;
1684         unsigned char buf[4096];
1685         ssize_t len;
1686
1687         if (events & (EPOLLERR | EPOLLHUP))
1688                 return;
1689
1690         len = read(hci->vhci_fd, buf, sizeof(buf));
1691         if (len < 1)
1692                 return;
1693
1694         switch (buf[0]) {
1695         case BT_H4_CMD_PKT:
1696                 process_command(hci, buf + 1, len - 1);
1697                 break;
1698         }
1699 }
1700
1701 static void phy_recv_callback(uint16_t type, const void *data,
1702                                                 size_t size, void *user_data)
1703 {
1704         struct bt_le *hci = user_data;
1705
1706         switch (type) {
1707         case BT_PHY_PKT_ADV:
1708                 if (!(hci->le_event_mask[0] & 0x02))
1709                         return;
1710
1711                 if (hci->le_scan_enable == 0x01) {
1712                         const struct bt_phy_pkt_adv *pkt = data;
1713                         uint8_t buf[100];
1714                         struct bt_hci_evt_le_adv_report *evt = (void *) buf;
1715                         uint8_t tx_addr_type, tx_addr[6];
1716
1717                         resolve_peer_addr(hci, pkt->tx_addr_type, pkt->tx_addr,
1718                                                         &tx_addr_type, tx_addr);
1719
1720                         if (hci->le_scan_filter_policy == 0x01 ||
1721                                         hci->le_scan_filter_policy == 0x03) {
1722                                 if (!is_in_white_list(hci, tx_addr_type,
1723                                                                 tx_addr))
1724                                         break;
1725                         }
1726
1727                         if (hci->le_scan_filter_dup) {
1728                                 if (!add_to_scan_cache(hci, tx_addr_type,
1729                                                                 tx_addr))
1730                                         break;
1731                         }
1732
1733                         memset(buf, 0, sizeof(buf));
1734                         evt->num_reports = 0x01;
1735                         evt->event_type = pkt->pdu_type;
1736                         evt->addr_type = tx_addr_type;
1737                         memcpy(evt->addr, tx_addr, 6);
1738                         evt->data_len = pkt->adv_data_len;
1739                         memcpy(buf + sizeof(*evt), data + sizeof(*pkt),
1740                                                         pkt->adv_data_len);
1741
1742                         le_meta_event(hci, BT_HCI_EVT_LE_ADV_REPORT, buf,
1743                                         sizeof(*evt) + pkt->adv_data_len + 1);
1744
1745                         if (hci->le_scan_type == 0x00)
1746                                 break;
1747
1748                         memset(buf, 0, sizeof(buf));
1749                         evt->num_reports = 0x01;
1750                         evt->event_type = 0x04;
1751                         evt->addr_type = tx_addr_type;
1752                         memcpy(evt->addr, tx_addr, 6);
1753                         evt->data_len = pkt->scan_rsp_len;
1754                         memcpy(buf + sizeof(*evt), data + sizeof(*pkt) +
1755                                                         pkt->adv_data_len,
1756                                                         pkt->scan_rsp_len);
1757
1758                         le_meta_event(hci, BT_HCI_EVT_LE_ADV_REPORT, buf,
1759                                         sizeof(*evt) + pkt->scan_rsp_len + 1);
1760                 }
1761                 break;
1762         }
1763 }
1764
1765 struct bt_le *bt_le_new(void)
1766 {
1767         unsigned char setup_cmd[2];
1768         struct bt_le *hci;
1769
1770         hci = calloc(1, sizeof(*hci));
1771         if (!hci)
1772                 return NULL;
1773
1774         hci->adv_timeout_id = -1;
1775
1776         reset_defaults(hci);
1777
1778         hci->vhci_fd = open("/dev/vhci", O_RDWR);
1779         if (hci->vhci_fd < 0) {
1780                 free(hci);
1781                 return NULL;
1782         }
1783
1784         setup_cmd[0] = HCI_VENDOR_PKT;
1785         setup_cmd[1] = HCI_BREDR;
1786
1787         if (write(hci->vhci_fd, setup_cmd, sizeof(setup_cmd)) < 0) {
1788                 close(hci->vhci_fd);
1789                 free(hci);
1790                 return NULL;
1791         }
1792
1793         mainloop_add_fd(hci->vhci_fd, EPOLLIN, vhci_read_callback, hci, NULL);
1794
1795         hci->phy = bt_phy_new();
1796         hci->crypto = bt_crypto_new();
1797
1798         bt_phy_register(hci->phy, phy_recv_callback, hci);
1799
1800         return bt_le_ref(hci);
1801 }
1802
1803 struct bt_le *bt_le_ref(struct bt_le *hci)
1804 {
1805         if (!hci)
1806                 return NULL;
1807
1808         __sync_fetch_and_add(&hci->ref_count, 1);
1809
1810         return hci;
1811 }
1812
1813 void bt_le_unref(struct bt_le *hci)
1814 {
1815         if (!hci)
1816                 return;
1817
1818         if (__sync_sub_and_fetch(&hci->ref_count, 1))
1819                 return;
1820
1821         stop_adv(hci);
1822
1823         bt_crypto_unref(hci->crypto);
1824         bt_phy_unref(hci->phy);
1825
1826         mainloop_remove_fd(hci->vhci_fd);
1827
1828         close(hci->vhci_fd);
1829
1830         free(hci);
1831 }