Hi,
On Mon, May 17, 2021 at 2:52 PM Luiz Augusto von Dentz
<luiz.dentz@xxxxxxxxx> wrote:
>
> From: Luiz Augusto von Dentz <luiz.von.dentz@xxxxxxxxx>
>
> This adds support for commands related to resolving list.
> ---
> emulator/btdev.c | 211 ++++++++++++++++++++++++++++++++++++++++++++++-
> 1 file changed, 210 insertions(+), 1 deletion(-)
>
> diff --git a/emulator/btdev.c b/emulator/btdev.c
> index 2fd452970..692fabd71 100644
> --- a/emulator/btdev.c
> +++ b/emulator/btdev.c
> @@ -34,6 +34,7 @@
> #include "btdev.h"
>
> #define WL_SIZE 16
> +#define RL_SIZE 16
>
> #define has_bredr(btdev) (!((btdev)->features[4] & 0x20))
> #define has_le(btdev) (!!((btdev)->features[4] & 0x40))
> @@ -157,6 +158,9 @@ struct btdev {
> uint8_t le_iso_path[2];
>
> uint8_t le_wl[WL_SIZE][7];
> + uint8_t le_rl[RL_SIZE][39];
> + uint8_t le_rl_enable;
> + uint16_t le_rl_timeout;
>
> uint8_t le_local_sk256[32];
>
> @@ -3440,6 +3444,191 @@ static int cmd_remove_wl(struct btdev *dev, const void *data, uint8_t len)
> return 0;
> }
>
> +static int cmd_add_rl(struct btdev *dev, const void *data, uint8_t len)
> +{
> + const struct bt_hci_cmd_le_add_to_resolv_list *cmd = data;
> + uint8_t status;
> + bool exists = false;
> + int i, pos = -1;
> +
> + /* Valid range for address type is 0x00 to 0x01 */
> + if (cmd->addr_type > 0x01)
> + return -EINVAL;
> +
> + for (i = 0; i < RL_SIZE; i++) {
> + if (dev->le_rl[i][0] == cmd->addr_type &&
> + !memcmp(&dev->le_rl[i][1], cmd->addr, 6)) {
> + exists = true;
> + break;
> + } else if (pos < 0 && dev->le_rl[i][0] == 0xff)
> + pos = i;
> + }
> +
> + if (exists)
> + return -EALREADY;
> +
> + if (pos < 0) {
> + cmd_status(dev, BT_HCI_ERR_MEM_CAPACITY_EXCEEDED,
> + BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST);
> + return 0;
> + }
> +
> + dev->le_rl[pos][0] = cmd->addr_type;
> + memcpy(&dev->le_rl[pos][1], cmd->addr, 6);
> + memcpy(&dev->le_rl[pos][7], cmd->peer_irk, 16);
> + memcpy(&dev->le_rl[pos][23], cmd->local_irk, 16);
> +
> + status = BT_HCI_ERR_SUCCESS;
> + cmd_complete(dev, BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST,
> + &status, sizeof(status));
> +
> + return 0;
> +}
> +
> +static int cmd_remove_rl(struct btdev *dev, const void *data, uint8_t len)
> +{
> + const struct bt_hci_cmd_le_remove_from_resolv_list *cmd = data;
> + uint8_t status;
> + int i, pos = -1;
> +
> + /* Valid range for address type is 0x00 to 0x01 */
> + if (cmd->addr_type > 0x01)
> + return -EINVAL;
> +
> + for (i = 0; i < RL_SIZE; i++) {
> + if (dev->le_rl[i][0] == cmd->addr_type &&
> + !memcmp(&dev->le_rl[i][1], cmd->addr, 6)) {
> + pos = i;
> + break;
> + }
> + }
> +
> + if (pos < 0)
> + return -EINVAL;
> +
> + dev->le_rl[pos][0] = 0xff;
> + memset(&dev->le_rl[pos][1], 0, 38);
> +
> + status = BT_HCI_ERR_SUCCESS;
> + cmd_complete(dev, BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST,
> + &status, sizeof(status));
> +
> + return 0;
> +}
> +
> +static void rl_clear(struct btdev *dev)
> +{
> + int i;
> +
> + for (i = 0; i < RL_SIZE; i++) {
> + dev->le_rl[i][0] = 0xff;
> + memset(&dev->le_rl[i][1], 0, 38);
> + }
> +}
> +
> +static int cmd_clear_rl(struct btdev *dev, const void *data, uint8_t len)
> +{
> + uint8_t status;
> +
> + rl_clear(dev);
> +
> + status = BT_HCI_ERR_SUCCESS;
> + cmd_complete(dev, BT_HCI_CMD_LE_CLEAR_RESOLV_LIST,
> + &status, sizeof(status));
> +
> + return 0;
> +}
> +
> +static int cmd_read_rl_size(struct btdev *dev, const void *data, uint8_t len)
> +{
> + struct bt_hci_rsp_le_read_resolv_list_size rsp;
> +
> + rsp.status = BT_HCI_ERR_SUCCESS;
> + rsp.size = RL_SIZE;
> +
> + cmd_complete(dev, BT_HCI_CMD_LE_READ_RESOLV_LIST_SIZE,
> + &rsp, sizeof(rsp));
> +
> + return 0;
> +}
> +
> +static int cmd_read_peer_rl_addr(struct btdev *dev, const void *data,
> + uint8_t size)
> +{
> + const struct bt_hci_cmd_le_read_peer_resolv_addr *cmd = data;
> + struct bt_hci_rsp_le_read_peer_resolv_addr rsp;
> +
> + /* Valid range for address type is 0x00 to 0x01 */
> + if (cmd->addr_type > 0x01)
> + return -EINVAL;
> +
> + rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
> + memset(rsp.addr, 0, 6);
> +
> + cmd_complete(dev, BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR,
> + &rsp, sizeof(rsp));
> +
> + return 0;
> +}
> +
> +static int cmd_read_local_rl_addr(struct btdev *dev, const void *data,
> + uint8_t size)
> +{
> + const struct bt_hci_cmd_le_read_local_resolv_addr *cmd = data;
> + struct bt_hci_rsp_le_read_local_resolv_addr rsp;
> +
> + /* Valid range for address type is 0x00 to 0x01 */
> + if (cmd->addr_type > 0x01)
> + return -EINVAL;
> +
> + rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
> + memset(rsp.addr, 0, 6);
> +
> + cmd_complete(dev, BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR,
> + &rsp, sizeof(rsp));
> +
> + return 0;
> +}
> +
> +static int cmd_set_rl_enable(struct btdev *dev, const void *data, uint8_t len)
> +{
> + const struct bt_hci_cmd_le_set_resolv_enable *cmd = data;
> + uint8_t status;
> +
> + /* Valid range for address resolution enable is 0x00 to 0x01 */
> + if (cmd->enable > 0x01)
> + return -EINVAL;
> +
> + dev->le_rl_enable = cmd->enable;
> +
> + status = BT_HCI_ERR_SUCCESS;
> + cmd_complete(dev, BT_HCI_CMD_LE_SET_RESOLV_ENABLE,
> + &status, sizeof(status));
> +
> + return 0;
> +}
> +
> +static int cmd_set_rl_timeout(struct btdev *dev, const void *data, uint8_t len)
> +{
> + const struct bt_hci_cmd_le_set_resolv_timeout *cmd = data;
> + uint16_t timeout;
> + uint8_t status;
> +
> + timeout = le16_to_cpu(cmd->timeout);
> +
> + /* Valid range for RPA timeout is 0x0001 to 0xa1b8 */
> + if (timeout < 0x0001 || timeout > 0xa1b8)
> + return -EINVAL;
> +
> + dev->le_rl_timeout = timeout;
> +
> + status = BT_HCI_ERR_SUCCESS;
> + cmd_complete(dev, BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT,
> + &status, sizeof(status));
> +
> + return 0;
> +}
> +
> static int cmd_conn_update(struct btdev *dev, const void *data, uint8_t len)
> {
> cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_CONN_UPDATE);
> @@ -3828,7 +4017,16 @@ static int cmd_gen_dhkey(struct btdev *dev, const void *data, uint8_t len)
> CMD(BT_HCI_CMD_LE_CONN_PARAM_REQ_NEG_REPLY, cmd_conn_param_neg_reply, \
> cmd_conn_param_neg_reply_complete), \
> CMD(BT_HCI_CMD_LE_READ_LOCAL_PK256, cmd_read_local_pk256, NULL), \
> - CMD(BT_HCI_CMD_LE_GENERATE_DHKEY, cmd_gen_dhkey, NULL)
> + CMD(BT_HCI_CMD_LE_GENERATE_DHKEY, cmd_gen_dhkey, NULL), \
> + CMD(BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST, cmd_add_rl, NULL), \
> + CMD(BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST, cmd_remove_rl, NULL), \
> + CMD(BT_HCI_CMD_LE_CLEAR_RESOLV_LIST, cmd_clear_rl, NULL), \
> + CMD(BT_HCI_CMD_LE_READ_RESOLV_LIST_SIZE, cmd_read_rl_size, NULL), \
> + CMD(BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR, cmd_read_peer_rl_addr, NULL), \
> + CMD(BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR, cmd_read_local_rl_addr, \
> + NULL), \
> + CMD(BT_HCI_CMD_LE_SET_RESOLV_ENABLE, cmd_set_rl_enable, NULL), \
> + CMD(BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT, cmd_set_rl_timeout, NULL)
>
> static int cmd_set_default_phy(struct btdev *dev, const void *data,
> uint8_t len)
> @@ -5022,6 +5220,14 @@ static void set_le_commands(struct btdev *btdev)
> /* Extra LE commands for >= 4.2 adapters */
> btdev->commands[34] |= 0x02; /* LE Read Local P-256 Public Key */
> btdev->commands[34] |= 0x04; /* LE Generate DHKey */
> + btdev->commands[34] |= 0x08; /* LE Add Device To Resolving List */
> + btdev->commands[34] |= 0x10; /* LE Remove Dev From Resolving List */
> + btdev->commands[34] |= 0x20; /* LE Clear Resolving List */
> + btdev->commands[34] |= 0x40; /* LE Read Resolving List Size */
> + btdev->commands[34] |= 0x80; /* LE Read Peer Resolvable Address */
> + btdev->commands[35] |= 0x01; /* LE Read Local Resolvable Address */
> + btdev->commands[35] |= 0x02; /* LE Set Address Resolution Enable */
> + btdev->commands[35] |= 0x04; /* LE Set RPA Timeout */
>
> btdev->cmds = cmd_le;
>
> @@ -5302,6 +5508,9 @@ static void set_le_states(struct btdev *btdev)
> btdev->le_states[5] = 0x03;
>
> wl_clear(btdev);
> + rl_clear(btdev);
> + btdev->le_rl_enable = 0x00;
> + btdev->le_rl_timeout = 0x0384; /* 900 secs or 15 minutes */
> }
>
> static void set_amp_features(struct btdev *btdev)
> --
> 2.30.2
Pushed.
--
Luiz Augusto von Dentz
