emulator/bthost.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 <stdio.h>
  30 #include <ctype.h>
  31 #include <stdlib.h>
  32 #include <string.h>
  33 #include <endian.h>
  34 #include <stdbool.h>
  35
  36 #include "lib/bluetooth.h"
  37
  38 #include "src/shared/util.h"
  39 #include "monitor/bt.h"
  40 #include "monitor/rfcomm.h"
  41 #include "bthost.h"
  42
  43 #define lmp_bredr_capable(bthost)     (!((bthost)->features[4] & 0x20))
  44
  45 /* ACL handle and flags pack/unpack */
  46 #define acl_handle_pack(h, f)   (uint16_t)((h & 0x0fff)|(f << 12))
  47 #define acl_handle(h)           (h & 0x0fff)
  48 #define acl_flags(h)            (h >> 12)
  49
  50 #define L2CAP_FEAT_FIXED_CHAN   0x00000080
  51 #define L2CAP_FC_SIG_BREDR      0x02
  52 #define L2CAP_FC_SMP_BREDR      0x80
  53 #define L2CAP_IT_FEAT_MASK      0x0002
  54 #define L2CAP_IT_FIXED_CHAN     0x0003
  55
  56 /* RFCOMM setters */
  57 #define RFCOMM_ADDR(cr, dlci)   (((dlci & 0x3f) << 2) | (cr << 1) | 0x01)
  58 #define RFCOMM_CTRL(type, pf)   (((type & 0xef) | (pf << 4)))
  59 #define RFCOMM_LEN8(len)        (((len) << 1) | 1)
  60 #define RFCOMM_LEN16(len)       ((len) << 1)
  61 #define RFCOMM_MCC_TYPE(cr, type)       (((type << 2) | (cr << 1) | 0x01))
  62
  63 /* RFCOMM FCS calculation */
  64 #define CRC(data) (rfcomm_crc_table[rfcomm_crc_table[0xff ^ data[0]] ^ data[1]])
  65
  66 static unsigned char rfcomm_crc_table[256] = {
  67         0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75,
  68         0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b,
  69         0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69,
  70         0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67,
  71
  72         0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d,
  73         0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43,
  74         0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51,
  75         0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f,
  76
  77         0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05,
  78         0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b,
  79         0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19,
  80         0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17,
  81
  82         0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d,
  83         0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33,
  84         0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21,
  85         0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f,
  86
  87         0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95,
  88         0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b,
  89         0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89,
  90         0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87,
  91
  92         0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad,
  93         0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3,
  94         0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1,
  95         0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf,
  96
  97         0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5,
  98         0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb,
  99         0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9,
 100         0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7,
 101
 102         0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd,
 103         0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3,
 104         0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1,
 105         0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf
 106 };
 107
 108 static uint8_t rfcomm_fcs2(uint8_t *data)
 109 {
 110         return 0xff - rfcomm_crc_table[CRC(data) ^ data[2]];
 111 }
 112
 113 static uint8_t rfcomm_fcs(uint8_t *data)
 114 {
 115         return 0xff - CRC(data);
 116 }
 117
 118 struct cmd {
 119         struct cmd *next;
 120         struct cmd *prev;
 121         uint8_t data[256 + sizeof(struct bt_hci_cmd_hdr)];
 122         uint16_t len;
 123 };
 124
 125 struct cmd_queue {
 126         struct cmd *head;
 127         struct cmd *tail;
 128 };
 129
 130 struct cid_hook {
 131         uint16_t cid;
 132         bthost_cid_hook_func_t func;
 133         void *user_data;
 134         struct cid_hook *next;
 135 };
 136
 137 struct rfcomm_chan_hook {
 138         uint8_t channel;
 139         bthost_rfcomm_chan_hook_func_t func;
 140         void *user_data;
 141         struct rfcomm_chan_hook *next;
 142 };
 143
 144 struct btconn {
 145         uint16_t handle;
 146         uint8_t bdaddr[6];
 147         uint8_t addr_type;
 148         uint8_t encr_mode;
 149         uint16_t next_cid;
 150         uint64_t fixed_chan;
 151         struct l2conn *l2conns;
 152         struct rcconn *rcconns;
 153         struct cid_hook *cid_hooks;
 154         struct rfcomm_chan_hook *rfcomm_chan_hooks;
 155         struct btconn *next;
 156         void *smp_data;
 157 };
 158
 159 struct l2conn {
 160         uint16_t scid;
 161         uint16_t dcid;
 162         uint16_t psm;
 163         struct l2conn *next;
 164 };
 165
 166 struct rcconn {
 167         uint8_t channel;
 168         uint16_t scid;
 169         struct rcconn *next;
 170 };
 171
 172 struct l2cap_pending_req {
 173         uint8_t ident;
 174         bthost_l2cap_rsp_cb cb;
 175         void *user_data;
 176         struct l2cap_pending_req *next;
 177 };
 178
 179 struct l2cap_conn_cb_data {
 180         uint16_t psm;
 181         bthost_l2cap_connect_cb func;
 182         void *user_data;
 183         struct l2cap_conn_cb_data *next;
 184 };
 185
 186 struct rfcomm_conn_cb_data {
 187         uint8_t channel;
 188         bthost_rfcomm_connect_cb func;
 189         void *user_data;
 190         struct rfcomm_conn_cb_data *next;
 191 };
 192
 193 struct rfcomm_connection_data {
 194         uint8_t channel;
 195         struct btconn *conn;
 196         bthost_rfcomm_connect_cb cb;
 197         void *user_data;
 198 };
 199
 200 struct bthost {
 201         bool ready;
 202         bthost_ready_cb ready_cb;
 203         uint8_t bdaddr[6];
 204         uint8_t features[8];
 205         bthost_send_func send_handler;
 206         void *send_data;
 207         struct cmd_queue cmd_q;
 208         uint8_t ncmd;
 209         struct btconn *conns;
 210         bthost_cmd_complete_cb cmd_complete_cb;
 211         void *cmd_complete_data;
 212         bthost_new_conn_cb new_conn_cb;
 213         void *new_conn_data;
 214         struct rfcomm_connection_data *rfcomm_conn_data;
 215         struct l2cap_conn_cb_data *new_l2cap_conn_data;
 216         struct rfcomm_conn_cb_data *new_rfcomm_conn_data;
 217         struct l2cap_pending_req *l2reqs;
 218         uint8_t pin[16];
 219         uint8_t pin_len;
 220         uint8_t io_capability;
 221         uint8_t auth_req;
 222         bool reject_user_confirm;
 223         void *smp_data;
 224         bool conn_init;
 225         bool le;
 226         bool sc;
 227 };
 228
 229 struct bthost *bthost_create(void)
 230 {
 231         struct bthost *bthost;
 232
 233         bthost = new0(struct bthost, 1);
 234         if (!bthost)
 235                 return NULL;
 236
 237         bthost->smp_data = smp_start(bthost);
 238         if (!bthost->smp_data) {
 239                 free(bthost);
 240                 return NULL;
 241         }
 242
 243         /* Set defaults */
 244         bthost->io_capability = 0x03;
 245
 246         return bthost;
 247 }
 248
 249 static void l2conn_free(struct l2conn *conn)
 250 {
 251         free(conn);
 252 }
 253
 254 static void btconn_free(struct btconn *conn)
 255 {
 256         if (conn->smp_data)
 257                 smp_conn_del(conn->smp_data);
 258
 259         while (conn->l2conns) {
 260                 struct l2conn *l2conn = conn->l2conns;
 261
 262                 conn->l2conns = l2conn->next;
 263                 l2conn_free(l2conn);
 264         }
 265
 266         while (conn->cid_hooks) {
 267                 struct cid_hook *hook = conn->cid_hooks;
 268
 269                 conn->cid_hooks = hook->next;
 270                 free(hook);
 271         }
 272
 273         while (conn->rcconns) {
 274                 struct rcconn *rcconn = conn->rcconns;
 275
 276                 conn->rcconns = rcconn->next;
 277                 free(rcconn);
 278         }
 279
 280         while (conn->rfcomm_chan_hooks) {
 281                 struct rfcomm_chan_hook *hook = conn->rfcomm_chan_hooks;
 282
 283                 conn->rfcomm_chan_hooks = hook->next;
 284                 free(hook);
 285         }
 286
 287         free(conn);
 288 }
 289
 290 static struct btconn *bthost_find_conn(struct bthost *bthost, uint16_t handle)
 291 {
 292         struct btconn *conn;
 293
 294         for (conn = bthost->conns; conn != NULL; conn = conn->next) {
 295                 if (conn->handle == handle)
 296                         return conn;
 297         }
 298
 299         return NULL;
 300 }
 301
 302 static struct btconn *bthost_find_conn_by_bdaddr(struct bthost *bthost,
 303                                                         const uint8_t *bdaddr)
 304 {
 305         struct btconn *conn;
 306
 307         for (conn = bthost->conns; conn != NULL; conn = conn->next) {
 308                 if (!memcmp(conn->bdaddr, bdaddr, 6))
 309                         return conn;
 310         }
 311
 312         return NULL;
 313 }
 314
 315 static struct l2conn *bthost_add_l2cap_conn(struct bthost *bthost,
 316                                                 struct btconn *conn,
 317                                                 uint16_t scid, uint16_t dcid,
 318                                                 uint16_t psm)
 319 {
 320         struct l2conn *l2conn;
 321
 322         l2conn = malloc(sizeof(*l2conn));
 323         if (!l2conn)
 324                 return NULL;
 325
 326         memset(l2conn, 0, sizeof(*l2conn));
 327
 328         l2conn->psm = psm;
 329         l2conn->scid = scid;
 330         l2conn->dcid = dcid;
 331
 332         l2conn->next = conn->l2conns;
 333         conn->l2conns = l2conn;
 334
 335         return l2conn;
 336 }
 337
 338 static struct rcconn *bthost_add_rfcomm_conn(struct bthost *bthost,
 339                                                 struct btconn *conn,
 340                                                 struct l2conn *l2conn,
 341                                                 uint8_t channel)
 342 {
 343         struct rcconn *rcconn;
 344
 345         rcconn = malloc(sizeof(*rcconn));
 346         if (!rcconn)
 347                 return NULL;
 348
 349         memset(rcconn, 0, sizeof(*rcconn));
 350
 351         rcconn->channel = channel;
 352         rcconn->scid = l2conn->scid;
 353
 354         rcconn->next = conn->rcconns;
 355         conn->rcconns = rcconn;
 356
 357         return rcconn;
 358 }
 359
 360 static struct rcconn *btconn_find_rfcomm_conn_by_channel(struct btconn *conn,
 361                                                                 uint8_t chan)
 362 {
 363         struct rcconn *rcconn;
 364
 365         for (rcconn = conn->rcconns; rcconn != NULL; rcconn = rcconn->next) {
 366                 if (rcconn->channel == chan)
 367                         return rcconn;
 368         }
 369
 370         return NULL;
 371 }
 372
 373 static struct l2conn *btconn_find_l2cap_conn_by_scid(struct btconn *conn,
 374                                                                 uint16_t scid)
 375 {
 376         struct l2conn *l2conn;
 377
 378         for (l2conn = conn->l2conns; l2conn != NULL; l2conn = l2conn->next) {
 379                 if (l2conn->scid == scid)
 380                         return l2conn;
 381         }
 382
 383         return NULL;
 384 }
 385
 386 static struct l2cap_conn_cb_data *bthost_find_l2cap_cb_by_psm(
 387                                         struct bthost *bthost, uint16_t psm)
 388 {
 389         struct l2cap_conn_cb_data *cb;
 390
 391         for (cb = bthost->new_l2cap_conn_data; cb != NULL; cb = cb->next) {
 392                 if (cb->psm == psm)
 393                         return cb;
 394         }
 395
 396         return NULL;
 397 }
 398
 399 static struct rfcomm_conn_cb_data *bthost_find_rfcomm_cb_by_channel(
 400                                         struct bthost *bthost, uint8_t channel)
 401 {
 402         struct rfcomm_conn_cb_data *cb;
 403
 404         for (cb = bthost->new_rfcomm_conn_data; cb != NULL; cb = cb->next) {
 405                 if (cb->channel == channel)
 406                         return cb;
 407         }
 408
 409         return NULL;
 410 }
 411
 412 void bthost_destroy(struct bthost *bthost)
 413 {
 414         if (!bthost)
 415                 return;
 416
 417         while (bthost->cmd_q.tail) {
 418                 struct cmd *cmd = bthost->cmd_q.tail;
 419
 420                 bthost->cmd_q.tail = cmd->prev;
 421                 free(cmd);
 422         }
 423
 424         while (bthost->conns) {
 425                 struct btconn *conn = bthost->conns;
 426
 427                 bthost->conns = conn->next;
 428                 btconn_free(conn);
 429         }
 430
 431         while (bthost->l2reqs) {
 432                 struct l2cap_pending_req *req = bthost->l2reqs;
 433
 434                 bthost->l2reqs = req->next;
 435                 req->cb(0, NULL, 0, req->user_data);
 436                 free(req);
 437         }
 438
 439         while (bthost->new_l2cap_conn_data) {
 440                 struct l2cap_conn_cb_data *cb = bthost->new_l2cap_conn_data;
 441
 442                 bthost->new_l2cap_conn_data = cb->next;
 443                 free(cb);
 444         }
 445
 446         while (bthost->new_rfcomm_conn_data) {
 447                 struct rfcomm_conn_cb_data *cb = bthost->new_rfcomm_conn_data;
 448
 449                 bthost->new_rfcomm_conn_data = cb->next;
 450                 free(cb);
 451         }
 452
 453         if (bthost->rfcomm_conn_data)
 454                 free(bthost->rfcomm_conn_data);
 455
 456         smp_stop(bthost->smp_data);
 457
 458         free(bthost);
 459 }
 460
 461 void bthost_set_send_handler(struct bthost *bthost, bthost_send_func handler,
 462                                                         void *user_data)
 463 {
 464         if (!bthost)
 465                 return;
 466
 467         bthost->send_handler = handler;
 468         bthost->send_data = user_data;
 469 }
 470
 471 static void queue_command(struct bthost *bthost, const struct iovec *iov,
 472                                                                 int iovlen)
 473 {
 474         struct cmd_queue *cmd_q = &bthost->cmd_q;
 475         struct cmd *cmd;
 476         int i;
 477
 478         cmd = malloc(sizeof(*cmd));
 479         if (!cmd)
 480                 return;
 481
 482         memset(cmd, 0, sizeof(*cmd));
 483
 484         for (i = 0; i < iovlen; i++) {
 485                 memcpy(cmd->data + cmd->len, iov[i].iov_base, iov[i].iov_len);
 486                 cmd->len += iov[i].iov_len;
 487         }
 488
 489         if (cmd_q->tail)
 490                 cmd_q->tail->next = cmd;
 491         else
 492                 cmd_q->head = cmd;
 493
 494         cmd->prev = cmd_q->tail;
 495         cmd_q->tail = cmd;
 496 }
 497
 498 static void send_packet(struct bthost *bthost, const struct iovec *iov,
 499                                                                 int iovlen)
 500 {
 501         if (!bthost->send_handler)
 502                 return;
 503
 504         bthost->send_handler(iov, iovlen, bthost->send_data);
 505 }
 506
 507 static void send_iov(struct bthost *bthost, uint16_t handle, uint16_t cid,
 508                                         const struct iovec *iov, int iovcnt)
 509 {
 510         struct bt_hci_acl_hdr acl_hdr;
 511         struct bt_l2cap_hdr l2_hdr;
 512         uint8_t pkt = BT_H4_ACL_PKT;
 513         struct iovec pdu[3 + iovcnt];
 514         int i, len = 0;
 515
 516         for (i = 0; i < iovcnt; i++) {
 517                 pdu[3 + i].iov_base = iov[i].iov_base;
 518                 pdu[3 + i].iov_len = iov[i].iov_len;
 519                 len += iov[i].iov_len;
 520         }
 521
 522         pdu[0].iov_base = &pkt;
 523         pdu[0].iov_len = sizeof(pkt);
 524
 525         acl_hdr.handle = acl_handle_pack(handle, 0);
 526         acl_hdr.dlen = cpu_to_le16(len + sizeof(l2_hdr));
 527
 528         pdu[1].iov_base = &acl_hdr;
 529         pdu[1].iov_len = sizeof(acl_hdr);
 530
 531         l2_hdr.cid = cpu_to_le16(cid);
 532         l2_hdr.len = cpu_to_le16(len);
 533
 534         pdu[2].iov_base = &l2_hdr;
 535         pdu[2].iov_len = sizeof(l2_hdr);
 536
 537         send_packet(bthost, pdu, 3 + iovcnt);
 538 }
 539
 540 static void send_acl(struct bthost *bthost, uint16_t handle, uint16_t cid,
 541                                                 const void *data, uint16_t len)
 542 {
 543         struct iovec iov;
 544
 545         iov.iov_base = (void *) data;
 546         iov.iov_len = len;
 547
 548         send_iov(bthost, handle, cid, &iov, 1);
 549 }
 550
 551 static uint8_t l2cap_sig_send(struct bthost *bthost, struct btconn *conn,
 552                                         uint8_t code, uint8_t ident,
 553                                         const void *data, uint16_t len)
 554 {
 555         static uint8_t next_ident = 1;
 556         struct bt_l2cap_hdr_sig hdr;
 557         uint16_t cid;
 558         struct iovec iov[2];
 559
 560         if (!ident) {
 561                 ident = next_ident++;
 562                 if (!ident)
 563                         ident = next_ident++;
 564         }
 565
 566         hdr.code  = code;
 567         hdr.ident = ident;
 568         hdr.len   = cpu_to_le16(len);
 569
 570         iov[0].iov_base = &hdr;
 571         iov[0].iov_len = sizeof(hdr);
 572
 573         if (conn->addr_type == BDADDR_BREDR)
 574                 cid = 0x0001;
 575         else
 576                 cid = 0x0005;
 577
 578         if (len == 0) {
 579                 send_iov(bthost, conn->handle, cid, iov, 1);
 580                 return ident;
 581         }
 582
 583         iov[1].iov_base = (void *) data;
 584         iov[1].iov_len = len;
 585
 586         send_iov(bthost, conn->handle, cid, iov, 2);
 587
 588         return ident;
 589 }
 590
 591 void bthost_add_cid_hook(struct bthost *bthost, uint16_t handle, uint16_t cid,
 592                                 bthost_cid_hook_func_t func, void *user_data)
 593 {
 594         struct cid_hook *hook;
 595         struct btconn *conn;
 596
 597         conn = bthost_find_conn(bthost, handle);
 598         if (!conn)
 599                 return;
 600
 601         hook = malloc(sizeof(*hook));
 602         if (!hook)
 603                 return;
 604
 605         memset(hook, 0, sizeof(*hook));
 606
 607         hook->cid = cid;
 608         hook->func = func;
 609         hook->user_data = user_data;
 610
 611         hook->next = conn->cid_hooks;
 612         conn->cid_hooks = hook;
 613 }
 614
 615 void bthost_send_cid(struct bthost *bthost, uint16_t handle, uint16_t cid,
 616                                         const void *data, uint16_t len)
 617 {
 618         struct btconn *conn;
 619
 620         conn = bthost_find_conn(bthost, handle);
 621         if (!conn)
 622                 return;
 623
 624         send_acl(bthost, handle, cid, data, len);
 625 }
 626
 627 void bthost_send_cid_v(struct bthost *bthost, uint16_t handle, uint16_t cid,
 628                                         const struct iovec *iov, int iovcnt)
 629 {
 630         struct btconn *conn;
 631
 632         conn = bthost_find_conn(bthost, handle);
 633         if (!conn)
 634                 return;
 635
 636         send_iov(bthost, handle, cid, iov, iovcnt);
 637 }
 638
 639 bool bthost_l2cap_req(struct bthost *bthost, uint16_t handle, uint8_t code,
 640                                 const void *data, uint16_t len,
 641                                 bthost_l2cap_rsp_cb cb, void *user_data)
 642 {
 643         struct l2cap_pending_req *req;
 644         struct btconn *conn;
 645         uint8_t ident;
 646
 647         conn = bthost_find_conn(bthost, handle);
 648         if (!conn)
 649                 return false;
 650
 651         if (code == BT_L2CAP_PDU_CONN_REQ &&
 652                         len == sizeof(struct bt_l2cap_pdu_conn_req)) {
 653                 const struct bt_l2cap_pdu_conn_req *req = data;
 654
 655                 bthost_add_l2cap_conn(bthost, conn, le16_to_cpu(req->scid),
 656                                                         le16_to_cpu(req->scid),
 657                                                         le16_to_cpu(req->psm));
 658         }
 659
 660         ident = l2cap_sig_send(bthost, conn, code, 0, data, len);
 661         if (!ident)
 662                 return false;
 663
 664         if (!cb)
 665                 return true;
 666
 667         req = malloc(sizeof(*req));
 668         if (!req)
 669                 return false;
 670
 671         memset(req, 0, sizeof(*req));
 672         req->ident = ident;
 673         req->cb = cb;
 674         req->user_data = user_data;
 675
 676         req->next = bthost->l2reqs;
 677         bthost->l2reqs = req;
 678
 679         return true;
 680 }
 681
 682 static void send_command(struct bthost *bthost, uint16_t opcode,
 683                                                 const void *data, uint8_t len)
 684 {
 685         struct bt_hci_cmd_hdr hdr;
 686         uint8_t pkt = BT_H4_CMD_PKT;
 687         struct iovec iov[3];
 688
 689         iov[0].iov_base = &pkt;
 690         iov[0].iov_len = sizeof(pkt);
 691
 692         hdr.opcode = cpu_to_le16(opcode);
 693         hdr.plen = len;
 694
 695         iov[1].iov_base = &hdr;
 696         iov[1].iov_len = sizeof(hdr);
 697
 698         if (len > 0) {
 699                 iov[2].iov_base = (void *) data;
 700                 iov[2].iov_len = len;
 701         }
 702
 703         if (bthost->ncmd) {
 704                 send_packet(bthost, iov, len > 0 ? 3 : 2);
 705                 bthost->ncmd--;
 706         } else {
 707                 queue_command(bthost, iov, len > 0 ? 3 : 2);
 708         }
 709 }
 710
 711 static void next_cmd(struct bthost *bthost)
 712 {
 713         struct cmd_queue *cmd_q = &bthost->cmd_q;
 714         struct cmd *cmd = cmd_q->head;
 715         struct cmd *next;
 716         struct iovec iov;
 717
 718         if (!cmd)
 719                 return;
 720
 721         next = cmd->next;
 722
 723         if (!bthost->ncmd)
 724                 return;
 725
 726         iov.iov_base = cmd->data;
 727         iov.iov_len = cmd->len;
 728
 729         send_packet(bthost, &iov, 1);
 730         bthost->ncmd--;
 731
 732         if (next)
 733                 next->prev = NULL;
 734         else
 735                 cmd_q->tail = NULL;
 736
 737         cmd_q->head = next;
 738
 739         free(cmd);
 740 }
 741
 742 static void read_bd_addr_complete(struct bthost *bthost, const void *data,
 743                                                                 uint8_t len)
 744 {
 745         const struct bt_hci_rsp_read_bd_addr *ev = data;
 746
 747         if (len < sizeof(*ev))
 748                 return;
 749
 750         if (ev->status)
 751                 return;
 752
 753         memcpy(bthost->bdaddr, ev->bdaddr, 6);
 754
 755         bthost->ready = true;
 756
 757         if (bthost->ready_cb) {
 758                 bthost->ready_cb();
 759                 bthost->ready_cb = NULL;
 760         }
 761 }
 762
 763 void bthost_notify_ready(struct bthost *bthost, bthost_ready_cb cb)
 764 {
 765         if (bthost->ready) {
 766                 cb();
 767                 return;
 768         }
 769
 770         bthost->ready_cb = cb;
 771 }
 772
 773 static void read_local_features_complete(struct bthost *bthost,
 774                                                 const void *data, uint8_t len)
 775 {
 776         const struct bt_hci_rsp_read_local_features *ev = data;
 777
 778         if (len < sizeof(*ev))
 779                 return;
 780
 781         if (ev->status)
 782                 return;
 783
 784         memcpy(bthost->features, ev->features, 8);
 785 }
 786
 787 static void evt_cmd_complete(struct bthost *bthost, const void *data,
 788                                                                 uint8_t len)
 789 {
 790         const struct bt_hci_evt_cmd_complete *ev = data;
 791         const void *param;
 792         uint16_t opcode;
 793
 794         if (len < sizeof(*ev))
 795                 return;
 796
 797         param = data + sizeof(*ev);
 798
 799         bthost->ncmd = ev->ncmd;
 800
 801         opcode = le16toh(ev->opcode);
 802
 803         switch (opcode) {
 804         case BT_HCI_CMD_RESET:
 805                 break;
 806         case BT_HCI_CMD_READ_LOCAL_FEATURES:
 807                 read_local_features_complete(bthost, param, len - sizeof(*ev));
 808                 break;
 809         case BT_HCI_CMD_READ_BD_ADDR:
 810                 read_bd_addr_complete(bthost, param, len - sizeof(*ev));
 811                 break;
 812         case BT_HCI_CMD_WRITE_SCAN_ENABLE:
 813                 break;
 814         case BT_HCI_CMD_LE_SET_ADV_ENABLE:
 815                 break;
 816         case BT_HCI_CMD_LE_SET_ADV_PARAMETERS:
 817                 break;
 818         case BT_HCI_CMD_PIN_CODE_REQUEST_REPLY:
 819                 break;
 820         case BT_HCI_CMD_PIN_CODE_REQUEST_NEG_REPLY:
 821                 break;
 822         case BT_HCI_CMD_LINK_KEY_REQUEST_NEG_REPLY:
 823                 break;
 824         case BT_HCI_CMD_WRITE_SIMPLE_PAIRING_MODE:
 825                 break;
 826         case BT_HCI_CMD_WRITE_LE_HOST_SUPPORTED:
 827                 break;
 828         case BT_HCI_CMD_WRITE_SECURE_CONN_SUPPORT:
 829                 break;
 830         case BT_HCI_CMD_IO_CAPABILITY_REQUEST_REPLY:
 831                 break;
 832         case BT_HCI_CMD_USER_CONFIRM_REQUEST_REPLY:
 833                 break;
 834         case BT_HCI_CMD_USER_CONFIRM_REQUEST_NEG_REPLY:
 835                 break;
 836         case BT_HCI_CMD_LE_LTK_REQ_REPLY:
 837                 break;
 838         case BT_HCI_CMD_LE_LTK_REQ_NEG_REPLY:
 839                 break;
 840         case BT_HCI_CMD_LE_SET_ADV_DATA:
 841                 break;
 842         default:
 843                 printf("Unhandled cmd_complete opcode 0x%04x\n", opcode);
 844                 break;
 845         }
 846
 847         if (bthost->cmd_complete_cb)
 848                 bthost->cmd_complete_cb(opcode, 0, param, len - sizeof(*ev),
 849                                                 bthost->cmd_complete_data);
 850
 851         next_cmd(bthost);
 852 }
 853
 854 static void evt_cmd_status(struct bthost *bthost, const void *data,
 855                                                                 uint8_t len)
 856 {
 857         const struct bt_hci_evt_cmd_status *ev = data;
 858         uint16_t opcode;
 859
 860         if (len < sizeof(*ev))
 861                 return;
 862
 863         bthost->ncmd = ev->ncmd;
 864
 865         opcode = le16toh(ev->opcode);
 866
 867         if (ev->status && bthost->cmd_complete_cb)
 868                 bthost->cmd_complete_cb(opcode, ev->status, NULL, 0,
 869                                                 bthost->cmd_complete_data);
 870
 871         next_cmd(bthost);
 872 }
 873
 874 static void evt_conn_request(struct bthost *bthost, const void *data,
 875                                                                 uint8_t len)
 876 {
 877         const struct bt_hci_evt_conn_request *ev = data;
 878         struct bt_hci_cmd_accept_conn_request cmd;
 879
 880         if (len < sizeof(*ev))
 881                 return;
 882
 883         memset(&cmd, 0, sizeof(cmd));
 884         memcpy(cmd.bdaddr, ev->bdaddr, sizeof(ev->bdaddr));
 885
 886         send_command(bthost, BT_HCI_CMD_ACCEPT_CONN_REQUEST, &cmd,
 887                                                                 sizeof(cmd));
 888 }
 889
 890 static void init_conn(struct bthost *bthost, uint16_t handle,
 891                                 const uint8_t *bdaddr, uint8_t addr_type)
 892 {
 893         struct btconn *conn;
 894         const uint8_t *ia, *ra;
 895
 896         conn = malloc(sizeof(*conn));
 897         if (!conn)
 898                 return;
 899
 900         memset(conn, 0, sizeof(*conn));
 901         conn->handle = handle;
 902         memcpy(conn->bdaddr, bdaddr, 6);
 903         conn->addr_type = addr_type;
 904         conn->next_cid = 0x0040;
 905
 906         conn->next = bthost->conns;
 907         bthost->conns = conn;
 908
 909         if (bthost->conn_init) {
 910                 ia = bthost->bdaddr;
 911                 ra = conn->bdaddr;
 912         } else {
 913                 ia = conn->bdaddr;
 914                 ra = bthost->bdaddr;
 915         }
 916
 917         conn->smp_data = smp_conn_add(bthost->smp_data, handle, ia, ra,
 918                                                 addr_type, bthost->conn_init);
 919
 920         if (bthost->new_conn_cb)
 921                 bthost->new_conn_cb(conn->handle, bthost->new_conn_data);
 922
 923         if (addr_type == BDADDR_BREDR) {
 924                 struct bt_l2cap_pdu_info_req req;
 925                 req.type = L2CAP_IT_FIXED_CHAN;
 926                 l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_INFO_REQ, 1,
 927                                                         &req, sizeof(req));
 928         }
 929 }
 930
 931 static void evt_conn_complete(struct bthost *bthost, const void *data,
 932                                                                 uint8_t len)
 933 {
 934         const struct bt_hci_evt_conn_complete *ev = data;
 935
 936         if (len < sizeof(*ev))
 937                 return;
 938
 939         if (ev->status)
 940                 return;
 941
 942         init_conn(bthost, le16_to_cpu(ev->handle), ev->bdaddr, BDADDR_BREDR);
 943 }
 944
 945 static void evt_disconn_complete(struct bthost *bthost, const void *data,
 946                                                                 uint8_t len)
 947 {
 948         const struct bt_hci_evt_disconnect_complete *ev = data;
 949         struct btconn **curr;
 950         uint16_t handle;
 951
 952         if (len < sizeof(*ev))
 953                 return;
 954
 955         if (ev->status)
 956                 return;
 957
 958         handle = le16_to_cpu(ev->handle);
 959
 960         for (curr = &bthost->conns; *curr;) {
 961                 struct btconn *conn = *curr;
 962
 963                 if (conn->handle == handle) {
 964                         *curr = conn->next;
 965                         btconn_free(conn);
 966                 } else {
 967                         curr = &conn->next;
 968                 }
 969         }
 970 }
 971
 972 static void evt_num_completed_packets(struct bthost *bthost, const void *data,
 973                                                                 uint8_t len)
 974 {
 975         const struct bt_hci_evt_num_completed_packets *ev = data;
 976
 977         if (len < sizeof(*ev))
 978                 return;
 979 }
 980
 981 static void evt_auth_complete(struct bthost *bthost, const void *data,
 982                                                                 uint8_t len)
 983 {
 984         const struct bt_hci_evt_auth_complete *ev = data;
 985         struct bt_hci_cmd_set_conn_encrypt cp;
 986
 987         if (len < sizeof(*ev))
 988                 return;
 989
 990         if (ev->status)
 991                 return;
 992
 993         cp.handle = ev->handle;
 994         cp.encr_mode = 0x01;
 995
 996         send_command(bthost, BT_HCI_CMD_SET_CONN_ENCRYPT, &cp, sizeof(cp));
 997 }
 998
 999 static void evt_pin_code_request(struct bthost *bthost, const void *data,
1000                                                                 uint8_t len)
1001 {
1002         const struct bt_hci_evt_pin_code_request *ev = data;
1003
1004         if (len < sizeof(*ev))
1005                 return;
1006
1007         if (bthost->pin_len > 0) {
1008                 struct bt_hci_cmd_pin_code_request_reply cp;
1009
1010                 memset(&cp, 0, sizeof(cp));
1011                 memcpy(cp.bdaddr, ev->bdaddr, 6);
1012                 cp.pin_len = bthost->pin_len;
1013                 memcpy(cp.pin_code, bthost->pin, bthost->pin_len);
1014
1015                 send_command(bthost, BT_HCI_CMD_PIN_CODE_REQUEST_REPLY,
1016                                                         &cp, sizeof(cp));
1017         } else {
1018                 struct bt_hci_cmd_pin_code_request_neg_reply cp;
1019
1020                 memcpy(cp.bdaddr, ev->bdaddr, 6);
1021                 send_command(bthost, BT_HCI_CMD_PIN_CODE_REQUEST_NEG_REPLY,
1022                                                         &cp, sizeof(cp));
1023         }
1024 }
1025
1026 static void evt_link_key_request(struct bthost *bthost, const void *data,
1027                                                                 uint8_t len)
1028 {
1029         const struct bt_hci_evt_link_key_request *ev = data;
1030         struct bt_hci_cmd_link_key_request_neg_reply cp;
1031
1032         if (len < sizeof(*ev))
1033                 return;
1034
1035         memset(&cp, 0, sizeof(cp));
1036         memcpy(cp.bdaddr, ev->bdaddr, 6);
1037
1038         send_command(bthost, BT_HCI_CMD_LINK_KEY_REQUEST_NEG_REPLY,
1039                                                         &cp, sizeof(cp));
1040 }
1041
1042 static void evt_link_key_notify(struct bthost *bthost, const void *data,
1043                                                                 uint8_t len)
1044 {
1045         const struct bt_hci_evt_link_key_notify *ev = data;
1046
1047         if (len < sizeof(*ev))
1048                 return;
1049 }
1050
1051 static void evt_encrypt_change(struct bthost *bthost, const void *data,
1052                                                                 uint8_t len)
1053 {
1054         const struct bt_hci_evt_encrypt_change *ev = data;
1055         struct btconn *conn;
1056         uint16_t handle;
1057
1058         if (len < sizeof(*ev))
1059                 return;
1060
1061         handle = acl_handle(ev->handle);
1062         conn = bthost_find_conn(bthost, handle);
1063         if (!conn)
1064                 return;
1065
1066         if (ev->status)
1067                 return;
1068
1069         conn->encr_mode = ev->encr_mode;
1070
1071         if (conn->smp_data)
1072                 smp_conn_encrypted(conn->smp_data, conn->encr_mode);
1073 }
1074
1075 static void evt_io_cap_response(struct bthost *bthost, const void *data,
1076                                                                 uint8_t len)
1077 {
1078         const struct bt_hci_evt_io_capability_response *ev = data;
1079         struct btconn *conn;
1080
1081         if (len < sizeof(*ev))
1082                 return;
1083
1084         conn = bthost_find_conn_by_bdaddr(bthost, ev->bdaddr);
1085         if (!conn)
1086                 return;
1087 }
1088
1089 static void evt_io_cap_request(struct bthost *bthost, const void *data,
1090                                                                 uint8_t len)
1091 {
1092         const struct bt_hci_evt_io_capability_request *ev = data;
1093         struct bt_hci_cmd_io_capability_request_reply cp;
1094         struct btconn *conn;
1095
1096         if (len < sizeof(*ev))
1097                 return;
1098
1099         conn = bthost_find_conn_by_bdaddr(bthost, ev->bdaddr);
1100         if (!conn)
1101                 return;
1102
1103         memcpy(cp.bdaddr, ev->bdaddr, 6);
1104         cp.capability = bthost->io_capability;
1105         cp.oob_data = 0x00;
1106         cp.authentication = bthost->auth_req;
1107
1108         send_command(bthost, BT_HCI_CMD_IO_CAPABILITY_REQUEST_REPLY,
1109                                                         &cp, sizeof(cp));
1110 }
1111
1112 static void evt_user_confirm_request(struct bthost *bthost, const void *data,
1113                                                                 uint8_t len)
1114 {
1115         const struct bt_hci_evt_user_confirm_request *ev = data;
1116         struct btconn *conn;
1117
1118         if (len < sizeof(*ev))
1119                 return;
1120
1121         conn = bthost_find_conn_by_bdaddr(bthost, ev->bdaddr);
1122         if (!conn)
1123                 return;
1124
1125         if (bthost->reject_user_confirm) {
1126                 send_command(bthost, BT_HCI_CMD_USER_CONFIRM_REQUEST_NEG_REPLY,
1127                                                                 ev->bdaddr, 6);
1128                 return;
1129         }
1130
1131         send_command(bthost, BT_HCI_CMD_USER_CONFIRM_REQUEST_REPLY,
1132                                                                 ev->bdaddr, 6);
1133 }
1134
1135 static void evt_simple_pairing_complete(struct bthost *bthost, const void *data,
1136                                                                 uint8_t len)
1137 {
1138         const struct bt_hci_evt_simple_pairing_complete *ev = data;
1139
1140         if (len < sizeof(*ev))
1141                 return;
1142 }
1143
1144 static void evt_le_conn_complete(struct bthost *bthost, const void *data,
1145                                                                 uint8_t len)
1146 {
1147         const struct bt_hci_evt_le_conn_complete *ev = data;
1148         uint8_t addr_type;
1149
1150         if (len < sizeof(*ev))
1151                 return;
1152
1153         if (ev->status)
1154                 return;
1155
1156         if (ev->peer_addr_type == 0x00)
1157                 addr_type = BDADDR_LE_PUBLIC;
1158         else
1159                 addr_type = BDADDR_LE_RANDOM;
1160
1161         init_conn(bthost, le16_to_cpu(ev->handle), ev->peer_addr, addr_type);
1162 }
1163
1164 static void evt_le_conn_update_complete(struct bthost *bthost, const void *data,
1165                                                                 uint8_t len)
1166 {
1167         const struct bt_hci_evt_le_conn_update_complete *ev = data;
1168
1169         if (len < sizeof(*ev))
1170                 return;
1171
1172         if (ev->status)
1173                 return;
1174 }
1175
1176 static void evt_le_remote_features_complete(struct bthost *bthost,
1177                                                 const void *data, uint8_t len)
1178 {
1179         const struct bt_hci_evt_le_remote_features_complete *ev = data;
1180
1181         if (len < sizeof(*ev))
1182                 return;
1183
1184         if (ev->status)
1185                 return;
1186 }
1187
1188 static void evt_le_ltk_request(struct bthost *bthost, const void *data,
1189                                                                 uint8_t len)
1190 {
1191         const struct bt_hci_evt_le_long_term_key_request *ev = data;
1192         struct bt_hci_cmd_le_ltk_req_reply cp;
1193         struct bt_hci_cmd_le_ltk_req_neg_reply *neg_cp = (void *) &cp;
1194         uint16_t handle, ediv;
1195         uint64_t rand;
1196         struct btconn *conn;
1197         int err;
1198
1199         if (len < sizeof(*ev))
1200                 return;
1201
1202         handle = acl_handle(ev->handle);
1203         conn = bthost_find_conn(bthost, handle);
1204         if (!conn)
1205                 return;
1206
1207         rand = le64_to_cpu(ev->rand);
1208         ediv = le16_to_cpu(ev->ediv);
1209
1210         cp.handle = ev->handle;
1211
1212         err = smp_get_ltk(conn->smp_data, rand, ediv, cp.ltk);
1213         if (err < 0)
1214                 send_command(bthost, BT_HCI_CMD_LE_LTK_REQ_NEG_REPLY,
1215                                                 neg_cp, sizeof(*neg_cp));
1216         else
1217                 send_command(bthost, BT_HCI_CMD_LE_LTK_REQ_REPLY, &cp,
1218                                                                 sizeof(cp));
1219 }
1220
1221 static void evt_le_meta_event(struct bthost *bthost, const void *data,
1222                                                                 uint8_t len)
1223 {
1224         const uint8_t *event = data;
1225         const void *evt_data = data + 1;
1226
1227         if (len < 1)
1228                 return;
1229
1230         switch (*event) {
1231         case BT_HCI_EVT_LE_CONN_COMPLETE:
1232                 evt_le_conn_complete(bthost, evt_data, len - 1);
1233                 break;
1234         case BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE:
1235                 evt_le_conn_update_complete(bthost, evt_data, len - 1);
1236                 break;
1237         case BT_HCI_EVT_LE_REMOTE_FEATURES_COMPLETE:
1238                 evt_le_remote_features_complete(bthost, evt_data, len - 1);
1239                 break;
1240         case BT_HCI_EVT_LE_LONG_TERM_KEY_REQUEST:
1241                 evt_le_ltk_request(bthost, evt_data, len - 1);
1242                 break;
1243         default:
1244                 printf("Unsupported LE Meta event 0x%2.2x\n", *event);
1245                 break;
1246         }
1247 }
1248
1249 static void process_evt(struct bthost *bthost, const void *data, uint16_t len)
1250 {
1251         const struct bt_hci_evt_hdr *hdr = data;
1252         const void *param;
1253
1254         if (len < sizeof(*hdr))
1255                 return;
1256
1257         if (sizeof(*hdr) + hdr->plen != len)
1258                 return;
1259
1260         param = data + sizeof(*hdr);
1261
1262         switch (hdr->evt) {
1263         case BT_HCI_EVT_CMD_COMPLETE:
1264                 evt_cmd_complete(bthost, param, hdr->plen);
1265                 break;
1266
1267         case BT_HCI_EVT_CMD_STATUS:
1268                 evt_cmd_status(bthost, param, hdr->plen);
1269                 break;
1270
1271         case BT_HCI_EVT_CONN_REQUEST:
1272                 evt_conn_request(bthost, param, hdr->plen);
1273                 break;
1274
1275         case BT_HCI_EVT_CONN_COMPLETE:
1276                 evt_conn_complete(bthost, param, hdr->plen);
1277                 break;
1278
1279         case BT_HCI_EVT_DISCONNECT_COMPLETE:
1280                 evt_disconn_complete(bthost, param, hdr->plen);
1281                 break;
1282
1283         case BT_HCI_EVT_NUM_COMPLETED_PACKETS:
1284                 evt_num_completed_packets(bthost, param, hdr->plen);
1285                 break;
1286
1287         case BT_HCI_EVT_AUTH_COMPLETE:
1288                 evt_auth_complete(bthost, param, hdr->plen);
1289                 break;
1290
1291         case BT_HCI_EVT_PIN_CODE_REQUEST:
1292                 evt_pin_code_request(bthost, param, hdr->plen);
1293                 break;
1294
1295         case BT_HCI_EVT_LINK_KEY_REQUEST:
1296                 evt_link_key_request(bthost, param, hdr->plen);
1297                 break;
1298
1299         case BT_HCI_EVT_LINK_KEY_NOTIFY:
1300                 evt_link_key_notify(bthost, param, hdr->plen);
1301                 break;
1302
1303         case BT_HCI_EVT_ENCRYPT_CHANGE:
1304                 evt_encrypt_change(bthost, param, hdr->plen);
1305                 break;
1306
1307         case BT_HCI_EVT_IO_CAPABILITY_RESPONSE:
1308                 evt_io_cap_response(bthost, param, hdr->plen);
1309                 break;
1310
1311         case BT_HCI_EVT_IO_CAPABILITY_REQUEST:
1312                 evt_io_cap_request(bthost, param, hdr->plen);
1313                 break;
1314
1315         case BT_HCI_EVT_USER_CONFIRM_REQUEST:
1316                 evt_user_confirm_request(bthost, param, hdr->plen);
1317                 break;
1318
1319         case BT_HCI_EVT_SIMPLE_PAIRING_COMPLETE:
1320                 evt_simple_pairing_complete(bthost, param, hdr->plen);
1321                 break;
1322
1323         case BT_HCI_EVT_LE_META_EVENT:
1324                 evt_le_meta_event(bthost, param, hdr->plen);
1325                 break;
1326
1327         default:
1328                 printf("Unsupported event 0x%2.2x\n", hdr->evt);
1329                 break;
1330         }
1331 }
1332
1333 static bool l2cap_cmd_rej(struct bthost *bthost, struct btconn *conn,
1334                                 uint8_t ident, const void *data, uint16_t len)
1335 {
1336         const struct bt_l2cap_pdu_cmd_reject *rsp = data;
1337
1338         if (len < sizeof(*rsp))
1339                 return false;
1340
1341         return true;
1342 }
1343
1344 static bool l2cap_conn_req(struct bthost *bthost, struct btconn *conn,
1345                                 uint8_t ident, const void *data, uint16_t len)
1346 {
1347         const struct bt_l2cap_pdu_conn_req *req = data;
1348         struct l2cap_conn_cb_data *cb_data;
1349         struct bt_l2cap_pdu_conn_rsp rsp;
1350         uint16_t psm;
1351
1352         if (len < sizeof(*req))
1353                 return false;
1354
1355         psm = le16_to_cpu(req->psm);
1356
1357         memset(&rsp, 0, sizeof(rsp));
1358         rsp.scid = req->scid;
1359
1360         cb_data = bthost_find_l2cap_cb_by_psm(bthost, psm);
1361         if (cb_data)
1362                 rsp.dcid = rsp.scid;
1363         else
1364                 rsp.result = cpu_to_le16(0x0002); /* PSM Not Supported */
1365
1366         l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONN_RSP, ident, &rsp,
1367                                                                 sizeof(rsp));
1368
1369         if (!rsp.result) {
1370                 struct bt_l2cap_pdu_config_req conf_req;
1371                 struct l2conn *l2conn;
1372
1373                 l2conn = bthost_add_l2cap_conn(bthost, conn,
1374                                                         le16_to_cpu(rsp.dcid),
1375                                                         le16_to_cpu(rsp.scid),
1376                                                         le16_to_cpu(psm));
1377
1378                 memset(&conf_req, 0, sizeof(conf_req));
1379                 conf_req.dcid = rsp.scid;
1380
1381                 l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONFIG_REQ, 0,
1382                                                 &conf_req, sizeof(conf_req));
1383
1384                 if (cb_data && l2conn->psm == cb_data->psm && cb_data->func)
1385                         cb_data->func(conn->handle, l2conn->dcid,
1386                                                         cb_data->user_data);
1387         }
1388
1389         return true;
1390 }
1391
1392 static void rfcomm_sabm_send(struct bthost *bthost, struct btconn *conn,
1393                         struct l2conn *l2conn, uint8_t cr, uint8_t dlci)
1394 {
1395         struct rfcomm_cmd cmd;
1396
1397         cmd.address = RFCOMM_ADDR(cr, dlci);
1398         cmd.control = RFCOMM_CTRL(RFCOMM_SABM, 1);
1399         cmd.length = RFCOMM_LEN8(0);
1400         cmd.fcs = rfcomm_fcs2((uint8_t *)&cmd);
1401
1402         send_acl(bthost, conn->handle, l2conn->dcid, &cmd, sizeof(cmd));
1403 }
1404
1405 static bool l2cap_conn_rsp(struct bthost *bthost, struct btconn *conn,
1406                                 uint8_t ident, const void *data, uint16_t len)
1407 {
1408         const struct bt_l2cap_pdu_conn_rsp *rsp = data;
1409         struct bt_l2cap_pdu_config_req req;
1410         struct l2conn *l2conn;
1411
1412         if (len < sizeof(*rsp))
1413                 return false;
1414
1415         l2conn = btconn_find_l2cap_conn_by_scid(conn, le16_to_cpu(rsp->scid));
1416         if (l2conn)
1417                 l2conn->dcid = le16_to_cpu(rsp->dcid);
1418         else
1419                 return false;
1420
1421         if (rsp->result)
1422                 return true;
1423
1424         memset(&req, 0, sizeof(req));
1425         req.dcid = rsp->dcid;
1426
1427         l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONFIG_REQ, 0,
1428                                                         &req, sizeof(req));
1429
1430         return true;
1431 }
1432
1433 static bool l2cap_config_req(struct bthost *bthost, struct btconn *conn,
1434                                 uint8_t ident, const void *data, uint16_t len)
1435 {
1436         const struct bt_l2cap_pdu_config_req *req = data;
1437         struct bt_l2cap_pdu_config_rsp rsp;
1438         struct l2conn *l2conn;
1439         uint16_t dcid;
1440
1441         if (len < sizeof(*req))
1442                 return false;
1443
1444         dcid = le16_to_cpu(req->dcid);
1445
1446         l2conn = btconn_find_l2cap_conn_by_scid(conn, dcid);
1447         if (!l2conn)
1448                 return false;
1449
1450         memset(&rsp, 0, sizeof(rsp));
1451         rsp.scid  = cpu_to_le16(l2conn->dcid);
1452         rsp.flags = req->flags;
1453
1454         l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONFIG_RSP, ident, &rsp,
1455                                                                 sizeof(rsp));
1456
1457         return true;
1458 }
1459
1460 static bool l2cap_config_rsp(struct bthost *bthost, struct btconn *conn,
1461                                 uint8_t ident, const void *data, uint16_t len)
1462 {
1463         const struct bt_l2cap_pdu_config_rsp *rsp = data;
1464         struct l2conn *l2conn;
1465
1466         if (len < sizeof(*rsp))
1467                 return false;
1468
1469         l2conn = btconn_find_l2cap_conn_by_scid(conn, rsp->scid);
1470         if (!l2conn)
1471                 return false;
1472
1473         if (l2conn->psm == 0x0003 && !rsp->result && bthost->rfcomm_conn_data)
1474                 rfcomm_sabm_send(bthost, conn, l2conn, 1, 0);
1475
1476         return true;
1477 }
1478
1479 static bool l2cap_disconn_req(struct bthost *bthost, struct btconn *conn,
1480                                 uint8_t ident, const void *data, uint16_t len)
1481 {
1482         const struct bt_l2cap_pdu_disconn_req *req = data;
1483         struct bt_l2cap_pdu_disconn_rsp rsp;
1484
1485         if (len < sizeof(*req))
1486                 return false;
1487
1488         memset(&rsp, 0, sizeof(rsp));
1489         rsp.dcid = req->dcid;
1490         rsp.scid = req->scid;
1491
1492         l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_DISCONN_RSP, ident, &rsp,
1493                                                                 sizeof(rsp));
1494
1495         return true;
1496 }
1497
1498 static bool l2cap_info_req(struct bthost *bthost, struct btconn *conn,
1499                                 uint8_t ident, const void *data, uint16_t len)
1500 {
1501         const struct bt_l2cap_pdu_info_req *req = data;
1502         uint64_t fixed_chan;
1503         uint16_t type;
1504         uint8_t buf[12];
1505         struct bt_l2cap_pdu_info_rsp *rsp = (void *) buf;
1506
1507         if (len < sizeof(*req))
1508                 return false;
1509
1510         memset(buf, 0, sizeof(buf));
1511         rsp->type = req->type;
1512
1513         type = le16_to_cpu(req->type);
1514
1515         switch (type) {
1516         case L2CAP_IT_FEAT_MASK:
1517                 rsp->result = 0x0000;
1518                 put_le32(L2CAP_FEAT_FIXED_CHAN, rsp->data);
1519                 l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_INFO_RSP, ident,
1520                                                         rsp, sizeof(*rsp) + 4);
1521                 break;
1522         case L2CAP_IT_FIXED_CHAN:
1523                 rsp->result = 0x0000;
1524                 fixed_chan = L2CAP_FC_SIG_BREDR;
1525                 if (bthost->sc && bthost->le)
1526                         fixed_chan |= L2CAP_FC_SMP_BREDR;
1527                 put_le64(fixed_chan, rsp->data);
1528                 l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_INFO_RSP, ident,
1529                                 rsp, sizeof(*rsp) + sizeof(fixed_chan));
1530                 break;
1531         default:
1532                 rsp->result = cpu_to_le16(0x0001); /* Not Supported */
1533                 l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_INFO_RSP, ident,
1534                                                         rsp, sizeof(*rsp));
1535                 break;
1536         }
1537
1538         return true;
1539 }
1540
1541 static bool l2cap_info_rsp(struct bthost *bthost, struct btconn *conn,
1542                                 uint8_t ident, const void *data, uint16_t len)
1543 {
1544         const struct bt_l2cap_pdu_info_rsp *rsp = data;
1545         uint16_t type;
1546
1547         if (len < sizeof(*rsp))
1548                 return false;
1549
1550         if (rsp->result)
1551                 return true;
1552
1553         type = le16_to_cpu(rsp->type);
1554
1555         switch (type) {
1556         case L2CAP_IT_FIXED_CHAN:
1557                 if (len < sizeof(*rsp) + 8)
1558                         return false;
1559                 conn->fixed_chan = get_le64(rsp->data);
1560                 if (conn->smp_data && conn->encr_mode)
1561                         smp_conn_encrypted(conn->smp_data, conn->encr_mode);
1562                 break;
1563         default:
1564                 break;
1565         }
1566
1567         return true;
1568 }
1569
1570 static void handle_pending_l2reqs(struct bthost *bthost, struct btconn *conn,
1571                                                 uint8_t ident, uint8_t code,
1572                                                 const void *data, uint16_t len)
1573 {
1574         struct l2cap_pending_req **curr;
1575
1576         for (curr = &bthost->l2reqs; *curr != NULL;) {
1577                 struct l2cap_pending_req *req = *curr;
1578
1579                 if (req->ident != ident) {
1580                         curr = &req->next;
1581                         continue;
1582                 }
1583
1584                 *curr = req->next;
1585                 req->cb(code, data, len, req->user_data);
1586                 free(req);
1587         }
1588 }
1589
1590 static void l2cap_sig(struct bthost *bthost, struct btconn *conn,
1591                                                 const void *data, uint16_t len)
1592 {
1593         const struct bt_l2cap_hdr_sig *hdr = data;
1594         struct bt_l2cap_pdu_cmd_reject rej;
1595         uint16_t hdr_len;
1596         bool ret;
1597
1598         if (len < sizeof(*hdr))
1599                 goto reject;
1600
1601         hdr_len = le16_to_cpu(hdr->len);
1602
1603         if (sizeof(*hdr) + hdr_len != len)
1604                 goto reject;
1605
1606         switch (hdr->code) {
1607         case BT_L2CAP_PDU_CMD_REJECT:
1608                 ret = l2cap_cmd_rej(bthost, conn, hdr->ident,
1609                                                 data + sizeof(*hdr), hdr_len);
1610                 break;
1611
1612         case BT_L2CAP_PDU_CONN_REQ:
1613                 ret = l2cap_conn_req(bthost, conn, hdr->ident,
1614                                                 data + sizeof(*hdr), hdr_len);
1615                 break;
1616
1617         case BT_L2CAP_PDU_CONN_RSP:
1618                 ret = l2cap_conn_rsp(bthost, conn, hdr->ident,
1619                                                 data + sizeof(*hdr), hdr_len);
1620                 break;
1621
1622         case BT_L2CAP_PDU_CONFIG_REQ:
1623                 ret = l2cap_config_req(bthost, conn, hdr->ident,
1624                                                 data + sizeof(*hdr), hdr_len);
1625                 break;
1626
1627         case BT_L2CAP_PDU_CONFIG_RSP:
1628                 ret = l2cap_config_rsp(bthost, conn, hdr->ident,
1629                                                 data + sizeof(*hdr), hdr_len);
1630                 break;
1631
1632         case BT_L2CAP_PDU_DISCONN_REQ:
1633                 ret = l2cap_disconn_req(bthost, conn, hdr->ident,
1634                                                 data + sizeof(*hdr), hdr_len);
1635                 break;
1636
1637         case BT_L2CAP_PDU_INFO_REQ:
1638                 ret = l2cap_info_req(bthost, conn, hdr->ident,
1639                                                 data + sizeof(*hdr), hdr_len);
1640                 break;
1641
1642         case BT_L2CAP_PDU_INFO_RSP:
1643                 ret = l2cap_info_rsp(bthost, conn, hdr->ident,
1644                                                 data + sizeof(*hdr), hdr_len);
1645                 break;
1646
1647         default:
1648                 printf("Unknown L2CAP code 0x%02x\n", hdr->code);
1649                 ret = false;
1650         }
1651
1652         handle_pending_l2reqs(bthost, conn, hdr->ident, hdr->code,
1653                                                 data + sizeof(*hdr), hdr_len);
1654
1655         if (ret)
1656                 return;
1657
1658 reject:
1659         memset(&rej, 0, sizeof(rej));
1660         l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CMD_REJECT, 0,
1661                                                         &rej, sizeof(rej));
1662 }
1663
1664 static bool l2cap_conn_param_req(struct bthost *bthost, struct btconn *conn,
1665                                 uint8_t ident, const void *data, uint16_t len)
1666 {
1667         const struct bt_l2cap_pdu_conn_param_req *req = data;
1668         struct bt_l2cap_pdu_conn_param_rsp rsp;
1669         struct bt_hci_cmd_le_conn_update hci_cmd;
1670
1671         if (len < sizeof(*req))
1672                 return false;
1673
1674         memset(&hci_cmd, 0, sizeof(hci_cmd));
1675         hci_cmd.handle = cpu_to_le16(conn->handle);
1676         hci_cmd.min_interval = req->min_interval;
1677         hci_cmd.max_interval = req->max_interval;
1678         hci_cmd.latency = req->latency;
1679         hci_cmd.supv_timeout = req->timeout;
1680         hci_cmd.min_length = cpu_to_le16(0x0001);
1681         hci_cmd.max_length = cpu_to_le16(0x0001);
1682
1683         send_command(bthost, BT_HCI_CMD_LE_CONN_UPDATE,
1684                                                 &hci_cmd, sizeof(hci_cmd));
1685
1686         memset(&rsp, 0, sizeof(rsp));
1687         l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONN_PARAM_RSP, ident,
1688                                                         &rsp, sizeof(rsp));
1689
1690         return true;
1691 }
1692
1693 static bool l2cap_conn_param_rsp(struct bthost *bthost, struct btconn *conn,
1694                                 uint8_t ident, const void *data, uint16_t len)
1695 {
1696         const struct bt_l2cap_pdu_conn_param_req *rsp = data;
1697
1698         if (len < sizeof(*rsp))
1699                 return false;
1700
1701         return true;
1702 }
1703
1704 static bool l2cap_le_conn_req(struct bthost *bthost, struct btconn *conn,
1705                                 uint8_t ident, const void *data, uint16_t len)
1706 {
1707         const struct bt_l2cap_pdu_le_conn_req *req = data;
1708         struct bt_l2cap_pdu_le_conn_rsp rsp;
1709         uint16_t psm;
1710
1711         if (len < sizeof(*req))
1712                 return false;
1713
1714         psm = le16_to_cpu(req->psm);
1715
1716         memset(&rsp, 0, sizeof(rsp));
1717
1718         rsp.mtu = 23;
1719         rsp.mps = 23;
1720         rsp.credits = 1;
1721
1722         if (bthost_find_l2cap_cb_by_psm(bthost, psm))
1723                 rsp.dcid = cpu_to_le16(conn->next_cid++);
1724         else
1725                 rsp.result = cpu_to_le16(0x0002); /* PSM Not Supported */
1726
1727         l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_LE_CONN_RSP, ident, &rsp,
1728                                                                 sizeof(rsp));
1729
1730         return true;
1731 }
1732
1733 static bool l2cap_le_conn_rsp(struct bthost *bthost, struct btconn *conn,
1734                                 uint8_t ident, const void *data, uint16_t len)
1735 {
1736         const struct bt_l2cap_pdu_le_conn_rsp *rsp = data;
1737
1738         if (len < sizeof(*rsp))
1739                 return false;
1740         /* TODO add L2CAP connection before with proper PSM */
1741         bthost_add_l2cap_conn(bthost, conn, 0, le16_to_cpu(rsp->dcid), 0);
1742
1743         return true;
1744 }
1745
1746 static void l2cap_le_sig(struct bthost *bthost, struct btconn *conn,
1747                                                 const void *data, uint16_t len)
1748 {
1749         const struct bt_l2cap_hdr_sig *hdr = data;
1750         struct bt_l2cap_pdu_cmd_reject rej;
1751         uint16_t hdr_len;
1752         bool ret;
1753
1754         if (len < sizeof(*hdr))
1755                 goto reject;
1756
1757         hdr_len = le16_to_cpu(hdr->len);
1758
1759         if (sizeof(*hdr) + hdr_len != len)
1760                 goto reject;
1761
1762         switch (hdr->code) {
1763         case BT_L2CAP_PDU_CMD_REJECT:
1764                 ret = l2cap_cmd_rej(bthost, conn, hdr->ident,
1765                                                 data + sizeof(*hdr), hdr_len);
1766                 break;
1767
1768         case BT_L2CAP_PDU_DISCONN_REQ:
1769                 ret = l2cap_disconn_req(bthost, conn, hdr->ident,
1770                                                 data + sizeof(*hdr), hdr_len);
1771                 break;
1772
1773         case BT_L2CAP_PDU_CONN_PARAM_REQ:
1774                 ret = l2cap_conn_param_req(bthost, conn, hdr->ident,
1775                                                 data + sizeof(*hdr), hdr_len);
1776                 break;
1777
1778         case BT_L2CAP_PDU_CONN_PARAM_RSP:
1779                 ret = l2cap_conn_param_rsp(bthost, conn, hdr->ident,
1780                                                 data + sizeof(*hdr), hdr_len);
1781                 break;
1782
1783         case BT_L2CAP_PDU_LE_CONN_REQ:
1784                 ret = l2cap_le_conn_req(bthost, conn, hdr->ident,
1785                                                 data + sizeof(*hdr), hdr_len);
1786                 break;
1787
1788         case BT_L2CAP_PDU_LE_CONN_RSP:
1789                 ret = l2cap_le_conn_rsp(bthost, conn, hdr->ident,
1790                                                 data + sizeof(*hdr), hdr_len);
1791                 break;
1792
1793         default:
1794                 printf("Unknown L2CAP code 0x%02x\n", hdr->code);
1795                 ret = false;
1796         }
1797
1798         handle_pending_l2reqs(bthost, conn, hdr->ident, hdr->code,
1799                                                 data + sizeof(*hdr), hdr_len);
1800
1801         if (ret)
1802                 return;
1803
1804 reject:
1805         memset(&rej, 0, sizeof(rej));
1806         l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CMD_REJECT, 0,
1807                                                         &rej, sizeof(rej));
1808 }
1809
1810 static struct cid_hook *find_cid_hook(struct btconn *conn, uint16_t cid)
1811 {
1812         struct cid_hook *hook;
1813
1814         for (hook = conn->cid_hooks; hook != NULL; hook = hook->next) {
1815                 if (hook->cid == cid)
1816                         return hook;
1817         }
1818
1819         return NULL;
1820 }
1821
1822 static struct rfcomm_chan_hook *find_rfcomm_chan_hook(struct btconn *conn,
1823                                                         uint8_t channel)
1824 {
1825         struct rfcomm_chan_hook *hook;
1826
1827         for (hook = conn->rfcomm_chan_hooks; hook != NULL; hook = hook->next)
1828                 if (hook->channel == channel)
1829                         return hook;
1830
1831         return NULL;
1832 }
1833
1834 static void rfcomm_ua_send(struct bthost *bthost, struct btconn *conn,
1835                         struct l2conn *l2conn, uint8_t cr, uint8_t dlci)
1836 {
1837         struct rfcomm_cmd cmd;
1838
1839         cmd.address = RFCOMM_ADDR(cr, dlci);
1840         cmd.control = RFCOMM_CTRL(RFCOMM_UA, 1);
1841         cmd.length = RFCOMM_LEN8(0);
1842         cmd.fcs = rfcomm_fcs2((uint8_t *)&cmd);
1843
1844         send_acl(bthost, conn->handle, l2conn->dcid, &cmd, sizeof(cmd));
1845 }
1846
1847 static void rfcomm_dm_send(struct bthost *bthost, struct btconn *conn,
1848                         struct l2conn *l2conn, uint8_t cr, uint8_t dlci)
1849 {
1850         struct rfcomm_cmd cmd;
1851
1852         cmd.address = RFCOMM_ADDR(cr, dlci);
1853         cmd.control = RFCOMM_CTRL(RFCOMM_DM, 1);
1854         cmd.length = RFCOMM_LEN8(0);
1855         cmd.fcs = rfcomm_fcs2((uint8_t *)&cmd);
1856
1857         send_acl(bthost, conn->handle, l2conn->dcid, &cmd, sizeof(cmd));
1858 }
1859
1860 static void rfcomm_sabm_recv(struct bthost *bthost, struct btconn *conn,
1861                                 struct l2conn *l2conn, const void *data,
1862                                 uint16_t len)
1863 {
1864         const struct rfcomm_cmd *hdr = data;
1865         uint8_t dlci;
1866         struct rfcomm_conn_cb_data *cb;
1867         uint8_t chan;
1868
1869         if (len < sizeof(*hdr))
1870                 return;
1871
1872         chan = RFCOMM_GET_CHANNEL(hdr->address);
1873         dlci = RFCOMM_GET_DLCI(hdr->address);
1874
1875         cb = bthost_find_rfcomm_cb_by_channel(bthost, chan);
1876         if (!dlci || cb) {
1877                 bthost_add_rfcomm_conn(bthost, conn, l2conn, chan);
1878                 rfcomm_ua_send(bthost, conn, l2conn, 1, dlci);
1879                 if (cb && cb->func)
1880                         cb->func(conn->handle, l2conn->scid, cb->user_data,
1881                                                                         true);
1882         } else {
1883                 rfcomm_dm_send(bthost, conn, l2conn, 1, dlci);
1884         }
1885 }
1886
1887 static void rfcomm_disc_recv(struct bthost *bthost, struct btconn *conn,
1888                                 struct l2conn *l2conn, const void *data,
1889                                 uint16_t len)
1890 {
1891         const struct rfcomm_cmd *hdr = data;
1892         uint8_t dlci;
1893
1894         if (len < sizeof(*hdr))
1895                 return;
1896
1897         dlci = RFCOMM_GET_DLCI(hdr->address);
1898
1899         rfcomm_ua_send(bthost, conn, l2conn, 0, dlci);
1900 }
1901
1902 static void rfcomm_uih_send(struct bthost *bthost, struct btconn *conn,
1903                                 struct l2conn *l2conn, uint8_t address,
1904                                 uint8_t type, const void *data, uint16_t len)
1905 {
1906         struct rfcomm_hdr hdr;
1907         struct rfcomm_mcc mcc;
1908         uint8_t fcs;
1909         struct iovec iov[4];
1910
1911         hdr.address = address;
1912         hdr.control = RFCOMM_CTRL(RFCOMM_UIH, 0);
1913         hdr.length  = RFCOMM_LEN8(sizeof(mcc) + len);
1914
1915         iov[0].iov_base = &hdr;
1916         iov[0].iov_len = sizeof(hdr);
1917
1918         mcc.type = type;
1919         mcc.length = RFCOMM_LEN8(len);
1920
1921         iov[1].iov_base = &mcc;
1922         iov[1].iov_len = sizeof(mcc);
1923
1924         iov[2].iov_base = (void *) data;
1925         iov[2].iov_len = len;
1926
1927         fcs = rfcomm_fcs((uint8_t *) &hdr);
1928
1929         iov[3].iov_base = &fcs;
1930         iov[3].iov_len = sizeof(fcs);
1931
1932         send_iov(bthost, conn->handle, l2conn->dcid, iov, 4);
1933 }
1934
1935 static void rfcomm_ua_recv(struct bthost *bthost, struct btconn *conn,
1936                                 struct l2conn *l2conn, const void *data,
1937                                 uint16_t len)
1938 {
1939         const struct rfcomm_cmd *ua_hdr = data;
1940         uint8_t channel;
1941         struct rfcomm_connection_data *conn_data = bthost->rfcomm_conn_data;
1942         uint8_t type;
1943         struct rfcomm_pn pn_cmd;
1944
1945         if (len < sizeof(*ua_hdr))
1946                 return;
1947
1948         channel = RFCOMM_GET_CHANNEL(ua_hdr->address);
1949         type = RFCOMM_GET_TYPE(ua_hdr->control);
1950
1951         if (channel && conn_data && conn_data->channel == channel) {
1952                 bthost_add_rfcomm_conn(bthost, conn, l2conn, channel);
1953                 if (conn_data->cb)
1954                         conn_data->cb(conn->handle, l2conn->scid,
1955                                                 conn_data->user_data, true);
1956                 free(bthost->rfcomm_conn_data);
1957                 bthost->rfcomm_conn_data = NULL;
1958                 return;
1959         }
1960
1961         if (!conn_data || !RFCOMM_TEST_CR(type))
1962                 return;
1963
1964         bthost_add_rfcomm_conn(bthost, conn, l2conn, channel);
1965
1966         pn_cmd.dlci = conn_data->channel * 2;
1967         pn_cmd.priority = 7;
1968         pn_cmd.ack_timer = 0;
1969         pn_cmd.max_retrans = 0;
1970         pn_cmd.mtu = 667;
1971         pn_cmd.credits = 7;
1972
1973         rfcomm_uih_send(bthost, conn, l2conn, RFCOMM_ADDR(1, 0),
1974                         RFCOMM_MCC_TYPE(1, RFCOMM_PN), &pn_cmd, sizeof(pn_cmd));
1975 }
1976
1977 static void rfcomm_dm_recv(struct bthost *bthost, struct btconn *conn,
1978                                 struct l2conn *l2conn, const void *data,
1979                                 uint16_t len)
1980 {
1981         const struct rfcomm_cmd *hdr = data;
1982         uint8_t channel;
1983         struct rfcomm_connection_data *conn_data = bthost->rfcomm_conn_data;
1984
1985         if (len < sizeof(*hdr))
1986                 return;
1987
1988         channel = RFCOMM_GET_CHANNEL(hdr->address);
1989
1990         if (conn_data && conn_data->channel == channel) {
1991                 if (conn_data->cb)
1992                         conn_data->cb(conn->handle, l2conn->scid,
1993                                                 conn_data->user_data, false);
1994                 free(bthost->rfcomm_conn_data);
1995                 bthost->rfcomm_conn_data = NULL;
1996         }
1997 }
1998
1999 static void rfcomm_msc_recv(struct bthost *bthost, struct btconn *conn,
2000                                         struct l2conn *l2conn, uint8_t cr,
2001                                         const struct rfcomm_msc *msc)
2002 {
2003         struct rfcomm_msc msc_cmd;
2004
2005         msc_cmd.dlci = msc->dlci;
2006         msc_cmd.v24_sig = msc->v24_sig;
2007
2008         rfcomm_uih_send(bthost, conn, l2conn, RFCOMM_ADDR(0, 0),
2009                                 RFCOMM_MCC_TYPE(cr, RFCOMM_MSC), &msc_cmd,
2010                                 sizeof(msc_cmd));
2011 }
2012
2013 static void rfcomm_pn_recv(struct bthost *bthost, struct btconn *conn,
2014                                         struct l2conn *l2conn, uint8_t cr,
2015                                         const struct rfcomm_pn *pn)
2016 {
2017         struct rfcomm_pn pn_cmd;
2018
2019         if (!cr) {
2020                 rfcomm_sabm_send(bthost, conn, l2conn, 1,
2021                                         bthost->rfcomm_conn_data->channel * 2);
2022                 return;
2023         }
2024
2025         pn_cmd.dlci = pn->dlci;
2026         pn_cmd.flow_ctrl = pn->flow_ctrl;
2027         pn_cmd.priority = pn->priority;
2028         pn_cmd.ack_timer = pn->ack_timer;
2029         pn_cmd.max_retrans = pn->max_retrans;
2030         pn_cmd.mtu = pn->mtu;
2031         pn_cmd.credits = pn->credits;
2032
2033         rfcomm_uih_send(bthost, conn, l2conn, RFCOMM_ADDR(1, 0),
2034                         RFCOMM_MCC_TYPE(0, RFCOMM_PN), &pn_cmd, sizeof(pn_cmd));
2035 }
2036
2037 static void rfcomm_mcc_recv(struct bthost *bthost, struct btconn *conn,
2038                         struct l2conn *l2conn, const void *data, uint16_t len)
2039 {
2040         const struct rfcomm_mcc *mcc = data;
2041         const struct rfcomm_msc *msc;
2042         const struct rfcomm_pn *pn;
2043
2044         if (len < sizeof(*mcc))
2045                 return;
2046
2047         switch (RFCOMM_GET_MCC_TYPE(mcc->type)) {
2048         case RFCOMM_MSC:
2049                 if (len - sizeof(*mcc) < sizeof(*msc))
2050                         break;
2051
2052                 msc = data + sizeof(*mcc);
2053
2054                 rfcomm_msc_recv(bthost, conn, l2conn,
2055                                 RFCOMM_TEST_CR(mcc->type) / 2, msc);
2056                 break;
2057         case RFCOMM_PN:
2058                 if (len - sizeof(*mcc) < sizeof(*pn))
2059                         break;
2060
2061                 pn = data + sizeof(*mcc);
2062
2063                 rfcomm_pn_recv(bthost, conn, l2conn,
2064                                 RFCOMM_TEST_CR(mcc->type) / 2, pn);
2065                 break;
2066         default:
2067                 break;
2068         }
2069 }
2070
2071 #define GET_LEN8(length)        ((length & 0xfe) >> 1)
2072 #define GET_LEN16(length)       ((length & 0xfffe) >> 1)
2073
2074 static void rfcomm_uih_recv(struct bthost *bthost, struct btconn *conn,
2075                                 struct l2conn *l2conn, const void *data,
2076                                 uint16_t len)
2077 {
2078         const struct rfcomm_hdr *hdr = data;
2079         uint16_t hdr_len, data_len;
2080         const void *p;
2081
2082         if (len < sizeof(*hdr))
2083                 return;
2084
2085         if (RFCOMM_TEST_EA(hdr->length)) {
2086                 data_len = (uint16_t) GET_LEN8(hdr->length);
2087                 hdr_len = sizeof(*hdr);
2088         } else {
2089                 uint8_t ex_len = *((uint8_t *)(data + sizeof(*hdr)));
2090                 data_len = ((uint16_t) hdr->length << 8) | ex_len;
2091                 hdr_len = sizeof(*hdr) + sizeof(uint8_t);
2092         }
2093
2094         if (len < hdr_len + data_len)
2095                 return;
2096
2097         p = data + hdr_len;
2098
2099         if (RFCOMM_GET_DLCI(hdr->address)) {
2100                 struct rfcomm_chan_hook *hook;
2101
2102                 hook = find_rfcomm_chan_hook(conn,
2103                                         RFCOMM_GET_CHANNEL(hdr->address));
2104                 if (hook && data_len)
2105                         hook->func(p, data_len, hook->user_data);
2106         } else {
2107                 rfcomm_mcc_recv(bthost, conn, l2conn, p, data_len);
2108         }
2109 }
2110
2111 static void process_rfcomm(struct bthost *bthost, struct btconn *conn,
2112                                 struct l2conn *l2conn, const void *data,
2113                                 uint16_t len)
2114 {
2115         const struct rfcomm_hdr *hdr = data;
2116
2117         switch (RFCOMM_GET_TYPE(hdr->control)) {
2118         case RFCOMM_SABM:
2119                 rfcomm_sabm_recv(bthost, conn, l2conn, data, len);
2120                 break;
2121         case RFCOMM_DISC:
2122                 rfcomm_disc_recv(bthost, conn, l2conn, data, len);
2123                 break;
2124         case RFCOMM_UA:
2125                 rfcomm_ua_recv(bthost, conn, l2conn, data, len);
2126                 break;
2127         case RFCOMM_DM:
2128                 rfcomm_dm_recv(bthost, conn, l2conn, data, len);
2129                 break;
2130         case RFCOMM_UIH:
2131                 rfcomm_uih_recv(bthost, conn, l2conn, data, len);
2132                 break;
2133         default:
2134                 printf("Unknown frame type\n");
2135                 break;
2136         }
2137 }
2138
2139 static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
2140 {
2141         const struct bt_hci_acl_hdr *acl_hdr = data;
2142         const struct bt_l2cap_hdr *l2_hdr = data + sizeof(*acl_hdr);
2143         uint16_t handle, cid, acl_len, l2_len;
2144         struct cid_hook *hook;
2145         struct btconn *conn;
2146         struct l2conn *l2conn;
2147         const void *l2_data;
2148
2149         if (len < sizeof(*acl_hdr) + sizeof(*l2_hdr))
2150                 return;
2151
2152         acl_len = le16_to_cpu(acl_hdr->dlen);
2153         if (len != sizeof(*acl_hdr) + acl_len)
2154                 return;
2155
2156         handle = acl_handle(acl_hdr->handle);
2157         conn = bthost_find_conn(bthost, handle);
2158         if (!conn) {
2159                 printf("ACL data for unknown handle 0x%04x\n", handle);
2160                 return;
2161         }
2162
2163         l2_len = le16_to_cpu(l2_hdr->len);
2164         if (len - sizeof(*acl_hdr) != sizeof(*l2_hdr) + l2_len)
2165                 return;
2166
2167         l2_data = data + sizeof(*acl_hdr) + sizeof(*l2_hdr);
2168
2169         cid = le16_to_cpu(l2_hdr->cid);
2170
2171         hook = find_cid_hook(conn, cid);
2172         if (hook) {
2173                 hook->func(l2_data, l2_len, hook->user_data);
2174                 return;
2175         }
2176
2177         switch (cid) {
2178         case 0x0001:
2179                 l2cap_sig(bthost, conn, l2_data, l2_len);
2180                 break;
2181         case 0x0005:
2182                 l2cap_le_sig(bthost, conn, l2_data, l2_len);
2183                 break;
2184         case 0x0006:
2185                 smp_data(conn->smp_data, l2_data, l2_len);
2186                 break;
2187         case 0x0007:
2188                 smp_bredr_data(conn->smp_data, l2_data, l2_len);
2189                 break;
2190         default:
2191                 l2conn = btconn_find_l2cap_conn_by_scid(conn, cid);
2192                 if (l2conn && l2conn->psm == 0x0003)
2193                         process_rfcomm(bthost, conn, l2conn, l2_data, l2_len);
2194                 else
2195                         printf("Packet for unknown CID 0x%04x (%u)\n", cid,
2196                                                                         cid);
2197                 break;
2198         }
2199 }
2200
2201 void bthost_receive_h4(struct bthost *bthost, const void *data, uint16_t len)
2202 {
2203         uint8_t pkt_type;
2204
2205         if (!bthost)
2206                 return;
2207
2208         if (len < 1)
2209                 return;
2210
2211         pkt_type = ((const uint8_t *) data)[0];
2212
2213         switch (pkt_type) {
2214         case BT_H4_EVT_PKT:
2215                 process_evt(bthost, data + 1, len - 1);
2216                 break;
2217         case BT_H4_ACL_PKT:
2218                 process_acl(bthost, data + 1, len - 1);
2219                 break;
2220         default:
2221                 printf("Unsupported packet 0x%2.2x\n", pkt_type);
2222                 break;
2223         }
2224 }
2225
2226 void bthost_set_cmd_complete_cb(struct bthost *bthost,
2227                                 bthost_cmd_complete_cb cb, void *user_data)
2228 {
2229         bthost->cmd_complete_cb = cb;
2230         bthost->cmd_complete_data = user_data;
2231 }
2232
2233 void bthost_set_connect_cb(struct bthost *bthost, bthost_new_conn_cb cb,
2234                                                         void *user_data)
2235 {
2236         bthost->new_conn_cb = cb;
2237         bthost->new_conn_data = user_data;
2238 }
2239
2240 void bthost_hci_connect(struct bthost *bthost, const uint8_t *bdaddr,
2241                                                         uint8_t addr_type)
2242 {
2243         bthost->conn_init = true;
2244
2245         if (addr_type == BDADDR_BREDR) {
2246                 struct bt_hci_cmd_create_conn cc;
2247
2248                 memset(&cc, 0, sizeof(cc));
2249                 memcpy(cc.bdaddr, bdaddr, sizeof(cc.bdaddr));
2250
2251                 send_command(bthost, BT_HCI_CMD_CREATE_CONN, &cc, sizeof(cc));
2252         } else {
2253                 struct bt_hci_cmd_le_create_conn cc;
2254
2255                 memset(&cc, 0, sizeof(cc));
2256                 memcpy(cc.peer_addr, bdaddr, sizeof(cc.peer_addr));
2257
2258                 if (addr_type == BDADDR_LE_RANDOM)
2259                         cc.peer_addr_type = 0x01;
2260
2261                 send_command(bthost, BT_HCI_CMD_LE_CREATE_CONN,
2262                                                         &cc, sizeof(cc));
2263         }
2264 }
2265
2266 void bthost_hci_disconnect(struct bthost *bthost, uint16_t handle,
2267                                                                 uint8_t reason)
2268 {
2269         struct bt_hci_cmd_disconnect disc;
2270
2271         disc.handle = cpu_to_le16(handle);
2272         disc.reason = reason;
2273
2274         send_command(bthost, BT_HCI_CMD_DISCONNECT, &disc, sizeof(disc));
2275 }
2276
2277 void bthost_write_scan_enable(struct bthost *bthost, uint8_t scan)
2278 {
2279         send_command(bthost, BT_HCI_CMD_WRITE_SCAN_ENABLE, &scan, 1);
2280 }
2281
2282 void bthost_set_adv_data(struct bthost *bthost, const uint8_t *data,
2283                                                                 uint8_t len)
2284 {
2285         struct bt_hci_cmd_le_set_adv_data adv_cp;
2286
2287         memset(adv_cp.data, 0, 31);
2288
2289         if (len) {
2290                 adv_cp.len = len;
2291                 memcpy(adv_cp.data, data, len);
2292         }
2293
2294         send_command(bthost, BT_HCI_CMD_LE_SET_ADV_DATA, &adv_cp,
2295                                                         sizeof(adv_cp));
2296 }
2297
2298 void bthost_set_adv_enable(struct bthost *bthost, uint8_t enable)
2299 {
2300         struct bt_hci_cmd_le_set_adv_parameters cp;
2301
2302         memset(&cp, 0, sizeof(cp));
2303         send_command(bthost, BT_HCI_CMD_LE_SET_ADV_PARAMETERS,
2304                                                         &cp, sizeof(cp));
2305
2306         send_command(bthost, BT_HCI_CMD_LE_SET_ADV_ENABLE, &enable, 1);
2307 }
2308
2309 void bthost_write_ssp_mode(struct bthost *bthost, uint8_t mode)
2310 {
2311         send_command(bthost, BT_HCI_CMD_WRITE_SIMPLE_PAIRING_MODE, &mode, 1);
2312 }
2313
2314 void bthost_write_le_host_supported(struct bthost *bthost, uint8_t mode)
2315 {
2316         struct bt_hci_cmd_write_le_host_supported cmd;
2317
2318         bthost->le = mode;
2319
2320         memset(&cmd, 0, sizeof(cmd));
2321         cmd.supported = mode;
2322         send_command(bthost, BT_HCI_CMD_WRITE_LE_HOST_SUPPORTED,
2323                                                         &cmd, sizeof(cmd));
2324 }
2325
2326 void bthost_request_auth(struct bthost *bthost, uint16_t handle)
2327 {
2328         struct btconn *conn;
2329
2330         conn = bthost_find_conn(bthost, handle);
2331         if (!conn)
2332                 return;
2333
2334         if (conn->addr_type == BDADDR_BREDR) {
2335                 struct bt_hci_cmd_auth_requested cp;
2336
2337                 cp.handle = cpu_to_le16(handle);
2338                 send_command(bthost, BT_HCI_CMD_AUTH_REQUESTED, &cp, sizeof(cp));
2339         } else {
2340                 uint8_t auth_req = bthost->auth_req;
2341
2342                 if (bthost->sc)
2343                         auth_req |= 0x08;
2344
2345                 smp_pair(conn->smp_data, bthost->io_capability, auth_req);
2346         }
2347 }
2348
2349 void bthost_le_start_encrypt(struct bthost *bthost, uint16_t handle,
2350                                                         const uint8_t ltk[16])
2351 {
2352         struct bt_hci_cmd_le_start_encrypt cmd;
2353
2354         memset(&cmd, 0, sizeof(cmd));
2355         cmd.handle = htobs(handle);
2356         memcpy(cmd.ltk, ltk, 16);
2357
2358         send_command(bthost, BT_HCI_CMD_LE_START_ENCRYPT, &cmd, sizeof(cmd));
2359 }
2360
2361 uint64_t bthost_conn_get_fixed_chan(struct bthost *bthost, uint16_t handle)
2362 {
2363         struct btconn *conn;
2364
2365         conn = bthost_find_conn(bthost, handle);
2366         if (!conn)
2367                 return 0;
2368
2369         return conn->fixed_chan;
2370 }
2371
2372 void bthost_add_l2cap_server(struct bthost *bthost, uint16_t psm,
2373                                 bthost_l2cap_connect_cb func, void *user_data)
2374 {
2375         struct l2cap_conn_cb_data *data;
2376
2377         data = malloc(sizeof(struct l2cap_conn_cb_data));
2378         if (!data)
2379                 return;
2380
2381         data->psm = psm;
2382         data->user_data = user_data;
2383         data->func = func;
2384         data->next = bthost->new_l2cap_conn_data;
2385
2386         bthost->new_l2cap_conn_data = data;
2387 }
2388
2389 void bthost_set_sc_support(struct bthost *bthost, bool enable)
2390 {
2391         struct bt_hci_cmd_write_secure_conn_support cmd;
2392
2393         bthost->sc = enable;
2394
2395         if (!lmp_bredr_capable(bthost))
2396                 return;
2397
2398         cmd.support = enable;
2399         send_command(bthost, BT_HCI_CMD_WRITE_SECURE_CONN_SUPPORT,
2400                                                         &cmd, sizeof(cmd));
2401 }
2402
2403 void bthost_set_pin_code(struct bthost *bthost, const uint8_t *pin,
2404                                                         uint8_t pin_len)
2405 {
2406         memcpy(bthost->pin, pin, pin_len);
2407         bthost->pin_len = pin_len;
2408 }
2409
2410 void bthost_set_io_capability(struct bthost *bthost, uint8_t io_capability)
2411 {
2412         bthost->io_capability = io_capability;
2413 }
2414
2415 uint8_t bthost_get_io_capability(struct bthost *bthost)
2416 {
2417         return bthost->io_capability;
2418 }
2419
2420 void bthost_set_auth_req(struct bthost *bthost, uint8_t auth_req)
2421 {
2422         bthost->auth_req = auth_req;
2423 }
2424
2425 uint8_t bthost_get_auth_req(struct bthost *bthost)
2426 {
2427         uint8_t auth_req = bthost->auth_req;
2428
2429         if (bthost->sc)
2430                 auth_req |= 0x08;
2431
2432         return auth_req;
2433 }
2434
2435 void bthost_set_reject_user_confirm(struct bthost *bthost, bool reject)
2436 {
2437         bthost->reject_user_confirm = reject;
2438 }
2439
2440 bool bthost_get_reject_user_confirm(struct bthost *bthost)
2441 {
2442         return bthost->reject_user_confirm;
2443 }
2444
2445 void bthost_add_rfcomm_server(struct bthost *bthost, uint8_t channel,
2446                                 bthost_rfcomm_connect_cb func, void *user_data)
2447 {
2448         struct rfcomm_conn_cb_data *data;
2449
2450         data = malloc(sizeof(struct rfcomm_conn_cb_data));
2451         if (!data)
2452                 return;
2453
2454         data->channel = channel;
2455         data->user_data = user_data;
2456         data->func = func;
2457         data->next = bthost->new_rfcomm_conn_data;
2458
2459         bthost->new_rfcomm_conn_data = data;
2460 }
2461
2462 void bthost_start(struct bthost *bthost)
2463 {
2464         if (!bthost)
2465                 return;
2466
2467         bthost->ncmd = 1;
2468
2469         send_command(bthost, BT_HCI_CMD_RESET, NULL, 0);
2470
2471         send_command(bthost, BT_HCI_CMD_READ_LOCAL_FEATURES, NULL, 0);
2472         send_command(bthost, BT_HCI_CMD_READ_BD_ADDR, NULL, 0);
2473 }
2474
2475 bool bthost_connect_rfcomm(struct bthost *bthost, uint16_t handle,
2476                                 uint8_t channel, bthost_rfcomm_connect_cb func,
2477                                 void *user_data)
2478 {
2479         struct rfcomm_connection_data *data;
2480         struct bt_l2cap_pdu_conn_req req;
2481         struct btconn *conn;
2482
2483         if (bthost->rfcomm_conn_data)
2484                 return false;
2485
2486         conn = bthost_find_conn(bthost, handle);
2487         if (!conn)
2488                 return false;
2489
2490         data = malloc(sizeof(struct rfcomm_connection_data));
2491         if (!data)
2492                 return false;
2493
2494         data->channel = channel;
2495         data->conn = conn;
2496         data->cb = func;
2497         data->user_data = user_data;
2498
2499         bthost->rfcomm_conn_data = data;
2500
2501         req.psm = cpu_to_le16(0x0003);
2502         req.scid = cpu_to_le16(conn->next_cid++);
2503
2504         return bthost_l2cap_req(bthost, handle, BT_L2CAP_PDU_CONN_REQ,
2505                                         &req, sizeof(req), NULL, NULL);
2506 }
2507
2508 void bthost_add_rfcomm_chan_hook(struct bthost *bthost, uint16_t handle,
2509                                         uint8_t channel,
2510                                         bthost_rfcomm_chan_hook_func_t func,
2511                                         void *user_data)
2512 {
2513         struct rfcomm_chan_hook *hook;
2514         struct btconn *conn;
2515
2516         conn = bthost_find_conn(bthost, handle);
2517         if (!conn)
2518                 return;
2519
2520         hook = malloc(sizeof(*hook));
2521         if (!hook)
2522                 return;
2523
2524         memset(hook, 0, sizeof(*hook));
2525
2526         hook->channel = channel;
2527         hook->func = func;
2528         hook->user_data = user_data;
2529
2530         hook->next = conn->rfcomm_chan_hooks;
2531         conn->rfcomm_chan_hooks = hook;
2532 }
2533
2534 void bthost_send_rfcomm_data(struct bthost *bthost, uint16_t handle,
2535                                         uint8_t channel, const void *data,
2536                                         uint16_t len)
2537 {
2538         struct btconn *conn;
2539         struct rcconn *rcconn;
2540         struct rfcomm_hdr *hdr;
2541         uint8_t *uih_frame;
2542         uint16_t uih_len;
2543
2544         conn = bthost_find_conn(bthost, handle);
2545         if (!conn)
2546                 return;
2547
2548         rcconn = btconn_find_rfcomm_conn_by_channel(conn, channel);
2549         if (!rcconn)
2550                 return;
2551
2552         if (len > 127)
2553                 uih_len = len + sizeof(struct rfcomm_cmd) + sizeof(uint8_t);
2554         else
2555                 uih_len = len + sizeof(struct rfcomm_cmd);
2556
2557         uih_frame = malloc(uih_len);
2558         if (!uih_frame)
2559                 return;
2560
2561         hdr = (struct rfcomm_hdr *) uih_frame;
2562         hdr->address = RFCOMM_ADDR(1, channel * 2);
2563         hdr->control = RFCOMM_CTRL(RFCOMM_UIH, 0);
2564         if (len > 127) {
2565                 hdr->length  = RFCOMM_LEN16(cpu_to_le16(sizeof(*hdr) + len));
2566                 memcpy(uih_frame + sizeof(*hdr) + 1, data, len);
2567         } else {
2568                 hdr->length  = RFCOMM_LEN8(sizeof(*hdr) + len);
2569                 memcpy(uih_frame + sizeof(*hdr), data, len);
2570         }
2571
2572         uih_frame[uih_len - 1] = rfcomm_fcs((void *)hdr);
2573         send_acl(bthost, handle, rcconn->scid, uih_frame, uih_len);
2574
2575         free(uih_frame);
2576 }