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