3 * BlueZ - Bluetooth protocol stack for Linux
5 * Copyright (C) 2011-2012 Intel Corporation
6 * Copyright (C) 2004-2010 Marcel Holtmann <marcel@holtmann.org>
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.
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.
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
36 #include "lib/bluetooth.h"
38 #include "src/shared/util.h"
39 #include "monitor/bt.h"
40 #include "monitor/rfcomm.h"
43 #define lmp_bredr_capable(bthost) (!((bthost)->features[4] & 0x20))
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)
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
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))
63 /* RFCOMM FCS calculation */
64 #define CRC(data) (rfcomm_crc_table[rfcomm_crc_table[0xff ^ data[0]] ^ data[1]])
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,
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,
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,
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,
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,
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,
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,
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
108 static uint8_t rfcomm_fcs2(uint8_t *data)
110 return 0xff - rfcomm_crc_table[CRC(data) ^ data[2]];
113 static uint8_t rfcomm_fcs(uint8_t *data)
115 return 0xff - CRC(data);
121 uint8_t data[256 + sizeof(struct bt_hci_cmd_hdr)];
132 bthost_cid_hook_func_t func;
134 struct cid_hook *next;
137 struct rfcomm_chan_hook {
139 bthost_rfcomm_chan_hook_func_t func;
141 struct rfcomm_chan_hook *next;
151 struct l2conn *l2conns;
152 struct rcconn *rcconns;
153 struct cid_hook *cid_hooks;
154 struct rfcomm_chan_hook *rfcomm_chan_hooks;
172 struct l2cap_pending_req {
174 bthost_l2cap_rsp_cb cb;
176 struct l2cap_pending_req *next;
179 struct l2cap_conn_cb_data {
181 bthost_l2cap_connect_cb func;
183 struct l2cap_conn_cb_data *next;
186 struct rfcomm_conn_cb_data {
188 bthost_rfcomm_connect_cb func;
190 struct rfcomm_conn_cb_data *next;
193 struct rfcomm_connection_data {
196 bthost_rfcomm_connect_cb cb;
202 bthost_ready_cb ready_cb;
205 bthost_send_func send_handler;
207 struct cmd_queue cmd_q;
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;
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;
220 uint8_t io_capability;
222 bool reject_user_confirm;
229 struct bthost *bthost_create(void)
231 struct bthost *bthost;
233 bthost = new0(struct bthost, 1);
237 bthost->smp_data = smp_start(bthost);
238 if (!bthost->smp_data) {
244 bthost->io_capability = 0x03;
249 static void l2conn_free(struct l2conn *conn)
254 static void btconn_free(struct btconn *conn)
257 smp_conn_del(conn->smp_data);
259 while (conn->l2conns) {
260 struct l2conn *l2conn = conn->l2conns;
262 conn->l2conns = l2conn->next;
266 while (conn->cid_hooks) {
267 struct cid_hook *hook = conn->cid_hooks;
269 conn->cid_hooks = hook->next;
273 while (conn->rcconns) {
274 struct rcconn *rcconn = conn->rcconns;
276 conn->rcconns = rcconn->next;
280 while (conn->rfcomm_chan_hooks) {
281 struct rfcomm_chan_hook *hook = conn->rfcomm_chan_hooks;
283 conn->rfcomm_chan_hooks = hook->next;
290 static struct btconn *bthost_find_conn(struct bthost *bthost, uint16_t handle)
294 for (conn = bthost->conns; conn != NULL; conn = conn->next) {
295 if (conn->handle == handle)
302 static struct btconn *bthost_find_conn_by_bdaddr(struct bthost *bthost,
303 const uint8_t *bdaddr)
307 for (conn = bthost->conns; conn != NULL; conn = conn->next) {
308 if (!memcmp(conn->bdaddr, bdaddr, 6))
315 static struct l2conn *bthost_add_l2cap_conn(struct bthost *bthost,
317 uint16_t scid, uint16_t dcid,
320 struct l2conn *l2conn;
322 l2conn = malloc(sizeof(*l2conn));
326 memset(l2conn, 0, sizeof(*l2conn));
332 l2conn->next = conn->l2conns;
333 conn->l2conns = l2conn;
338 static struct rcconn *bthost_add_rfcomm_conn(struct bthost *bthost,
340 struct l2conn *l2conn,
343 struct rcconn *rcconn;
345 rcconn = malloc(sizeof(*rcconn));
349 memset(rcconn, 0, sizeof(*rcconn));
351 rcconn->channel = channel;
352 rcconn->scid = l2conn->scid;
354 rcconn->next = conn->rcconns;
355 conn->rcconns = rcconn;
360 static struct rcconn *btconn_find_rfcomm_conn_by_channel(struct btconn *conn,
363 struct rcconn *rcconn;
365 for (rcconn = conn->rcconns; rcconn != NULL; rcconn = rcconn->next) {
366 if (rcconn->channel == chan)
373 static struct l2conn *btconn_find_l2cap_conn_by_scid(struct btconn *conn,
376 struct l2conn *l2conn;
378 for (l2conn = conn->l2conns; l2conn != NULL; l2conn = l2conn->next) {
379 if (l2conn->scid == scid)
386 static struct l2cap_conn_cb_data *bthost_find_l2cap_cb_by_psm(
387 struct bthost *bthost, uint16_t psm)
389 struct l2cap_conn_cb_data *cb;
391 for (cb = bthost->new_l2cap_conn_data; cb != NULL; cb = cb->next) {
399 static struct rfcomm_conn_cb_data *bthost_find_rfcomm_cb_by_channel(
400 struct bthost *bthost, uint8_t channel)
402 struct rfcomm_conn_cb_data *cb;
404 for (cb = bthost->new_rfcomm_conn_data; cb != NULL; cb = cb->next) {
405 if (cb->channel == channel)
412 void bthost_destroy(struct bthost *bthost)
417 while (bthost->cmd_q.tail) {
418 struct cmd *cmd = bthost->cmd_q.tail;
420 bthost->cmd_q.tail = cmd->prev;
424 while (bthost->conns) {
425 struct btconn *conn = bthost->conns;
427 bthost->conns = conn->next;
431 while (bthost->l2reqs) {
432 struct l2cap_pending_req *req = bthost->l2reqs;
434 bthost->l2reqs = req->next;
435 req->cb(0, NULL, 0, req->user_data);
439 while (bthost->new_l2cap_conn_data) {
440 struct l2cap_conn_cb_data *cb = bthost->new_l2cap_conn_data;
442 bthost->new_l2cap_conn_data = cb->next;
446 while (bthost->new_rfcomm_conn_data) {
447 struct rfcomm_conn_cb_data *cb = bthost->new_rfcomm_conn_data;
449 bthost->new_rfcomm_conn_data = cb->next;
453 if (bthost->rfcomm_conn_data)
454 free(bthost->rfcomm_conn_data);
456 smp_stop(bthost->smp_data);
461 void bthost_set_send_handler(struct bthost *bthost, bthost_send_func handler,
467 bthost->send_handler = handler;
468 bthost->send_data = user_data;
471 static void queue_command(struct bthost *bthost, const struct iovec *iov,
474 struct cmd_queue *cmd_q = &bthost->cmd_q;
478 cmd = malloc(sizeof(*cmd));
482 memset(cmd, 0, sizeof(*cmd));
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;
490 cmd_q->tail->next = cmd;
494 cmd->prev = cmd_q->tail;
498 static void send_packet(struct bthost *bthost, const struct iovec *iov,
501 if (!bthost->send_handler)
504 bthost->send_handler(iov, iovlen, bthost->send_data);
507 static void send_iov(struct bthost *bthost, uint16_t handle, uint16_t cid,
508 const struct iovec *iov, int iovcnt)
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];
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;
522 pdu[0].iov_base = &pkt;
523 pdu[0].iov_len = sizeof(pkt);
525 acl_hdr.handle = acl_handle_pack(handle, 0);
526 acl_hdr.dlen = cpu_to_le16(len + sizeof(l2_hdr));
528 pdu[1].iov_base = &acl_hdr;
529 pdu[1].iov_len = sizeof(acl_hdr);
531 l2_hdr.cid = cpu_to_le16(cid);
532 l2_hdr.len = cpu_to_le16(len);
534 pdu[2].iov_base = &l2_hdr;
535 pdu[2].iov_len = sizeof(l2_hdr);
537 send_packet(bthost, pdu, 3 + iovcnt);
540 static void send_acl(struct bthost *bthost, uint16_t handle, uint16_t cid,
541 const void *data, uint16_t len)
545 iov.iov_base = (void *) data;
548 send_iov(bthost, handle, cid, &iov, 1);
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)
555 static uint8_t next_ident = 1;
556 struct bt_l2cap_hdr_sig hdr;
561 ident = next_ident++;
563 ident = next_ident++;
568 hdr.len = cpu_to_le16(len);
570 iov[0].iov_base = &hdr;
571 iov[0].iov_len = sizeof(hdr);
573 if (conn->addr_type == BDADDR_BREDR)
579 send_iov(bthost, conn->handle, cid, iov, 1);
583 iov[1].iov_base = (void *) data;
584 iov[1].iov_len = len;
586 send_iov(bthost, conn->handle, cid, iov, 2);
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)
594 struct cid_hook *hook;
597 conn = bthost_find_conn(bthost, handle);
601 hook = malloc(sizeof(*hook));
605 memset(hook, 0, sizeof(*hook));
609 hook->user_data = user_data;
611 hook->next = conn->cid_hooks;
612 conn->cid_hooks = hook;
615 void bthost_send_cid(struct bthost *bthost, uint16_t handle, uint16_t cid,
616 const void *data, uint16_t len)
620 conn = bthost_find_conn(bthost, handle);
624 send_acl(bthost, handle, cid, data, len);
627 void bthost_send_cid_v(struct bthost *bthost, uint16_t handle, uint16_t cid,
628 const struct iovec *iov, int iovcnt)
632 conn = bthost_find_conn(bthost, handle);
636 send_iov(bthost, handle, cid, iov, iovcnt);
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)
643 struct l2cap_pending_req *req;
647 conn = bthost_find_conn(bthost, handle);
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;
655 bthost_add_l2cap_conn(bthost, conn, le16_to_cpu(req->scid),
656 le16_to_cpu(req->scid),
657 le16_to_cpu(req->psm));
660 ident = l2cap_sig_send(bthost, conn, code, 0, data, len);
667 req = malloc(sizeof(*req));
671 memset(req, 0, sizeof(*req));
674 req->user_data = user_data;
676 req->next = bthost->l2reqs;
677 bthost->l2reqs = req;
682 static void send_command(struct bthost *bthost, uint16_t opcode,
683 const void *data, uint8_t len)
685 struct bt_hci_cmd_hdr hdr;
686 uint8_t pkt = BT_H4_CMD_PKT;
689 iov[0].iov_base = &pkt;
690 iov[0].iov_len = sizeof(pkt);
692 hdr.opcode = cpu_to_le16(opcode);
695 iov[1].iov_base = &hdr;
696 iov[1].iov_len = sizeof(hdr);
699 iov[2].iov_base = (void *) data;
700 iov[2].iov_len = len;
704 send_packet(bthost, iov, len > 0 ? 3 : 2);
707 queue_command(bthost, iov, len > 0 ? 3 : 2);
711 static void next_cmd(struct bthost *bthost)
713 struct cmd_queue *cmd_q = &bthost->cmd_q;
714 struct cmd *cmd = cmd_q->head;
726 iov.iov_base = cmd->data;
727 iov.iov_len = cmd->len;
729 send_packet(bthost, &iov, 1);
742 static void read_bd_addr_complete(struct bthost *bthost, const void *data,
745 const struct bt_hci_rsp_read_bd_addr *ev = data;
747 if (len < sizeof(*ev))
753 memcpy(bthost->bdaddr, ev->bdaddr, 6);
755 bthost->ready = true;
757 if (bthost->ready_cb) {
759 bthost->ready_cb = NULL;
763 void bthost_notify_ready(struct bthost *bthost, bthost_ready_cb cb)
770 bthost->ready_cb = cb;
773 static void read_local_features_complete(struct bthost *bthost,
774 const void *data, uint8_t len)
776 const struct bt_hci_rsp_read_local_features *ev = data;
778 if (len < sizeof(*ev))
784 memcpy(bthost->features, ev->features, 8);
787 static void evt_cmd_complete(struct bthost *bthost, const void *data,
790 const struct bt_hci_evt_cmd_complete *ev = data;
794 if (len < sizeof(*ev))
797 param = data + sizeof(*ev);
799 bthost->ncmd = ev->ncmd;
801 opcode = le16toh(ev->opcode);
804 case BT_HCI_CMD_RESET:
806 case BT_HCI_CMD_READ_LOCAL_FEATURES:
807 read_local_features_complete(bthost, param, len - sizeof(*ev));
809 case BT_HCI_CMD_READ_BD_ADDR:
810 read_bd_addr_complete(bthost, param, len - sizeof(*ev));
812 case BT_HCI_CMD_WRITE_SCAN_ENABLE:
814 case BT_HCI_CMD_LE_SET_ADV_ENABLE:
816 case BT_HCI_CMD_LE_SET_ADV_PARAMETERS:
818 case BT_HCI_CMD_PIN_CODE_REQUEST_REPLY:
820 case BT_HCI_CMD_PIN_CODE_REQUEST_NEG_REPLY:
822 case BT_HCI_CMD_LINK_KEY_REQUEST_NEG_REPLY:
824 case BT_HCI_CMD_WRITE_SIMPLE_PAIRING_MODE:
826 case BT_HCI_CMD_WRITE_LE_HOST_SUPPORTED:
828 case BT_HCI_CMD_WRITE_SECURE_CONN_SUPPORT:
830 case BT_HCI_CMD_IO_CAPABILITY_REQUEST_REPLY:
832 case BT_HCI_CMD_USER_CONFIRM_REQUEST_REPLY:
834 case BT_HCI_CMD_USER_CONFIRM_REQUEST_NEG_REPLY:
836 case BT_HCI_CMD_LE_LTK_REQ_REPLY:
838 case BT_HCI_CMD_LE_LTK_REQ_NEG_REPLY:
840 case BT_HCI_CMD_LE_SET_ADV_DATA:
843 printf("Unhandled cmd_complete opcode 0x%04x\n", opcode);
847 if (bthost->cmd_complete_cb)
848 bthost->cmd_complete_cb(opcode, 0, param, len - sizeof(*ev),
849 bthost->cmd_complete_data);
854 static void evt_cmd_status(struct bthost *bthost, const void *data,
857 const struct bt_hci_evt_cmd_status *ev = data;
860 if (len < sizeof(*ev))
863 bthost->ncmd = ev->ncmd;
865 opcode = le16toh(ev->opcode);
867 if (ev->status && bthost->cmd_complete_cb)
868 bthost->cmd_complete_cb(opcode, ev->status, NULL, 0,
869 bthost->cmd_complete_data);
874 static void evt_conn_request(struct bthost *bthost, const void *data,
877 const struct bt_hci_evt_conn_request *ev = data;
878 struct bt_hci_cmd_accept_conn_request cmd;
880 if (len < sizeof(*ev))
883 memset(&cmd, 0, sizeof(cmd));
884 memcpy(cmd.bdaddr, ev->bdaddr, sizeof(ev->bdaddr));
886 send_command(bthost, BT_HCI_CMD_ACCEPT_CONN_REQUEST, &cmd,
890 static void init_conn(struct bthost *bthost, uint16_t handle,
891 const uint8_t *bdaddr, uint8_t addr_type)
894 const uint8_t *ia, *ra;
896 conn = malloc(sizeof(*conn));
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;
906 conn->next = bthost->conns;
907 bthost->conns = conn;
909 if (bthost->conn_init) {
917 conn->smp_data = smp_conn_add(bthost->smp_data, handle, ia, ra,
918 addr_type, bthost->conn_init);
920 if (bthost->new_conn_cb)
921 bthost->new_conn_cb(conn->handle, bthost->new_conn_data);
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,
931 static void evt_conn_complete(struct bthost *bthost, const void *data,
934 const struct bt_hci_evt_conn_complete *ev = data;
936 if (len < sizeof(*ev))
942 init_conn(bthost, le16_to_cpu(ev->handle), ev->bdaddr, BDADDR_BREDR);
945 static void evt_disconn_complete(struct bthost *bthost, const void *data,
948 const struct bt_hci_evt_disconnect_complete *ev = data;
949 struct btconn **curr;
952 if (len < sizeof(*ev))
958 handle = le16_to_cpu(ev->handle);
960 for (curr = &bthost->conns; *curr;) {
961 struct btconn *conn = *curr;
963 if (conn->handle == handle) {
972 static void evt_num_completed_packets(struct bthost *bthost, const void *data,
975 const struct bt_hci_evt_num_completed_packets *ev = data;
977 if (len < sizeof(*ev))
981 static void evt_auth_complete(struct bthost *bthost, const void *data,
984 const struct bt_hci_evt_auth_complete *ev = data;
985 struct bt_hci_cmd_set_conn_encrypt cp;
987 if (len < sizeof(*ev))
993 cp.handle = ev->handle;
996 send_command(bthost, BT_HCI_CMD_SET_CONN_ENCRYPT, &cp, sizeof(cp));
999 static void evt_pin_code_request(struct bthost *bthost, const void *data,
1002 const struct bt_hci_evt_pin_code_request *ev = data;
1004 if (len < sizeof(*ev))
1007 if (bthost->pin_len > 0) {
1008 struct bt_hci_cmd_pin_code_request_reply cp;
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);
1015 send_command(bthost, BT_HCI_CMD_PIN_CODE_REQUEST_REPLY,
1018 struct bt_hci_cmd_pin_code_request_neg_reply cp;
1020 memcpy(cp.bdaddr, ev->bdaddr, 6);
1021 send_command(bthost, BT_HCI_CMD_PIN_CODE_REQUEST_NEG_REPLY,
1026 static void evt_link_key_request(struct bthost *bthost, const void *data,
1029 const struct bt_hci_evt_link_key_request *ev = data;
1030 struct bt_hci_cmd_link_key_request_neg_reply cp;
1032 if (len < sizeof(*ev))
1035 memset(&cp, 0, sizeof(cp));
1036 memcpy(cp.bdaddr, ev->bdaddr, 6);
1038 send_command(bthost, BT_HCI_CMD_LINK_KEY_REQUEST_NEG_REPLY,
1042 static void evt_link_key_notify(struct bthost *bthost, const void *data,
1045 const struct bt_hci_evt_link_key_notify *ev = data;
1047 if (len < sizeof(*ev))
1051 static void evt_encrypt_change(struct bthost *bthost, const void *data,
1054 const struct bt_hci_evt_encrypt_change *ev = data;
1055 struct btconn *conn;
1058 if (len < sizeof(*ev))
1061 handle = acl_handle(ev->handle);
1062 conn = bthost_find_conn(bthost, handle);
1069 conn->encr_mode = ev->encr_mode;
1072 smp_conn_encrypted(conn->smp_data, conn->encr_mode);
1075 static void evt_io_cap_response(struct bthost *bthost, const void *data,
1078 const struct bt_hci_evt_io_capability_response *ev = data;
1079 struct btconn *conn;
1081 if (len < sizeof(*ev))
1084 conn = bthost_find_conn_by_bdaddr(bthost, ev->bdaddr);
1089 static void evt_io_cap_request(struct bthost *bthost, const void *data,
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;
1096 if (len < sizeof(*ev))
1099 conn = bthost_find_conn_by_bdaddr(bthost, ev->bdaddr);
1103 memcpy(cp.bdaddr, ev->bdaddr, 6);
1104 cp.capability = bthost->io_capability;
1106 cp.authentication = bthost->auth_req;
1108 send_command(bthost, BT_HCI_CMD_IO_CAPABILITY_REQUEST_REPLY,
1112 static void evt_user_confirm_request(struct bthost *bthost, const void *data,
1115 const struct bt_hci_evt_user_confirm_request *ev = data;
1116 struct btconn *conn;
1118 if (len < sizeof(*ev))
1121 conn = bthost_find_conn_by_bdaddr(bthost, ev->bdaddr);
1125 if (bthost->reject_user_confirm) {
1126 send_command(bthost, BT_HCI_CMD_USER_CONFIRM_REQUEST_NEG_REPLY,
1131 send_command(bthost, BT_HCI_CMD_USER_CONFIRM_REQUEST_REPLY,
1135 static void evt_simple_pairing_complete(struct bthost *bthost, const void *data,
1138 const struct bt_hci_evt_simple_pairing_complete *ev = data;
1140 if (len < sizeof(*ev))
1144 static void evt_le_conn_complete(struct bthost *bthost, const void *data,
1147 const struct bt_hci_evt_le_conn_complete *ev = data;
1150 if (len < sizeof(*ev))
1156 if (ev->peer_addr_type == 0x00)
1157 addr_type = BDADDR_LE_PUBLIC;
1159 addr_type = BDADDR_LE_RANDOM;
1161 init_conn(bthost, le16_to_cpu(ev->handle), ev->peer_addr, addr_type);
1164 static void evt_le_conn_update_complete(struct bthost *bthost, const void *data,
1167 const struct bt_hci_evt_le_conn_update_complete *ev = data;
1169 if (len < sizeof(*ev))
1176 static void evt_le_remote_features_complete(struct bthost *bthost,
1177 const void *data, uint8_t len)
1179 const struct bt_hci_evt_le_remote_features_complete *ev = data;
1181 if (len < sizeof(*ev))
1188 static void evt_le_ltk_request(struct bthost *bthost, const void *data,
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;
1196 struct btconn *conn;
1199 if (len < sizeof(*ev))
1202 handle = acl_handle(ev->handle);
1203 conn = bthost_find_conn(bthost, handle);
1207 rand = le64_to_cpu(ev->rand);
1208 ediv = le16_to_cpu(ev->ediv);
1210 cp.handle = ev->handle;
1212 err = smp_get_ltk(conn->smp_data, rand, ediv, cp.ltk);
1214 send_command(bthost, BT_HCI_CMD_LE_LTK_REQ_NEG_REPLY,
1215 neg_cp, sizeof(*neg_cp));
1217 send_command(bthost, BT_HCI_CMD_LE_LTK_REQ_REPLY, &cp,
1221 static void evt_le_meta_event(struct bthost *bthost, const void *data,
1224 const uint8_t *event = data;
1225 const void *evt_data = data + 1;
1231 case BT_HCI_EVT_LE_CONN_COMPLETE:
1232 evt_le_conn_complete(bthost, evt_data, len - 1);
1234 case BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE:
1235 evt_le_conn_update_complete(bthost, evt_data, len - 1);
1237 case BT_HCI_EVT_LE_REMOTE_FEATURES_COMPLETE:
1238 evt_le_remote_features_complete(bthost, evt_data, len - 1);
1240 case BT_HCI_EVT_LE_LONG_TERM_KEY_REQUEST:
1241 evt_le_ltk_request(bthost, evt_data, len - 1);
1244 printf("Unsupported LE Meta event 0x%2.2x\n", *event);
1249 static void process_evt(struct bthost *bthost, const void *data, uint16_t len)
1251 const struct bt_hci_evt_hdr *hdr = data;
1254 if (len < sizeof(*hdr))
1257 if (sizeof(*hdr) + hdr->plen != len)
1260 param = data + sizeof(*hdr);
1263 case BT_HCI_EVT_CMD_COMPLETE:
1264 evt_cmd_complete(bthost, param, hdr->plen);
1267 case BT_HCI_EVT_CMD_STATUS:
1268 evt_cmd_status(bthost, param, hdr->plen);
1271 case BT_HCI_EVT_CONN_REQUEST:
1272 evt_conn_request(bthost, param, hdr->plen);
1275 case BT_HCI_EVT_CONN_COMPLETE:
1276 evt_conn_complete(bthost, param, hdr->plen);
1279 case BT_HCI_EVT_DISCONNECT_COMPLETE:
1280 evt_disconn_complete(bthost, param, hdr->plen);
1283 case BT_HCI_EVT_NUM_COMPLETED_PACKETS:
1284 evt_num_completed_packets(bthost, param, hdr->plen);
1287 case BT_HCI_EVT_AUTH_COMPLETE:
1288 evt_auth_complete(bthost, param, hdr->plen);
1291 case BT_HCI_EVT_PIN_CODE_REQUEST:
1292 evt_pin_code_request(bthost, param, hdr->plen);
1295 case BT_HCI_EVT_LINK_KEY_REQUEST:
1296 evt_link_key_request(bthost, param, hdr->plen);
1299 case BT_HCI_EVT_LINK_KEY_NOTIFY:
1300 evt_link_key_notify(bthost, param, hdr->plen);
1303 case BT_HCI_EVT_ENCRYPT_CHANGE:
1304 evt_encrypt_change(bthost, param, hdr->plen);
1307 case BT_HCI_EVT_IO_CAPABILITY_RESPONSE:
1308 evt_io_cap_response(bthost, param, hdr->plen);
1311 case BT_HCI_EVT_IO_CAPABILITY_REQUEST:
1312 evt_io_cap_request(bthost, param, hdr->plen);
1315 case BT_HCI_EVT_USER_CONFIRM_REQUEST:
1316 evt_user_confirm_request(bthost, param, hdr->plen);
1319 case BT_HCI_EVT_SIMPLE_PAIRING_COMPLETE:
1320 evt_simple_pairing_complete(bthost, param, hdr->plen);
1323 case BT_HCI_EVT_LE_META_EVENT:
1324 evt_le_meta_event(bthost, param, hdr->plen);
1328 printf("Unsupported event 0x%2.2x\n", hdr->evt);
1333 static bool l2cap_cmd_rej(struct bthost *bthost, struct btconn *conn,
1334 uint8_t ident, const void *data, uint16_t len)
1336 const struct bt_l2cap_pdu_cmd_reject *rsp = data;
1338 if (len < sizeof(*rsp))
1344 static bool l2cap_conn_req(struct bthost *bthost, struct btconn *conn,
1345 uint8_t ident, const void *data, uint16_t len)
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;
1352 if (len < sizeof(*req))
1355 psm = le16_to_cpu(req->psm);
1357 memset(&rsp, 0, sizeof(rsp));
1358 rsp.scid = req->scid;
1360 cb_data = bthost_find_l2cap_cb_by_psm(bthost, psm);
1362 rsp.dcid = rsp.scid;
1364 rsp.result = cpu_to_le16(0x0002); /* PSM Not Supported */
1366 l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONN_RSP, ident, &rsp,
1370 struct bt_l2cap_pdu_config_req conf_req;
1371 struct l2conn *l2conn;
1373 l2conn = bthost_add_l2cap_conn(bthost, conn,
1374 le16_to_cpu(rsp.dcid),
1375 le16_to_cpu(rsp.scid),
1378 memset(&conf_req, 0, sizeof(conf_req));
1379 conf_req.dcid = rsp.scid;
1381 l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONFIG_REQ, 0,
1382 &conf_req, sizeof(conf_req));
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);
1392 static void rfcomm_sabm_send(struct bthost *bthost, struct btconn *conn,
1393 struct l2conn *l2conn, uint8_t cr, uint8_t dlci)
1395 struct rfcomm_cmd cmd;
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);
1402 send_acl(bthost, conn->handle, l2conn->dcid, &cmd, sizeof(cmd));
1405 static bool l2cap_conn_rsp(struct bthost *bthost, struct btconn *conn,
1406 uint8_t ident, const void *data, uint16_t len)
1408 const struct bt_l2cap_pdu_conn_rsp *rsp = data;
1409 struct bt_l2cap_pdu_config_req req;
1410 struct l2conn *l2conn;
1412 if (len < sizeof(*rsp))
1415 l2conn = btconn_find_l2cap_conn_by_scid(conn, le16_to_cpu(rsp->scid));
1417 l2conn->dcid = le16_to_cpu(rsp->dcid);
1424 memset(&req, 0, sizeof(req));
1425 req.dcid = rsp->dcid;
1427 l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONFIG_REQ, 0,
1433 static bool l2cap_config_req(struct bthost *bthost, struct btconn *conn,
1434 uint8_t ident, const void *data, uint16_t len)
1436 const struct bt_l2cap_pdu_config_req *req = data;
1437 struct bt_l2cap_pdu_config_rsp rsp;
1438 struct l2conn *l2conn;
1441 if (len < sizeof(*req))
1444 dcid = le16_to_cpu(req->dcid);
1446 l2conn = btconn_find_l2cap_conn_by_scid(conn, dcid);
1450 memset(&rsp, 0, sizeof(rsp));
1451 rsp.scid = cpu_to_le16(l2conn->dcid);
1452 rsp.flags = req->flags;
1454 l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONFIG_RSP, ident, &rsp,
1460 static bool l2cap_config_rsp(struct bthost *bthost, struct btconn *conn,
1461 uint8_t ident, const void *data, uint16_t len)
1463 const struct bt_l2cap_pdu_config_rsp *rsp = data;
1464 struct l2conn *l2conn;
1466 if (len < sizeof(*rsp))
1469 l2conn = btconn_find_l2cap_conn_by_scid(conn, rsp->scid);
1473 if (l2conn->psm == 0x0003 && !rsp->result && bthost->rfcomm_conn_data)
1474 rfcomm_sabm_send(bthost, conn, l2conn, 1, 0);
1479 static bool l2cap_disconn_req(struct bthost *bthost, struct btconn *conn,
1480 uint8_t ident, const void *data, uint16_t len)
1482 const struct bt_l2cap_pdu_disconn_req *req = data;
1483 struct bt_l2cap_pdu_disconn_rsp rsp;
1485 if (len < sizeof(*req))
1488 memset(&rsp, 0, sizeof(rsp));
1489 rsp.dcid = req->dcid;
1490 rsp.scid = req->scid;
1492 l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_DISCONN_RSP, ident, &rsp,
1498 static bool l2cap_info_req(struct bthost *bthost, struct btconn *conn,
1499 uint8_t ident, const void *data, uint16_t len)
1501 const struct bt_l2cap_pdu_info_req *req = data;
1502 uint64_t fixed_chan;
1505 struct bt_l2cap_pdu_info_rsp *rsp = (void *) buf;
1507 if (len < sizeof(*req))
1510 memset(buf, 0, sizeof(buf));
1511 rsp->type = req->type;
1513 type = le16_to_cpu(req->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);
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));
1532 rsp->result = cpu_to_le16(0x0001); /* Not Supported */
1533 l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_INFO_RSP, ident,
1541 static bool l2cap_info_rsp(struct bthost *bthost, struct btconn *conn,
1542 uint8_t ident, const void *data, uint16_t len)
1544 const struct bt_l2cap_pdu_info_rsp *rsp = data;
1547 if (len < sizeof(*rsp))
1553 type = le16_to_cpu(rsp->type);
1556 case L2CAP_IT_FIXED_CHAN:
1557 if (len < sizeof(*rsp) + 8)
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);
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)
1574 struct l2cap_pending_req **curr;
1576 for (curr = &bthost->l2reqs; *curr != NULL;) {
1577 struct l2cap_pending_req *req = *curr;
1579 if (req->ident != ident) {
1585 req->cb(code, data, len, req->user_data);
1590 static void l2cap_sig(struct bthost *bthost, struct btconn *conn,
1591 const void *data, uint16_t len)
1593 const struct bt_l2cap_hdr_sig *hdr = data;
1594 struct bt_l2cap_pdu_cmd_reject rej;
1598 if (len < sizeof(*hdr))
1601 hdr_len = le16_to_cpu(hdr->len);
1603 if (sizeof(*hdr) + hdr_len != len)
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);
1612 case BT_L2CAP_PDU_CONN_REQ:
1613 ret = l2cap_conn_req(bthost, conn, hdr->ident,
1614 data + sizeof(*hdr), hdr_len);
1617 case BT_L2CAP_PDU_CONN_RSP:
1618 ret = l2cap_conn_rsp(bthost, conn, hdr->ident,
1619 data + sizeof(*hdr), hdr_len);
1622 case BT_L2CAP_PDU_CONFIG_REQ:
1623 ret = l2cap_config_req(bthost, conn, hdr->ident,
1624 data + sizeof(*hdr), hdr_len);
1627 case BT_L2CAP_PDU_CONFIG_RSP:
1628 ret = l2cap_config_rsp(bthost, conn, hdr->ident,
1629 data + sizeof(*hdr), hdr_len);
1632 case BT_L2CAP_PDU_DISCONN_REQ:
1633 ret = l2cap_disconn_req(bthost, conn, hdr->ident,
1634 data + sizeof(*hdr), hdr_len);
1637 case BT_L2CAP_PDU_INFO_REQ:
1638 ret = l2cap_info_req(bthost, conn, hdr->ident,
1639 data + sizeof(*hdr), hdr_len);
1642 case BT_L2CAP_PDU_INFO_RSP:
1643 ret = l2cap_info_rsp(bthost, conn, hdr->ident,
1644 data + sizeof(*hdr), hdr_len);
1648 printf("Unknown L2CAP code 0x%02x\n", hdr->code);
1652 handle_pending_l2reqs(bthost, conn, hdr->ident, hdr->code,
1653 data + sizeof(*hdr), hdr_len);
1659 memset(&rej, 0, sizeof(rej));
1660 l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CMD_REJECT, 0,
1664 static bool l2cap_conn_param_req(struct bthost *bthost, struct btconn *conn,
1665 uint8_t ident, const void *data, uint16_t len)
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;
1671 if (len < sizeof(*req))
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);
1683 send_command(bthost, BT_HCI_CMD_LE_CONN_UPDATE,
1684 &hci_cmd, sizeof(hci_cmd));
1686 memset(&rsp, 0, sizeof(rsp));
1687 l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONN_PARAM_RSP, ident,
1693 static bool l2cap_conn_param_rsp(struct bthost *bthost, struct btconn *conn,
1694 uint8_t ident, const void *data, uint16_t len)
1696 const struct bt_l2cap_pdu_conn_param_req *rsp = data;
1698 if (len < sizeof(*rsp))
1704 static bool l2cap_le_conn_req(struct bthost *bthost, struct btconn *conn,
1705 uint8_t ident, const void *data, uint16_t len)
1707 const struct bt_l2cap_pdu_le_conn_req *req = data;
1708 struct bt_l2cap_pdu_le_conn_rsp rsp;
1711 if (len < sizeof(*req))
1714 psm = le16_to_cpu(req->psm);
1716 memset(&rsp, 0, sizeof(rsp));
1722 if (bthost_find_l2cap_cb_by_psm(bthost, psm))
1723 rsp.dcid = cpu_to_le16(conn->next_cid++);
1725 rsp.result = cpu_to_le16(0x0002); /* PSM Not Supported */
1727 l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_LE_CONN_RSP, ident, &rsp,
1733 static bool l2cap_le_conn_rsp(struct bthost *bthost, struct btconn *conn,
1734 uint8_t ident, const void *data, uint16_t len)
1736 const struct bt_l2cap_pdu_le_conn_rsp *rsp = data;
1738 if (len < sizeof(*rsp))
1740 /* TODO add L2CAP connection before with proper PSM */
1741 bthost_add_l2cap_conn(bthost, conn, 0, le16_to_cpu(rsp->dcid), 0);
1746 static void l2cap_le_sig(struct bthost *bthost, struct btconn *conn,
1747 const void *data, uint16_t len)
1749 const struct bt_l2cap_hdr_sig *hdr = data;
1750 struct bt_l2cap_pdu_cmd_reject rej;
1754 if (len < sizeof(*hdr))
1757 hdr_len = le16_to_cpu(hdr->len);
1759 if (sizeof(*hdr) + hdr_len != len)
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);
1768 case BT_L2CAP_PDU_DISCONN_REQ:
1769 ret = l2cap_disconn_req(bthost, conn, hdr->ident,
1770 data + sizeof(*hdr), hdr_len);
1773 case BT_L2CAP_PDU_CONN_PARAM_REQ:
1774 ret = l2cap_conn_param_req(bthost, conn, hdr->ident,
1775 data + sizeof(*hdr), hdr_len);
1778 case BT_L2CAP_PDU_CONN_PARAM_RSP:
1779 ret = l2cap_conn_param_rsp(bthost, conn, hdr->ident,
1780 data + sizeof(*hdr), hdr_len);
1783 case BT_L2CAP_PDU_LE_CONN_REQ:
1784 ret = l2cap_le_conn_req(bthost, conn, hdr->ident,
1785 data + sizeof(*hdr), hdr_len);
1788 case BT_L2CAP_PDU_LE_CONN_RSP:
1789 ret = l2cap_le_conn_rsp(bthost, conn, hdr->ident,
1790 data + sizeof(*hdr), hdr_len);
1794 printf("Unknown L2CAP code 0x%02x\n", hdr->code);
1798 handle_pending_l2reqs(bthost, conn, hdr->ident, hdr->code,
1799 data + sizeof(*hdr), hdr_len);
1805 memset(&rej, 0, sizeof(rej));
1806 l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CMD_REJECT, 0,
1810 static struct cid_hook *find_cid_hook(struct btconn *conn, uint16_t cid)
1812 struct cid_hook *hook;
1814 for (hook = conn->cid_hooks; hook != NULL; hook = hook->next) {
1815 if (hook->cid == cid)
1822 static struct rfcomm_chan_hook *find_rfcomm_chan_hook(struct btconn *conn,
1825 struct rfcomm_chan_hook *hook;
1827 for (hook = conn->rfcomm_chan_hooks; hook != NULL; hook = hook->next)
1828 if (hook->channel == channel)
1834 static void rfcomm_ua_send(struct bthost *bthost, struct btconn *conn,
1835 struct l2conn *l2conn, uint8_t cr, uint8_t dlci)
1837 struct rfcomm_cmd cmd;
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);
1844 send_acl(bthost, conn->handle, l2conn->dcid, &cmd, sizeof(cmd));
1847 static void rfcomm_dm_send(struct bthost *bthost, struct btconn *conn,
1848 struct l2conn *l2conn, uint8_t cr, uint8_t dlci)
1850 struct rfcomm_cmd cmd;
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);
1857 send_acl(bthost, conn->handle, l2conn->dcid, &cmd, sizeof(cmd));
1860 static void rfcomm_sabm_recv(struct bthost *bthost, struct btconn *conn,
1861 struct l2conn *l2conn, const void *data,
1864 const struct rfcomm_cmd *hdr = data;
1866 struct rfcomm_conn_cb_data *cb;
1869 if (len < sizeof(*hdr))
1872 chan = RFCOMM_GET_CHANNEL(hdr->address);
1873 dlci = RFCOMM_GET_DLCI(hdr->address);
1875 cb = bthost_find_rfcomm_cb_by_channel(bthost, chan);
1877 bthost_add_rfcomm_conn(bthost, conn, l2conn, chan);
1878 rfcomm_ua_send(bthost, conn, l2conn, 1, dlci);
1880 cb->func(conn->handle, l2conn->scid, cb->user_data,
1883 rfcomm_dm_send(bthost, conn, l2conn, 1, dlci);
1887 static void rfcomm_disc_recv(struct bthost *bthost, struct btconn *conn,
1888 struct l2conn *l2conn, const void *data,
1891 const struct rfcomm_cmd *hdr = data;
1894 if (len < sizeof(*hdr))
1897 dlci = RFCOMM_GET_DLCI(hdr->address);
1899 rfcomm_ua_send(bthost, conn, l2conn, 0, dlci);
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)
1906 struct rfcomm_hdr hdr;
1907 struct rfcomm_mcc mcc;
1909 struct iovec iov[4];
1911 hdr.address = address;
1912 hdr.control = RFCOMM_CTRL(RFCOMM_UIH, 0);
1913 hdr.length = RFCOMM_LEN8(sizeof(mcc) + len);
1915 iov[0].iov_base = &hdr;
1916 iov[0].iov_len = sizeof(hdr);
1919 mcc.length = RFCOMM_LEN8(len);
1921 iov[1].iov_base = &mcc;
1922 iov[1].iov_len = sizeof(mcc);
1924 iov[2].iov_base = (void *) data;
1925 iov[2].iov_len = len;
1927 fcs = rfcomm_fcs((uint8_t *) &hdr);
1929 iov[3].iov_base = &fcs;
1930 iov[3].iov_len = sizeof(fcs);
1932 send_iov(bthost, conn->handle, l2conn->dcid, iov, 4);
1935 static void rfcomm_ua_recv(struct bthost *bthost, struct btconn *conn,
1936 struct l2conn *l2conn, const void *data,
1939 const struct rfcomm_cmd *ua_hdr = data;
1941 struct rfcomm_connection_data *conn_data = bthost->rfcomm_conn_data;
1943 struct rfcomm_pn pn_cmd;
1945 if (len < sizeof(*ua_hdr))
1948 channel = RFCOMM_GET_CHANNEL(ua_hdr->address);
1949 type = RFCOMM_GET_TYPE(ua_hdr->control);
1951 if (channel && conn_data && conn_data->channel == channel) {
1952 bthost_add_rfcomm_conn(bthost, conn, l2conn, channel);
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;
1961 if (!conn_data || !RFCOMM_TEST_CR(type))
1964 bthost_add_rfcomm_conn(bthost, conn, l2conn, channel);
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;
1973 rfcomm_uih_send(bthost, conn, l2conn, RFCOMM_ADDR(1, 0),
1974 RFCOMM_MCC_TYPE(1, RFCOMM_PN), &pn_cmd, sizeof(pn_cmd));
1977 static void rfcomm_dm_recv(struct bthost *bthost, struct btconn *conn,
1978 struct l2conn *l2conn, const void *data,
1981 const struct rfcomm_cmd *hdr = data;
1983 struct rfcomm_connection_data *conn_data = bthost->rfcomm_conn_data;
1985 if (len < sizeof(*hdr))
1988 channel = RFCOMM_GET_CHANNEL(hdr->address);
1990 if (conn_data && conn_data->channel == channel) {
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;
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)
2003 struct rfcomm_msc msc_cmd;
2005 msc_cmd.dlci = msc->dlci;
2006 msc_cmd.v24_sig = msc->v24_sig;
2008 rfcomm_uih_send(bthost, conn, l2conn, RFCOMM_ADDR(0, 0),
2009 RFCOMM_MCC_TYPE(cr, RFCOMM_MSC), &msc_cmd,
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)
2017 struct rfcomm_pn pn_cmd;
2020 rfcomm_sabm_send(bthost, conn, l2conn, 1,
2021 bthost->rfcomm_conn_data->channel * 2);
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;
2033 rfcomm_uih_send(bthost, conn, l2conn, RFCOMM_ADDR(1, 0),
2034 RFCOMM_MCC_TYPE(0, RFCOMM_PN), &pn_cmd, sizeof(pn_cmd));
2037 static void rfcomm_mcc_recv(struct bthost *bthost, struct btconn *conn,
2038 struct l2conn *l2conn, const void *data, uint16_t len)
2040 const struct rfcomm_mcc *mcc = data;
2041 const struct rfcomm_msc *msc;
2042 const struct rfcomm_pn *pn;
2044 if (len < sizeof(*mcc))
2047 switch (RFCOMM_GET_MCC_TYPE(mcc->type)) {
2049 if (len - sizeof(*mcc) < sizeof(*msc))
2052 msc = data + sizeof(*mcc);
2054 rfcomm_msc_recv(bthost, conn, l2conn,
2055 RFCOMM_TEST_CR(mcc->type) / 2, msc);
2058 if (len - sizeof(*mcc) < sizeof(*pn))
2061 pn = data + sizeof(*mcc);
2063 rfcomm_pn_recv(bthost, conn, l2conn,
2064 RFCOMM_TEST_CR(mcc->type) / 2, pn);
2071 #define GET_LEN8(length) ((length & 0xfe) >> 1)
2072 #define GET_LEN16(length) ((length & 0xfffe) >> 1)
2074 static void rfcomm_uih_recv(struct bthost *bthost, struct btconn *conn,
2075 struct l2conn *l2conn, const void *data,
2078 const struct rfcomm_hdr *hdr = data;
2079 uint16_t hdr_len, data_len;
2082 if (len < sizeof(*hdr))
2085 if (RFCOMM_TEST_EA(hdr->length)) {
2086 data_len = (uint16_t) GET_LEN8(hdr->length);
2087 hdr_len = sizeof(*hdr);
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);
2094 if (len < hdr_len + data_len)
2099 if (RFCOMM_GET_DLCI(hdr->address)) {
2100 struct rfcomm_chan_hook *hook;
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);
2107 rfcomm_mcc_recv(bthost, conn, l2conn, p, data_len);
2111 static void process_rfcomm(struct bthost *bthost, struct btconn *conn,
2112 struct l2conn *l2conn, const void *data,
2115 const struct rfcomm_hdr *hdr = data;
2117 switch (RFCOMM_GET_TYPE(hdr->control)) {
2119 rfcomm_sabm_recv(bthost, conn, l2conn, data, len);
2122 rfcomm_disc_recv(bthost, conn, l2conn, data, len);
2125 rfcomm_ua_recv(bthost, conn, l2conn, data, len);
2128 rfcomm_dm_recv(bthost, conn, l2conn, data, len);
2131 rfcomm_uih_recv(bthost, conn, l2conn, data, len);
2134 printf("Unknown frame type\n");
2139 static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
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;
2149 if (len < sizeof(*acl_hdr) + sizeof(*l2_hdr))
2152 acl_len = le16_to_cpu(acl_hdr->dlen);
2153 if (len != sizeof(*acl_hdr) + acl_len)
2156 handle = acl_handle(acl_hdr->handle);
2157 conn = bthost_find_conn(bthost, handle);
2159 printf("ACL data for unknown handle 0x%04x\n", handle);
2163 l2_len = le16_to_cpu(l2_hdr->len);
2164 if (len - sizeof(*acl_hdr) != sizeof(*l2_hdr) + l2_len)
2167 l2_data = data + sizeof(*acl_hdr) + sizeof(*l2_hdr);
2169 cid = le16_to_cpu(l2_hdr->cid);
2171 hook = find_cid_hook(conn, cid);
2173 hook->func(l2_data, l2_len, hook->user_data);
2179 l2cap_sig(bthost, conn, l2_data, l2_len);
2182 l2cap_le_sig(bthost, conn, l2_data, l2_len);
2185 smp_data(conn->smp_data, l2_data, l2_len);
2188 smp_bredr_data(conn->smp_data, l2_data, l2_len);
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);
2195 printf("Packet for unknown CID 0x%04x (%u)\n", cid,
2201 void bthost_receive_h4(struct bthost *bthost, const void *data, uint16_t len)
2211 pkt_type = ((const uint8_t *) data)[0];
2215 process_evt(bthost, data + 1, len - 1);
2218 process_acl(bthost, data + 1, len - 1);
2221 printf("Unsupported packet 0x%2.2x\n", pkt_type);
2226 void bthost_set_cmd_complete_cb(struct bthost *bthost,
2227 bthost_cmd_complete_cb cb, void *user_data)
2229 bthost->cmd_complete_cb = cb;
2230 bthost->cmd_complete_data = user_data;
2233 void bthost_set_connect_cb(struct bthost *bthost, bthost_new_conn_cb cb,
2236 bthost->new_conn_cb = cb;
2237 bthost->new_conn_data = user_data;
2240 void bthost_hci_connect(struct bthost *bthost, const uint8_t *bdaddr,
2243 bthost->conn_init = true;
2245 if (addr_type == BDADDR_BREDR) {
2246 struct bt_hci_cmd_create_conn cc;
2248 memset(&cc, 0, sizeof(cc));
2249 memcpy(cc.bdaddr, bdaddr, sizeof(cc.bdaddr));
2251 send_command(bthost, BT_HCI_CMD_CREATE_CONN, &cc, sizeof(cc));
2253 struct bt_hci_cmd_le_create_conn cc;
2255 memset(&cc, 0, sizeof(cc));
2256 memcpy(cc.peer_addr, bdaddr, sizeof(cc.peer_addr));
2258 if (addr_type == BDADDR_LE_RANDOM)
2259 cc.peer_addr_type = 0x01;
2261 cc.scan_interval = cpu_to_le16(0x0060);
2262 cc.scan_window = cpu_to_le16(0x0030);
2263 cc.min_interval = cpu_to_le16(0x0028);
2264 cc.max_interval = cpu_to_le16(0x0038);
2265 cc.supv_timeout = cpu_to_le16(0x002a);
2267 send_command(bthost, BT_HCI_CMD_LE_CREATE_CONN,
2272 void bthost_hci_disconnect(struct bthost *bthost, uint16_t handle,
2275 struct bt_hci_cmd_disconnect disc;
2277 disc.handle = cpu_to_le16(handle);
2278 disc.reason = reason;
2280 send_command(bthost, BT_HCI_CMD_DISCONNECT, &disc, sizeof(disc));
2283 void bthost_write_scan_enable(struct bthost *bthost, uint8_t scan)
2285 send_command(bthost, BT_HCI_CMD_WRITE_SCAN_ENABLE, &scan, 1);
2288 void bthost_set_adv_data(struct bthost *bthost, const uint8_t *data,
2291 struct bt_hci_cmd_le_set_adv_data adv_cp;
2293 memset(adv_cp.data, 0, 31);
2297 memcpy(adv_cp.data, data, len);
2300 send_command(bthost, BT_HCI_CMD_LE_SET_ADV_DATA, &adv_cp,
2304 void bthost_set_adv_enable(struct bthost *bthost, uint8_t enable)
2306 struct bt_hci_cmd_le_set_adv_parameters cp;
2308 memset(&cp, 0, sizeof(cp));
2309 send_command(bthost, BT_HCI_CMD_LE_SET_ADV_PARAMETERS,
2312 send_command(bthost, BT_HCI_CMD_LE_SET_ADV_ENABLE, &enable, 1);
2315 void bthost_write_ssp_mode(struct bthost *bthost, uint8_t mode)
2317 send_command(bthost, BT_HCI_CMD_WRITE_SIMPLE_PAIRING_MODE, &mode, 1);
2320 void bthost_write_le_host_supported(struct bthost *bthost, uint8_t mode)
2322 struct bt_hci_cmd_write_le_host_supported cmd;
2326 memset(&cmd, 0, sizeof(cmd));
2327 cmd.supported = mode;
2328 send_command(bthost, BT_HCI_CMD_WRITE_LE_HOST_SUPPORTED,
2332 void bthost_request_auth(struct bthost *bthost, uint16_t handle)
2334 struct btconn *conn;
2336 conn = bthost_find_conn(bthost, handle);
2340 if (conn->addr_type == BDADDR_BREDR) {
2341 struct bt_hci_cmd_auth_requested cp;
2343 cp.handle = cpu_to_le16(handle);
2344 send_command(bthost, BT_HCI_CMD_AUTH_REQUESTED, &cp, sizeof(cp));
2346 uint8_t auth_req = bthost->auth_req;
2351 smp_pair(conn->smp_data, bthost->io_capability, auth_req);
2355 void bthost_le_start_encrypt(struct bthost *bthost, uint16_t handle,
2356 const uint8_t ltk[16])
2358 struct bt_hci_cmd_le_start_encrypt cmd;
2360 memset(&cmd, 0, sizeof(cmd));
2361 cmd.handle = htobs(handle);
2362 memcpy(cmd.ltk, ltk, 16);
2364 send_command(bthost, BT_HCI_CMD_LE_START_ENCRYPT, &cmd, sizeof(cmd));
2367 uint64_t bthost_conn_get_fixed_chan(struct bthost *bthost, uint16_t handle)
2369 struct btconn *conn;
2371 conn = bthost_find_conn(bthost, handle);
2375 return conn->fixed_chan;
2378 void bthost_add_l2cap_server(struct bthost *bthost, uint16_t psm,
2379 bthost_l2cap_connect_cb func, void *user_data)
2381 struct l2cap_conn_cb_data *data;
2383 data = malloc(sizeof(struct l2cap_conn_cb_data));
2388 data->user_data = user_data;
2390 data->next = bthost->new_l2cap_conn_data;
2392 bthost->new_l2cap_conn_data = data;
2395 void bthost_set_sc_support(struct bthost *bthost, bool enable)
2397 struct bt_hci_cmd_write_secure_conn_support cmd;
2399 bthost->sc = enable;
2401 if (!lmp_bredr_capable(bthost))
2404 cmd.support = enable;
2405 send_command(bthost, BT_HCI_CMD_WRITE_SECURE_CONN_SUPPORT,
2409 void bthost_set_pin_code(struct bthost *bthost, const uint8_t *pin,
2412 memcpy(bthost->pin, pin, pin_len);
2413 bthost->pin_len = pin_len;
2416 void bthost_set_io_capability(struct bthost *bthost, uint8_t io_capability)
2418 bthost->io_capability = io_capability;
2421 uint8_t bthost_get_io_capability(struct bthost *bthost)
2423 return bthost->io_capability;
2426 void bthost_set_auth_req(struct bthost *bthost, uint8_t auth_req)
2428 bthost->auth_req = auth_req;
2431 uint8_t bthost_get_auth_req(struct bthost *bthost)
2433 uint8_t auth_req = bthost->auth_req;
2441 void bthost_set_reject_user_confirm(struct bthost *bthost, bool reject)
2443 bthost->reject_user_confirm = reject;
2446 bool bthost_get_reject_user_confirm(struct bthost *bthost)
2448 return bthost->reject_user_confirm;
2451 void bthost_add_rfcomm_server(struct bthost *bthost, uint8_t channel,
2452 bthost_rfcomm_connect_cb func, void *user_data)
2454 struct rfcomm_conn_cb_data *data;
2456 data = malloc(sizeof(struct rfcomm_conn_cb_data));
2460 data->channel = channel;
2461 data->user_data = user_data;
2463 data->next = bthost->new_rfcomm_conn_data;
2465 bthost->new_rfcomm_conn_data = data;
2468 void bthost_start(struct bthost *bthost)
2475 send_command(bthost, BT_HCI_CMD_RESET, NULL, 0);
2477 send_command(bthost, BT_HCI_CMD_READ_LOCAL_FEATURES, NULL, 0);
2478 send_command(bthost, BT_HCI_CMD_READ_BD_ADDR, NULL, 0);
2481 bool bthost_connect_rfcomm(struct bthost *bthost, uint16_t handle,
2482 uint8_t channel, bthost_rfcomm_connect_cb func,
2485 struct rfcomm_connection_data *data;
2486 struct bt_l2cap_pdu_conn_req req;
2487 struct btconn *conn;
2489 if (bthost->rfcomm_conn_data)
2492 conn = bthost_find_conn(bthost, handle);
2496 data = malloc(sizeof(struct rfcomm_connection_data));
2500 data->channel = channel;
2503 data->user_data = user_data;
2505 bthost->rfcomm_conn_data = data;
2507 req.psm = cpu_to_le16(0x0003);
2508 req.scid = cpu_to_le16(conn->next_cid++);
2510 return bthost_l2cap_req(bthost, handle, BT_L2CAP_PDU_CONN_REQ,
2511 &req, sizeof(req), NULL, NULL);
2514 void bthost_add_rfcomm_chan_hook(struct bthost *bthost, uint16_t handle,
2516 bthost_rfcomm_chan_hook_func_t func,
2519 struct rfcomm_chan_hook *hook;
2520 struct btconn *conn;
2522 conn = bthost_find_conn(bthost, handle);
2526 hook = malloc(sizeof(*hook));
2530 memset(hook, 0, sizeof(*hook));
2532 hook->channel = channel;
2534 hook->user_data = user_data;
2536 hook->next = conn->rfcomm_chan_hooks;
2537 conn->rfcomm_chan_hooks = hook;
2540 void bthost_send_rfcomm_data(struct bthost *bthost, uint16_t handle,
2541 uint8_t channel, const void *data,
2544 struct btconn *conn;
2545 struct rcconn *rcconn;
2546 struct rfcomm_hdr *hdr;
2550 conn = bthost_find_conn(bthost, handle);
2554 rcconn = btconn_find_rfcomm_conn_by_channel(conn, channel);
2559 uih_len = len + sizeof(struct rfcomm_cmd) + sizeof(uint8_t);
2561 uih_len = len + sizeof(struct rfcomm_cmd);
2563 uih_frame = malloc(uih_len);
2567 hdr = (struct rfcomm_hdr *) uih_frame;
2568 hdr->address = RFCOMM_ADDR(1, channel * 2);
2569 hdr->control = RFCOMM_CTRL(RFCOMM_UIH, 0);
2571 hdr->length = RFCOMM_LEN16(cpu_to_le16(sizeof(*hdr) + len));
2572 memcpy(uih_frame + sizeof(*hdr) + 1, data, len);
2574 hdr->length = RFCOMM_LEN8(sizeof(*hdr) + len);
2575 memcpy(uih_frame + sizeof(*hdr), data, len);
2578 uih_frame[uih_len - 1] = rfcomm_fcs((void *)hdr);
2579 send_acl(bthost, handle, rcconn->scid, uih_frame, uih_len);