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
