diff options
Diffstat (limited to 'emulator')
-rw-r--r-- | emulator/bthost.c | 45 | ||||
-rw-r--r-- | emulator/bthost.h | 3 |
2 files changed, 34 insertions, 14 deletions
diff --git a/emulator/bthost.c b/emulator/bthost.c index 3cce4666c..4671fe17d 100644 --- a/emulator/bthost.c +++ b/emulator/bthost.c @@ -38,6 +38,8 @@ #define acl_flags(h) (h >> 12) #define iso_flags_pb(f) (f & 0x0003) +#define iso_flags_ts(f) ((f >> 2) & 0x0001) +#define iso_flags_pack(pb, ts) (((pb) & 0x03) | (((ts) & 0x01) << 2)) #define iso_data_len_pack(h, f) ((uint16_t) ((h) | ((f) << 14))) #define L2CAP_FEAT_FIXED_CHAN 0x00000080 @@ -728,41 +730,58 @@ 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, +static void send_iso(struct bthost *bthost, uint16_t handle, bool ts, + uint16_t sn, uint32_t timestamp, 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]; + struct iovec pdu[4 + iovcnt]; + uint16_t flags, dlen; 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; + pdu[4 + i].iov_base = iov[i].iov_base; + pdu[4 + 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)); + flags = iso_flags_pack(0x02, ts); + dlen = len + sizeof(data_hdr); + if (ts) + dlen += sizeof(timestamp); + + iso_hdr.handle = acl_handle_pack(handle, flags); + iso_hdr.dlen = cpu_to_le16(dlen); pdu[1].iov_base = &iso_hdr; pdu[1].iov_len = sizeof(iso_hdr); - data_hdr.sn = cpu_to_le16(sn++); + if (ts) { + timestamp = cpu_to_le32(timestamp); + + pdu[2].iov_base = ×tamp; + pdu[2].iov_len = sizeof(timestamp); + } else { + pdu[2].iov_base = NULL; + pdu[2].iov_len = 0; + } + + 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); + pdu[3].iov_base = &data_hdr; + pdu[3].iov_len = sizeof(data_hdr); - send_packet(bthost, pdu, 3 + iovcnt); + send_packet(bthost, pdu, 4 + iovcnt); } -void bthost_send_iso(struct bthost *bthost, uint16_t handle, +void bthost_send_iso(struct bthost *bthost, uint16_t handle, bool ts, + uint16_t sn, uint32_t timestamp, const struct iovec *iov, int iovcnt) { struct btconn *conn; @@ -771,7 +790,7 @@ void bthost_send_iso(struct bthost *bthost, uint16_t handle, if (!conn) return; - send_iso(bthost, handle, iov, iovcnt); + send_iso(bthost, handle, ts, sn, timestamp, iov, iovcnt); } bool bthost_l2cap_req(struct bthost *bthost, uint16_t handle, uint8_t code, diff --git a/emulator/bthost.h b/emulator/bthost.h index c42444476..92182687f 100644 --- a/emulator/bthost.h +++ b/emulator/bthost.h @@ -79,7 +79,8 @@ 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, +void bthost_send_iso(struct bthost *bthost, uint16_t handle, bool ts, + uint16_t sn, uint32_t timestamp, const struct iovec *iov, int iovcnt); typedef void (*bthost_l2cap_rsp_cb) (uint8_t code, const void *data, |