diff mbox series

[BlueZ] btdev: Add resolving list support

Message ID 20210517215223.612671-1-luiz.dentz@gmail.com
State New
Headers show
Series [BlueZ] btdev: Add resolving list support | expand

Commit Message

Luiz Augusto von Dentz May 17, 2021, 9:52 p.m. UTC
From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

This adds support for commands related to resolving list.
---
 emulator/btdev.c | 211 ++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 210 insertions(+), 1 deletion(-)
diff mbox series

Patch

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)