diff --git a/drivers/bluetooth/hci/rpmsg.c b/drivers/bluetooth/hci/rpmsg.c index 786f701922b..4706d2c3b52 100644 --- a/drivers/bluetooth/hci/rpmsg.c +++ b/drivers/bluetooth/hci/rpmsg.c @@ -19,6 +19,7 @@ #define RPMSG_ACL 0x02 #define RPMSG_SCO 0x03 #define RPMSG_EVT 0x04 +#define RPMSG_ISO 0x05 int bt_rpmsg_platform_init(void); int bt_rpmsg_platform_send(struct net_buf *buf); @@ -123,6 +124,40 @@ static struct net_buf *bt_rpmsg_acl_recv(uint8_t *data, size_t remaining) return buf; } +static struct net_buf *bt_rpmsg_iso_recv(uint8_t *data, size_t remaining) +{ + struct bt_hci_iso_hdr hdr; + struct net_buf *buf; + + if (remaining < sizeof(hdr)) { + BT_ERR("Not enough data for ISO header"); + return NULL; + } + + buf = bt_buf_get_rx(BT_BUF_ISO_IN, K_NO_WAIT); + if (buf) { + memcpy((void *)&hdr, data, sizeof(hdr)); + data += sizeof(hdr); + remaining -= sizeof(hdr); + + net_buf_add_mem(buf, &hdr, sizeof(hdr)); + } else { + BT_ERR("No available ISO buffers!"); + return NULL; + } + + if (remaining != sys_le16_to_cpu(hdr.len)) { + BT_ERR("ISO payload length is not correct"); + net_buf_unref(buf); + return NULL; + } + + BT_DBG("len %zu", remaining); + net_buf_add_mem(buf, data, remaining); + + return buf; +} + void bt_rpmsg_rx(uint8_t *data, size_t len) { uint8_t pkt_indicator; @@ -143,6 +178,10 @@ void bt_rpmsg_rx(uint8_t *data, size_t len) buf = bt_rpmsg_acl_recv(data, remaining); break; + case RPMSG_ISO: + buf = bt_rpmsg_iso_recv(data, remaining); + break; + default: BT_ERR("Unknown HCI type %u", pkt_indicator); return; @@ -171,6 +210,9 @@ static int bt_rpmsg_send(struct net_buf *buf) case BT_BUF_CMD: pkt_indicator = RPMSG_CMD; break; + case BT_BUF_ISO_OUT: + pkt_indicator = RPMSG_ISO; + break; default: BT_ERR("Unknown type %u", bt_buf_get_type(buf)); goto done; diff --git a/samples/bluetooth/hci_rpmsg/src/main.c b/samples/bluetooth/hci_rpmsg/src/main.c index e4c9abae7b1..68dfe7abe9f 100644 --- a/samples/bluetooth/hci_rpmsg/src/main.c +++ b/samples/bluetooth/hci_rpmsg/src/main.c @@ -143,6 +143,7 @@ static K_FIFO_DEFINE(tx_queue); #define HCI_RPMSG_ACL 0x02 #define HCI_RPMSG_SCO 0x03 #define HCI_RPMSG_EVT 0x04 +#define HCI_RPMSG_ISO 0x05 static struct net_buf *hci_rpmsg_cmd_recv(uint8_t *data, size_t remaining) { @@ -206,6 +207,37 @@ static struct net_buf *hci_rpmsg_acl_recv(uint8_t *data, size_t remaining) return buf; } +static struct net_buf *hci_rpmsg_iso_recv(uint8_t *data, size_t remaining) +{ + struct bt_hci_iso_hdr *hdr = (void *)data; + struct net_buf *buf; + + if (remaining < sizeof(*hdr)) { + LOG_ERR("Not enough data for ISO header"); + return NULL; + } + + buf = bt_buf_get_tx(BT_BUF_ISO_OUT, K_NO_WAIT, hdr, sizeof(*hdr)); + if (buf) { + data += sizeof(*hdr); + remaining -= sizeof(*hdr); + } else { + LOG_ERR("No available ISO buffers!"); + return NULL; + } + + if (remaining != sys_le16_to_cpu(hdr->len)) { + LOG_ERR("ISO payload length is not correct"); + net_buf_unref(buf); + return NULL; + } + + LOG_DBG("len %zu", remaining); + net_buf_add_mem(buf, data, remaining); + + return buf; +} + static void hci_rpmsg_rx(uint8_t *data, size_t len) { uint8_t pkt_indicator; @@ -226,6 +258,10 @@ static void hci_rpmsg_rx(uint8_t *data, size_t len) buf = hci_rpmsg_acl_recv(data, remaining); break; + case HCI_RPMSG_ISO: + buf = hci_rpmsg_iso_recv(data, remaining); + break; + default: LOG_ERR("Unknown HCI type %u", pkt_indicator); return; @@ -276,6 +312,9 @@ static int hci_rpmsg_send(struct net_buf *buf) case BT_BUF_EVT: pkt_indicator = HCI_RPMSG_EVT; break; + case BT_BUF_ISO_IN: + pkt_indicator = HCI_RPMSG_ISO; + break; default: LOG_ERR("Unknown type %u", bt_buf_get_type(buf)); net_buf_unref(buf);