Support fragmentation to controller MTU when sending ISO packets.
By default, fragment when exceeding btdev iso_mtu.
---
emulator/bthost.c | 102 +++++++++++++++++++++++++++++++---------------
emulator/bthost.h | 1 +
emulator/hciemu.c | 7 ++--
3 files changed, 75 insertions(+), 35 deletions(-)
diff --git a/emulator/bthost.c b/emulator/bthost.c
index 7ab4533d1..52c5b0bed 100644
--- a/emulator/bthost.c
+++ b/emulator/bthost.c
@@ -257,6 +257,7 @@ struct bthost {
bthost_new_conn_cb new_iso_cb;
void *new_iso_data;
uint16_t acl_mtu;
+ uint16_t iso_mtu;
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;
@@ -297,6 +298,7 @@ struct bthost *bthost_create(void)
/* Set defaults */
bthost->io_capability = 0x03;
bthost->acl_mtu = UINT16_MAX;
+ bthost->iso_mtu = UINT16_MAX;
return bthost;
}
@@ -583,6 +585,14 @@ void bthost_set_acl_mtu(struct bthost *bthost, uint16_t mtu)
bthost->acl_mtu = mtu;
}
+void bthost_set_iso_mtu(struct bthost *bthost, uint16_t mtu)
+{
+ if (!bthost)
+ return;
+
+ bthost->iso_mtu = mtu;
+}
+
static void queue_command(struct bthost *bthost, const struct iovec *iov,
int iovlen)
{
@@ -912,52 +922,80 @@ void bthost_send_sco(struct bthost *bthost, uint16_t handle, uint8_t pkt_status,
static void send_iso(struct bthost *bthost, uint16_t handle, bool ts,
uint16_t sn, uint32_t timestamp, uint8_t pkt_status,
- const struct iovec *iov, int iovcnt)
+ const struct iovec *iov, unsigned 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[4 + iovcnt];
- uint16_t flags, dlen;
- int i, len = 0;
+ struct iovec payload[2 + iovcnt];
+ uint16_t payload_mtu = bthost->iso_mtu - sizeof(iso_hdr);
+ int len = 0, packet = 0;
+ unsigned int i;
for (i = 0; i < iovcnt; i++) {
- pdu[4 + i].iov_base = iov[i].iov_base;
- pdu[4 + i].iov_len = iov[i].iov_len;
+ payload[2 + i].iov_base = iov[i].iov_base;
+ payload[2 + i].iov_len = iov[i].iov_len;
len += iov[i].iov_len;
}
- pdu[0].iov_base = &pkt;
- pdu[0].iov_len = sizeof(pkt);
-
- 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);
-
- 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, pkt_status));
- pdu[3].iov_base = &data_hdr;
- pdu[3].iov_len = sizeof(data_hdr);
+ if (ts) {
+ timestamp = cpu_to_le32(timestamp);
- send_packet(bthost, pdu, 4 + iovcnt);
+ payload[0].iov_base = ×tamp;
+ payload[0].iov_len = sizeof(timestamp);
+ len += sizeof(timestamp);
+ } else {
+ payload[0].iov_base = NULL;
+ payload[0].iov_len = 0;
+ }
+ iovcnt++;
+
+ payload[1].iov_base = &data_hdr;
+ payload[1].iov_len = sizeof(data_hdr);
+ len += sizeof(data_hdr);
+ iovcnt++;
+
+ /* ISO fragmentation */
+
+ do {
+ unsigned int pdu_iovcnt;
+ uint16_t iso_len, pb, flags;
+
+ if (packet == 0 && len <= payload_mtu)
+ pb = 0x02;
+ else if (packet == 0)
+ pb = 0x00;
+ else if (len <= payload_mtu)
+ pb = 0x03;
+ else
+ pb = 0x01;
+
+ flags = iso_flags_pack(pb, ts);
+ iso_len = len <= payload_mtu ? len : payload_mtu;
+
+ iso_hdr.handle = acl_handle_pack(handle, flags);
+ iso_hdr.dlen = cpu_to_le16(iso_len);
+
+ pdu[0].iov_base = &pkt;
+ pdu[0].iov_len = sizeof(pkt);
+
+ pdu[1].iov_base = &iso_hdr;
+ pdu[1].iov_len = sizeof(iso_hdr);
+
+ iov_pull_n(payload, &iovcnt, &pdu[2], &pdu_iovcnt,
+ ARRAY_SIZE(pdu) - 2, iso_len);
+ pdu_iovcnt += 2;
+
+ send_packet(bthost, pdu, pdu_iovcnt);
+
+ packet++;
+ ts = false;
+ len -= iso_len;
+ } while (len);
}
void bthost_send_iso(struct bthost *bthost, uint16_t handle, bool ts,
diff --git a/emulator/bthost.h b/emulator/bthost.h
index 456f631d5..0d8eca530 100644
--- a/emulator/bthost.h
+++ b/emulator/bthost.h
@@ -37,6 +37,7 @@ void bthost_set_send_handler(struct bthost *bthost, bthost_send_func handler,
void *user_data);
void bthost_set_acl_mtu(struct bthost *bthost, uint16_t mtu);
+void bthost_set_iso_mtu(struct bthost *bthost, uint16_t mtu);
void bthost_receive_h4(struct bthost *bthost, const void *data, uint16_t len);
diff --git a/emulator/hciemu.c b/emulator/hciemu.c
index 8529caae8..a59773618 100644
--- a/emulator/hciemu.c
+++ b/emulator/hciemu.c
@@ -308,7 +308,7 @@ static struct hciemu_client *hciemu_client_new(struct hciemu *hciemu,
{
struct hciemu_client *client;
int sv[2];
- uint16_t mtu;
+ uint16_t acl_mtu, iso_mtu;
client = new0(struct hciemu_client, 1);
if (!client)
@@ -343,8 +343,9 @@ static struct hciemu_client *hciemu_client_new(struct hciemu *hciemu,
client->host_source = create_source_bthost(sv[1], client->host);
client->start_source = g_idle_add(start_host, client);
- btdev_get_mtu(client->dev, &mtu, NULL, NULL);
- bthost_set_acl_mtu(client->host, mtu);
+ btdev_get_mtu(client->dev, &acl_mtu, NULL, &iso_mtu);
+ bthost_set_acl_mtu(client->host, acl_mtu);
+ bthost_set_iso_mtu(client->host, iso_mtu);
return client;
}
--
2.50.1
