From 306bc0ca7620c0f723ebd276fb999aaff9a9d14b Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Fri, 4 Mar 2022 17:22:23 -0800 Subject: btdev: Send BT_HCI_EVT_LE_PER_SYNC_ESTABLISHED if remote start pa This sends BT_HCI_EVT_LE_PER_SYNC_ESTABLISHED if remote device start to periodic advertise when BT_HCI_CMD_LE_PERIODIC_ADV_CREATE_SYNC is pending. --- emulator/btdev.c | 144 ++++++++++++++++++++++++++++++------------------------- 1 file changed, 78 insertions(+), 66 deletions(-) (limited to 'emulator') diff --git a/emulator/btdev.c b/emulator/btdev.c index 59014b535..e79b0dc67 100644 --- a/emulator/btdev.c +++ b/emulator/btdev.c @@ -5044,11 +5044,77 @@ static int cmd_set_per_adv_data(struct btdev *dev, const void *data, return 0; } +static void send_per_adv(struct btdev *dev, const struct btdev *remote, + uint8_t offset) +{ + struct __packed { + struct bt_hci_le_per_adv_report ev; + uint8_t data[31]; + } pdu; + + memset(&pdu.ev, 0, sizeof(pdu.ev)); + pdu.ev.handle = cpu_to_le16(dev->le_periodic_sync_handle); + pdu.ev.tx_power = 127; + pdu.ev.rssi = 127; + pdu.ev.cte_type = 0x0ff; + + if ((size_t) remote->le_periodic_data_len - offset > sizeof(pdu.data)) { + pdu.ev.data_status = 0x01; + pdu.ev.data_len = sizeof(pdu.data); + } else { + pdu.ev.data_status = 0x00; + pdu.ev.data_len = remote->le_periodic_data_len - offset; + } + + memcpy(pdu.data, remote->le_periodic_data + offset, pdu.ev.data_len); + + le_meta_event(dev, BT_HCI_EVT_LE_PER_ADV_REPORT, &pdu, + sizeof(pdu.ev) + pdu.ev.data_len); + + if (pdu.ev.data_status == 0x01) { + offset += pdu.ev.data_len; + send_per_adv(dev, remote, offset); + } +} + +static void le_per_adv_sync_estabilished(struct btdev *dev, + struct btdev *remote, uint8_t status) +{ + struct bt_hci_evt_le_per_sync_established ev; + struct bt_hci_cmd_le_periodic_adv_create_sync *cmd = &dev->pa_sync_cmd; + + memset(&ev, 0, sizeof(ev)); + ev.status = status; + + if (status) { + memset(&dev->pa_sync_cmd, 0, sizeof(dev->pa_sync_cmd)); + dev->le_periodic_sync_handle = 0x0000; + le_meta_event(dev, BT_HCI_EVT_LE_PER_SYNC_ESTABLISHED, &ev, + sizeof(ev)); + return; + } + + dev->le_periodic_sync_handle = SYC_HANDLE; + + ev.handle = cpu_to_le16(dev->le_periodic_sync_handle); + ev.addr_type = cmd->addr_type; + memcpy(ev.addr, cmd->addr, sizeof(ev.addr)); + ev.phy = 0x01; + ev.interval = remote->le_periodic_min_interval; + ev.clock_accuracy = 0x07; + + memset(&dev->pa_sync_cmd, 0, sizeof(dev->pa_sync_cmd)); + + le_meta_event(dev, BT_HCI_EVT_LE_PER_SYNC_ESTABLISHED, &ev, sizeof(ev)); + send_per_adv(dev, remote, 0); +} + static int cmd_set_per_adv_enable(struct btdev *dev, const void *data, uint8_t len) { const struct bt_hci_cmd_le_set_periodic_adv_enable *cmd = data; uint8_t status; + int i; if (dev->le_periodic_adv_enable == cmd->enable) { status = BT_HCI_ERR_COMMAND_DISALLOWED; @@ -5060,6 +5126,18 @@ static int cmd_set_per_adv_enable(struct btdev *dev, const void *data, cmd_complete(dev, BT_HCI_CMD_LE_SET_PERIODIC_ADV_ENABLE, &status, sizeof(status)); + for (i = 0; i < MAX_BTDEV_ENTRIES; i++) { + struct btdev *remote = btdev_list[i]; + + if (!remote || remote == dev) + continue; + + if (remote->le_scan_enable && + remote->le_periodic_sync_handle == INV_HANDLE) + le_per_adv_sync_estabilished(remote, dev, + BT_HCI_ERR_SUCCESS); + } + return 0; } @@ -5161,78 +5239,12 @@ static void scan_ext_adv(struct btdev *dev, struct btdev *remote) } } -static void send_per_adv(struct btdev *dev, const struct btdev *remote, - uint8_t offset) -{ - struct __packed { - struct bt_hci_le_per_adv_report ev; - uint8_t data[31]; - } pdu; - - memset(&pdu.ev, 0, sizeof(pdu.ev)); - pdu.ev.handle = cpu_to_le16(dev->le_periodic_sync_handle); - pdu.ev.tx_power = 127; - pdu.ev.rssi = 127; - pdu.ev.cte_type = 0x0ff; - - if ((size_t) remote->le_periodic_data_len - offset > sizeof(pdu.data)) { - pdu.ev.data_status = 0x01; - pdu.ev.data_len = sizeof(pdu.data); - } else { - pdu.ev.data_status = 0x00; - pdu.ev.data_len = remote->le_periodic_data_len - offset; - } - - memcpy(pdu.data, remote->le_periodic_data + offset, pdu.ev.data_len); - - le_meta_event(dev, BT_HCI_EVT_LE_PER_ADV_REPORT, &pdu, - sizeof(pdu.ev) + pdu.ev.data_len); - - if (pdu.ev.data_status == 0x01) { - offset += pdu.ev.data_len; - send_per_adv(dev, remote, offset); - } -} - -static void le_per_adv_sync_estabilished(struct btdev *dev, - struct btdev *remote, uint8_t status) -{ - struct bt_hci_evt_le_per_sync_established ev; - struct bt_hci_cmd_le_periodic_adv_create_sync *cmd = &dev->pa_sync_cmd; - - memset(&ev, 0, sizeof(ev)); - ev.status = status; - - if (status) { - memset(&dev->pa_sync_cmd, 0, sizeof(dev->pa_sync_cmd)); - dev->le_periodic_sync_handle = 0x0000; - le_meta_event(dev, BT_HCI_EVT_LE_PER_SYNC_ESTABLISHED, &ev, - sizeof(ev)); - return; - } - - dev->le_periodic_sync_handle = SYC_HANDLE; - - ev.handle = cpu_to_le16(dev->le_periodic_sync_handle); - ev.addr_type = cmd->addr_type; - memcpy(ev.addr, cmd->addr, sizeof(ev.addr)); - ev.phy = 0x01; - ev.interval = remote->le_periodic_min_interval; - ev.clock_accuracy = 0x07; - - memset(&dev->pa_sync_cmd, 0, sizeof(dev->pa_sync_cmd)); - - le_meta_event(dev, BT_HCI_EVT_LE_PER_SYNC_ESTABLISHED, &ev, sizeof(ev)); - send_per_adv(dev, remote, 0); -} - static void scan_per_adv(struct btdev *dev, struct btdev *remote) { if (dev->le_periodic_sync_handle != INV_HANDLE || !remote->le_periodic_adv_enable) return; - if (remote != find_btdev_by_bdaddr_type(dev->pa_sync_cmd.addr, dev->pa_sync_cmd.addr_type)) return; -- cgit v1.2.1