bthost: Add support for BT_H4_ISO_PKT

This adds sending and receiving BT_H4_ISO_PKT packets.

Signed-off-by: Manika Shrivastava <manika.sh@samsung.com>
Signed-off-by: Ayush Garg <ayush.garg@samsung.com>

diff --git a/emulator/bthost.c b/emulator/bthost.c
index f949033..b43eb59 100755 (executable)
--- a/emulator/bthost.c
+++ b/emulator/bthost.c
@@ -36,6 +36,9 @@
 #define acl_handle(h)          (h & 0x0fff)
 #define acl_flags(h)           (h >> 12)
 
+#define iso_flags_pb(f)                (f & 0x0003)
+#define iso_data_len_pack(h, f)        ((uint16_t) ((h) | ((f) << 14)))
+
 #define L2CAP_FEAT_FIXED_CHAN  0x00000080
 #define L2CAP_FC_SIG_BREDR     0x02
 #define L2CAP_FC_SMP_BREDR     0x80
@@ -130,6 +133,11 @@ struct rfcomm_chan_hook {
        struct rfcomm_chan_hook *next;
 };
 
+struct iso_hook {
+       bthost_cid_hook_func_t func;
+       void *user_data;
+};
+
 struct btconn {
        uint16_t handle;
        uint8_t bdaddr[6];
@@ -141,8 +149,12 @@ struct btconn {
        struct rcconn *rcconns;
        struct cid_hook *cid_hooks;
        struct rfcomm_chan_hook *rfcomm_chan_hooks;
+       struct iso_hook *iso_hook;
        struct btconn *next;
        void *smp_data;
+       uint16_t recv_len;
+       uint16_t data_len;
+       void *recv_data;
 };
 
 struct l2conn {
@@ -202,9 +214,6 @@ struct bthost {
        uint8_t bdaddr[6];
        uint8_t features[8];
        bthost_send_func send_handler;
-       uint16_t acl_len;
-       uint16_t l2_len;
-       void *acl_data;
        void *send_data;
        struct cmd_queue cmd_q;
        uint8_t ncmd;
@@ -213,6 +222,8 @@ struct bthost {
        void *cmd_complete_data;
        bthost_new_conn_cb new_conn_cb;
        void *new_conn_data;
+       bthost_new_conn_cb new_iso_cb;
+       void *new_iso_data;
        struct rfcomm_connection_data *rfcomm_conn_data;
        struct l2cap_conn_cb_data *new_l2cap_conn_data;
        struct rfcomm_conn_cb_data *new_rfcomm_conn_data;
@@ -294,6 +305,8 @@ static void btconn_free(struct btconn *conn)
                free(hook);
        }
 
+       free(conn->iso_hook);
+       free(conn->recv_data);
        free(conn);
 }
 
@@ -493,7 +506,6 @@ void bthost_destroy(struct bthost *bthost)
 
        queue_destroy(bthost->le_ext_adv, le_ext_adv_free);
 
-       free(bthost->acl_data);
        free(bthost);
 }
 
@@ -662,6 +674,28 @@ void bthost_add_cid_hook(struct bthost *bthost, uint16_t handle, uint16_t cid,
        conn->cid_hooks = hook;
 }
 
+void bthost_add_iso_hook(struct bthost *bthost, uint16_t handle,
+                               bthost_cid_hook_func_t func, void *user_data)
+{
+       struct iso_hook *hook;
+       struct btconn *conn;
+
+       conn = bthost_find_conn(bthost, handle);
+       if (!conn || conn->iso_hook)
+               return;
+
+       hook = malloc(sizeof(*hook));
+       if (!hook)
+               return;
+
+       memset(hook, 0, sizeof(*hook));
+
+       hook->func = func;
+       hook->user_data = user_data;
+
+       conn->iso_hook = hook;
+}
+
 void bthost_send_cid(struct bthost *bthost, uint16_t handle, uint16_t cid,
                                        const void *data, uint16_t len)
 {
@@ -686,6 +720,52 @@ void bthost_send_cid_v(struct bthost *bthost, uint16_t handle, uint16_t cid,
        send_iov(bthost, handle, cid, iov, iovcnt);
 }
 
+static void send_iso(struct bthost *bthost, uint16_t handle,
+                                       const struct iovec *iov, int iovcnt)
+{
+       struct bt_hci_iso_hdr iso_hdr;
+       struct bt_hci_iso_data_start data_hdr;
+       uint8_t pkt = BT_H4_ISO_PKT;
+       struct iovec pdu[3 + iovcnt];
+       int i, len = 0;
+       static uint16_t sn;
+
+       for (i = 0; i < iovcnt; i++) {
+               pdu[3 + i].iov_base = iov[i].iov_base;
+               pdu[3 + i].iov_len = iov[i].iov_len;
+               len += iov[i].iov_len;
+       }
+
+       pdu[0].iov_base = &pkt;
+       pdu[0].iov_len = sizeof(pkt);
+
+       iso_hdr.handle = acl_handle_pack(handle, 0x02);
+       iso_hdr.dlen = cpu_to_le16(len + sizeof(data_hdr));
+
+       pdu[1].iov_base = &iso_hdr;
+       pdu[1].iov_len = sizeof(iso_hdr);
+
+       data_hdr.sn = cpu_to_le16(sn++);
+       data_hdr.slen = cpu_to_le16(iso_data_len_pack(len, 0));
+
+       pdu[2].iov_base = &data_hdr;
+       pdu[2].iov_len = sizeof(data_hdr);
+
+       send_packet(bthost, pdu, 3 + iovcnt);
+}
+
+void bthost_send_iso(struct bthost *bthost, uint16_t handle,
+                                       const struct iovec *iov, int iovcnt)
+{
+       struct btconn *conn;
+
+       conn = bthost_find_conn(bthost, handle);
+       if (!conn)
+               return;
+
+       send_iso(bthost, handle, iov, iovcnt);
+}
+
 bool bthost_l2cap_req(struct bthost *bthost, uint16_t handle, uint8_t code,
                                const void *data, uint16_t len,
                                bthost_l2cap_rsp_cb cb, void *user_data)
@@ -1336,6 +1416,40 @@ static void evt_le_ltk_request(struct bthost *bthost, const void *data,
                                                                sizeof(cp));
 }
 
+static void init_iso(struct bthost *bthost, uint16_t handle,
+                               const uint8_t *bdaddr, uint8_t addr_type)
+{
+       struct btconn *conn;
+
+       bthost_debug(bthost, "ISO handle 0x%4.4x", handle);
+
+       conn = malloc(sizeof(*conn));
+       if (!conn)
+               return;
+
+       memset(conn, 0, sizeof(*conn));
+       conn->handle = handle;
+       memcpy(conn->bdaddr, bdaddr, 6);
+       conn->addr_type = addr_type;
+
+       conn->next = bthost->conns;
+       bthost->conns = conn;
+
+       if (bthost->new_iso_cb)
+               bthost->new_iso_cb(handle, bthost->new_iso_data);
+}
+
+static void evt_le_cis_established(struct bthost *bthost, const void *data,
+                                                       uint8_t size)
+{
+       const struct bt_hci_evt_le_cis_established *ev = data;
+
+       if (ev->status)
+               return;
+
+       init_iso(bthost, ev->conn_handle, BDADDR_ANY->b, BDADDR_LE_PUBLIC);
+}
+
 static void evt_le_cis_req(struct bthost *bthost, const void *data, uint8_t len)
 {
        const struct bt_hci_evt_le_cis_req *ev = data;
@@ -1383,6 +1497,38 @@ static void evt_le_ext_adv_report(struct bthost *bthost, const void *data,
        }
 }
 
+static void evt_le_big_complete(struct bthost *bthost, const void *data,
+                                                       uint8_t size)
+{
+       const struct bt_hci_evt_le_big_complete *ev = data;
+       int i;
+
+       if (ev->status)
+               return;
+
+       for (i = 0; i < ev->num_bis; i++) {
+               uint16_t handle = le16_to_cpu(ev->bis_handle[i]);
+
+               init_iso(bthost, handle, BDADDR_ANY->b, BDADDR_LE_PUBLIC);
+       }
+}
+
+static void evt_le_big_sync_established(struct bthost *bthost,
+                                               const void *data, uint8_t size)
+{
+       const struct bt_hci_evt_le_big_sync_estabilished *ev = data;
+       int i;
+
+       if (ev->status)
+               return;
+
+       for (i = 0; i < ev->num_bis; i++) {
+               uint16_t handle = le16_to_cpu(ev->bis[i]);
+
+               init_iso(bthost, handle, BDADDR_ANY->b, BDADDR_LE_PUBLIC);
+       }
+}
+
 static void evt_le_meta_event(struct bthost *bthost, const void *data,
                                                                uint8_t len)
 {
@@ -1392,7 +1538,7 @@ static void evt_le_meta_event(struct bthost *bthost, const void *data,
        if (len < 1)
                return;
 
-       bthost_debug(bthost, "event 0x%02x", *event);
+       bthost_debug(bthost, "meta event 0x%02x", *event);
 
        switch (*event) {
        case BT_HCI_EVT_LE_CONN_COMPLETE:
@@ -1413,9 +1559,18 @@ static void evt_le_meta_event(struct bthost *bthost, const void *data,
        case BT_HCI_EVT_LE_EXT_ADV_REPORT:
                evt_le_ext_adv_report(bthost, evt_data, len - 1);
                break;
+       case BT_HCI_EVT_LE_CIS_ESTABLISHED:
+               evt_le_cis_established(bthost, evt_data, len - 1);
+               break;
        case BT_HCI_EVT_LE_CIS_REQ:
                evt_le_cis_req(bthost, evt_data, len - 1);
                break;
+       case BT_HCI_EVT_LE_BIG_COMPLETE:
+               evt_le_big_complete(bthost, evt_data, len - 1);
+               break;
+       case BT_HCI_EVT_LE_BIG_SYNC_ESTABILISHED:
+               evt_le_big_sync_established(bthost, evt_data, len - 1);
+               break;
        default:
                bthost_debug(bthost, "Unsupported LE Meta event 0x%2.2x",
                                                                *event);
@@ -2436,22 +2591,14 @@ static void process_rfcomm(struct bthost *bthost, struct btconn *conn,
        }
 }
 
-static void process_l2cap(struct bthost *bthost, uint16_t handle,
+static void process_l2cap(struct bthost *bthost, struct btconn *conn,
                                        const void *data, uint16_t len)
 {
        const struct bt_l2cap_hdr *l2_hdr = data;
        struct cid_hook *hook;
-       struct btconn *conn;
        struct l2conn *l2conn;
        uint16_t cid, l2_len;
 
-       conn = bthost_find_conn(bthost, handle);
-       if (!conn) {
-               bthost_debug(bthost, "ACL data for unknown handle 0x%04x",
-                                                               handle);
-               return;
-       }
-
        l2_len = le16_to_cpu(l2_hdr->len);
        if (len != sizeof(*l2_hdr) + l2_len) {
                bthost_debug(bthost, "L2CAP invalid length: %u != %zu",
@@ -2495,36 +2642,55 @@ static void process_l2cap(struct bthost *bthost, uint16_t handle,
        }
 }
 
-static void append_acl_data(struct bthost *bthost, uint16_t handle,
-                               uint8_t flags, const void *data, uint16_t len)
+static void append_recv_data(struct bthost *bthost, struct btconn *conn,
+                               const char *type, uint8_t flags,
+                               const void *data, uint16_t len)
 {
-       if (!bthost->acl_data) {
-               bthost_debug(bthost, "Unexpected ACL frame: handle 0x%4.4x "
-                               "flags 0x%2.2x", handle, flags);
+       if (!conn->recv_data) {
+               bthost_debug(bthost, "Unexpected %s frame: handle 0x%4.4x "
+                               "flags 0x%2.2x", type, conn->handle, flags);
                return;
        }
 
-       if (bthost->acl_len + len > bthost->l2_len) {
-               bthost_debug(bthost, "Unexpected ACL frame: handle 0x%4.4x "
-                               "flags 0x%2.2x", handle, flags);
+       if (conn->recv_len + len > conn->data_len) {
+               bthost_debug(bthost, "Unexpected %s frame: handle 0x%4.4x "
+                               "flags 0x%2.2x", type, conn->handle, flags);
                return;
        }
 
-       memcpy(bthost->acl_data + bthost->acl_len, data, len);
-       bthost->acl_len += len;
+       memcpy(conn->recv_data + conn->recv_len, data, len);
+       conn->recv_len += len;
 
-       bthost_debug(bthost, "ACL data: %u/%u bytes", bthost->acl_len,
-                                                       bthost->l2_len);
+       bthost_debug(bthost, "%s data: %u/%u bytes", type, conn->recv_len,
+                                                       conn->data_len);
+}
 
-       if (bthost->acl_len < bthost->l2_len)
+static void free_recv_data(struct btconn *conn)
+{
+       free(conn->recv_data);
+       conn->recv_data = NULL;
+       conn->recv_len = 0;
+       conn->data_len = 0;
+}
+
+static void append_acl_data(struct bthost *bthost, struct btconn *conn,
+                               uint8_t flags, const void *data, uint16_t len)
+{
+       append_recv_data(bthost, conn, "ACL", flags, data, len);
+
+       if (conn->recv_len < conn->data_len)
                return;
 
-       process_l2cap(bthost, handle, bthost->acl_data, bthost->acl_len);
+       process_l2cap(bthost, conn, conn->recv_data, conn->recv_len);
 
-       free(bthost->acl_data);
-       bthost->acl_data = NULL;
-       bthost->acl_len = 0;
-       bthost->l2_len = 0;
+       free_recv_data(conn);
+}
+
+static void new_recv_data(struct btconn *conn, uint16_t len)
+{
+       conn->recv_data = malloc(len);
+       conn->recv_len = 0;
+       conn->data_len = len;
 }
 
 static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
@@ -2533,6 +2699,7 @@ static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
        const struct bt_l2cap_hdr *l2_hdr = (void *) acl_hdr->data;
        uint16_t handle, acl_len, l2_len;
        uint8_t flags;
+       struct btconn *conn;
 
        acl_len = le16_to_cpu(acl_hdr->dlen);
        if (len != sizeof(*acl_hdr) + acl_len)
@@ -2541,14 +2708,18 @@ static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
        handle = acl_handle(acl_hdr->handle);
        flags = acl_flags(acl_hdr->handle);
 
+       conn = bthost_find_conn(bthost, handle);
+       if (!conn) {
+               bthost_debug(bthost, "Unknown handle: 0x%4.4x", handle);
+               return;
+       }
+
        switch (flags) {
        case 0x00:      /* start of a non-automatically-flushable PDU */
        case 0x02:      /* start of an automatically-flushable PDU */
-               if (bthost->acl_data) {
+               if (conn->recv_data) {
                        bthost_debug(bthost, "Unexpected ACL start frame");
-                       free(bthost->acl_data);
-                       bthost->acl_data = NULL;
-                       bthost->acl_len = 0;
+                       free_recv_data(conn);
                }
 
                l2_len = le16_to_cpu(l2_hdr->len) + sizeof(*l2_hdr);
@@ -2556,25 +2727,116 @@ static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
                bthost_debug(bthost, "acl_len %u l2_len %u", acl_len, l2_len);
 
                if (acl_len == l2_len) {
-                       process_l2cap(bthost, handle, acl_hdr->data, acl_len);
+                       process_l2cap(bthost, conn, acl_hdr->data, acl_len);
                        break;
                }
 
-               bthost->acl_data = malloc(l2_len);
-               bthost->acl_len = 0;
-               bthost->l2_len = l2_len;
+               new_recv_data(conn, l2_len);
                /* fall through */
        case 0x01:      /* continuing fragment */
-               append_acl_data(bthost, handle, flags, acl_hdr->data, acl_len);
+               append_acl_data(bthost, conn, flags, acl_hdr->data, acl_len);
                break;
        case 0x03:      /* complete automatically-flushable PDU */
-               process_l2cap(bthost, handle, acl_hdr->data, acl_len);
+               process_l2cap(bthost, conn, acl_hdr->data, acl_len);
                break;
        default:
                bthost_debug(bthost, "Invalid ACL frame flags 0x%2.2x", flags);
        }
 }
 
+static void process_iso_data(struct bthost *bthost, struct btconn *conn,
+                                       const void *data, uint16_t len)
+{
+       const struct bt_hci_iso_data_start *data_hdr = data;
+       uint16_t data_len;
+       struct iso_hook *hook;
+
+       data_len = le16_to_cpu(data_hdr->slen);
+       if (len != sizeof(*data_hdr) + data_len) {
+               bthost_debug(bthost, "ISO invalid length: %u != %zu",
+                                       len, sizeof(*data_hdr) + data_len);
+               return;
+       }
+
+       bthost_debug(bthost, "ISO data: %u bytes (%u)", data_len, data_hdr->sn);
+
+       hook = conn->iso_hook;
+       if (!hook)
+               return;
+
+       hook->func(data_hdr->data, data_len, hook->user_data);
+}
+
+static void append_iso_data(struct bthost *bthost, struct btconn *conn,
+                               uint8_t flags, const void *data, uint16_t len)
+{
+       append_recv_data(bthost, conn, "ISO", flags, data, len);
+
+       if (conn->recv_len < conn->data_len) {
+               if (flags == 0x03) {
+                       bthost_debug(bthost, "Unexpected ISO end frame");
+                       free_recv_data(conn);
+               }
+               return;
+       }
+
+       process_iso_data(bthost, conn, conn->recv_data, conn->recv_len);
+
+       free_recv_data(conn);
+}
+
+static void process_iso(struct bthost *bthost, const void *data, uint16_t len)
+{
+       const struct bt_hci_iso_hdr *iso_hdr = data;
+       const struct bt_hci_iso_data_start *data_hdr;
+       uint16_t handle, iso_len, data_len;
+       uint8_t flags;
+       struct btconn *conn;
+
+       iso_len = le16_to_cpu(iso_hdr->dlen);
+       if (len != sizeof(*iso_hdr) + iso_len)
+               return;
+
+       handle = acl_handle(iso_hdr->handle);
+       flags = iso_flags_pb(acl_flags(iso_hdr->handle));
+
+       conn = bthost_find_conn(bthost, handle);
+       if (!conn) {
+               bthost_debug(bthost, "Unknown handle: 0x%4.4x", handle);
+               return;
+       }
+
+       data_hdr = (void *) data + sizeof(*iso_hdr);
+
+       switch (flags) {
+       case 0x00:
+       case 0x02:
+               if (conn->recv_data) {
+                       bthost_debug(bthost, "Unexpected ISO start frame");
+                       free_recv_data(conn);
+               }
+
+               data_len = le16_to_cpu(data_hdr->slen) + sizeof(*data_hdr);
+
+               bthost_debug(bthost, "iso_len %u data_len %u", iso_len,
+                                                               data_len);
+
+               if (iso_len == data_len) {
+                       process_iso_data(bthost, conn, iso_hdr->data, iso_len);
+                       break;
+               }
+
+               new_recv_data(conn, data_len);
+               /* fall through */
+       case 0x01:
+       case 0x03:
+               append_iso_data(bthost, conn, flags, iso_hdr->data, iso_len);
+               break;
+       default:
+               bthost_debug(bthost, "Invalid ISO frame flags 0x%2.2x", flags);
+       }
+}
+
 void bthost_receive_h4(struct bthost *bthost, const void *data, uint16_t len)
 {
        uint8_t pkt_type;
@@ -2597,6 +2859,9 @@ void bthost_receive_h4(struct bthost *bthost, const void *data, uint16_t len)
        case BT_H4_ACL_PKT:
                process_acl(bthost, data + 1, len - 1);
                break;
+       case BT_H4_ISO_PKT:
+               process_iso(bthost, data + 1, len - 1);
+               break;
        default:
                bthost_debug(bthost, "Unsupported packet 0x%2.2x", pkt_type);
                break;
@@ -2617,6 +2882,13 @@ void bthost_set_connect_cb(struct bthost *bthost, bthost_new_conn_cb cb,
        bthost->new_conn_data = user_data;
 }
 
+void bthost_set_iso_cb(struct bthost *bthost, bthost_new_conn_cb cb,
+                                                       void *user_data)
+{
+       bthost->new_iso_cb = cb;
+       bthost->new_iso_data = user_data;
+}
+
 void bthost_hci_connect(struct bthost *bthost, const uint8_t *bdaddr,
                                                        uint8_t addr_type)
 {

diff --git a/emulator/bthost.h b/emulator/bthost.h
index 868af54..f597d75 100755 (executable)
--- a/emulator/bthost.h
+++ b/emulator/bthost.h
@@ -47,6 +47,9 @@ typedef void (*bthost_new_conn_cb) (uint16_t handle, void *user_data);
 void bthost_set_connect_cb(struct bthost *bthost, bthost_new_conn_cb cb,
                                                        void *user_data);
 
+void bthost_set_iso_cb(struct bthost *bthost, bthost_new_conn_cb cb,
+                                                       void *user_data);
+
 void bthost_hci_connect(struct bthost *bthost, const uint8_t *bdaddr,
                                                        uint8_t addr_type);
 
@@ -62,10 +65,18 @@ typedef void (*bthost_cid_hook_func_t)(const void *data, uint16_t len,
 void bthost_add_cid_hook(struct bthost *bthost, uint16_t handle, uint16_t cid,
                                bthost_cid_hook_func_t func, void *user_data);
 
+typedef void (*bthost_iso_hook_func_t)(const void *data, uint16_t len,
+                                                       void *user_data);
+
+void bthost_add_iso_hook(struct bthost *bthost, uint16_t handle,
+                               bthost_iso_hook_func_t func, void *user_data);
+
 void bthost_send_cid(struct bthost *bthost, uint16_t handle, uint16_t cid,
                                        const void *data, uint16_t len);
 void bthost_send_cid_v(struct bthost *bthost, uint16_t handle, uint16_t cid,
                                        const struct iovec *iov, int iovcnt);
+void bthost_send_iso(struct bthost *bthost, uint16_t handle,
+                                       const struct iovec *iov, int iovcnt);
 
 typedef void (*bthost_l2cap_rsp_cb) (uint8_t code, const void *data,
                                                uint16_t len, void *user_data);

diff --git a/emulator/hciemu.c b/emulator/hciemu.c
index 541409a..8bfaf38 100755 (executable)
--- a/emulator/hciemu.c
+++ b/emulator/hciemu.c
@@ -189,6 +189,7 @@ static gboolean receive_btdev(GIOChannel *channel, GIOCondition condition,
        case BT_H4_CMD_PKT:
        case BT_H4_ACL_PKT:
        case BT_H4_SCO_PKT:
+       case BT_H4_ISO_PKT:
                btdev_receive_h4(btdev, buf, len);
                break;
        }

diff --git a/monitor/bt.h b/monitor/bt.h
index 098abb7..297ff41 100755 (executable)
--- a/monitor/bt.h
+++ b/monitor/bt.h
@@ -499,11 +499,13 @@ struct bt_hci_sco_hdr {
 struct bt_hci_iso_hdr {
        uint16_t handle;
        uint16_t dlen;
+       uint8_t  data[];
 } __attribute__ ((packed));
 
 struct bt_hci_iso_data_start {
        uint16_t sn;
        uint16_t slen;
+       uint8_t  data[];
 } __attribute__ ((packed));
 
 struct bt_hci_evt_hdr {