From: Luiz Augusto von Dentz <luiz.von.dentz@xxxxxxxxx>
This adds support for Add to/Remove from White List.
---
emulator/btdev.c | 104 +++++++++++++++++++++++++++++++++++++++++++++--
1 file changed, 100 insertions(+), 4 deletions(-)
diff --git a/emulator/btdev.c b/emulator/btdev.c
index 50407d9a2..2fd452970 100644
--- a/emulator/btdev.c
+++ b/emulator/btdev.c
@@ -33,6 +33,8 @@
#include "monitor/bt.h"
#include "btdev.h"
+#define WL_SIZE 16
+
#define has_bredr(btdev) (!((btdev)->features[4] & 0x20))
#define has_le(btdev) (!!((btdev)->features[4] & 0x40))
@@ -154,6 +156,8 @@ struct btdev {
} __attribute__ ((packed)) le_cig;
uint8_t le_iso_path[2];
+ uint8_t le_wl[WL_SIZE][7];
+
uint8_t le_local_sk256[32];
uint16_t sync_train_interval;
@@ -383,6 +387,16 @@ static int cmd_set_event_mask(struct btdev *dev, const void *data, uint8_t len)
return 0;
}
+static void wl_clear(struct btdev *dev)
+{
+ int i;
+
+ for (i = 0; i < WL_SIZE; i++) {
+ dev->le_wl[i][0] = 0xff;
+ memset(&dev->le_wl[i][1], 0, 6);
+ }
+}
+
static void btdev_reset(struct btdev *btdev)
{
/* FIXME: include here clearing of all states that should be
@@ -391,6 +405,8 @@ static void btdev_reset(struct btdev *btdev)
btdev->le_scan_enable = 0x00;
btdev->le_adv_enable = 0x00;
+
+ wl_clear(btdev);
}
static int cmd_reset(struct btdev *dev, const void *data, uint8_t len)
@@ -3333,17 +3349,19 @@ static int cmd_read_wl_size(struct btdev *dev, const void *data, uint8_t len)
struct bt_hci_rsp_le_read_white_list_size rsp;
rsp.status = BT_HCI_ERR_SUCCESS;
- rsp.size = 0;
+ rsp.size = WL_SIZE;
cmd_complete(dev, BT_HCI_CMD_LE_READ_WHITE_LIST_SIZE, &rsp,
sizeof(rsp));
return 0;
}
-static int cmd_clear_wl(struct btdev *dev, const void *data, uint8_t len)
+static int cmd_wl_clear(struct btdev *dev, const void *data, uint8_t len)
{
uint8_t status;
+ wl_clear(dev);
+
status = BT_HCI_ERR_SUCCESS;
cmd_complete(dev, BT_HCI_CMD_LE_CLEAR_WHITE_LIST, &status,
sizeof(status));
@@ -3351,6 +3369,77 @@ static int cmd_clear_wl(struct btdev *dev, const void *data, uint8_t len)
return 0;
}
+static int cmd_add_wl(struct btdev *dev, const void *data, uint8_t len)
+{
+ const struct bt_hci_cmd_le_add_to_white_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 < WL_SIZE; i++) {
+ if (dev->le_wl[i][0] == cmd->addr_type &&
+ !memcmp(&dev->le_wl[i][1],
+ cmd->addr, 6)) {
+ exists = true;
+ break;
+ } else if (pos < 0 && dev->le_wl[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_WHITE_LIST);
+ return 0;
+ }
+
+ dev->le_wl[pos][0] = cmd->addr_type;
+ memcpy(&dev->le_wl[pos][1], cmd->addr, 6);
+
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(dev, BT_HCI_CMD_LE_ADD_TO_WHITE_LIST,
+ &status, sizeof(status));
+
+ return 0;
+}
+
+static int cmd_remove_wl(struct btdev *dev, const void *data, uint8_t len)
+{
+ const struct bt_hci_cmd_le_remove_from_white_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 < WL_SIZE; i++) {
+ if (dev->le_wl[i][0] == cmd->addr_type &&
+ !memcmp(&dev->le_wl[i][1], cmd->addr, 6)) {
+ pos = i;
+ break;
+ }
+ }
+
+ if (pos < 0)
+ return -EINVAL;
+
+ dev->le_wl[pos][0] = 0xff;
+ memset(&dev->le_wl[pos][1], 0, 6);
+
+ status = BT_HCI_ERR_SUCCESS;
+ cmd_complete(dev, BT_HCI_CMD_LE_REMOVE_FROM_WHITE_LIST,
+ &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);
@@ -3717,7 +3806,9 @@ static int cmd_gen_dhkey(struct btdev *dev, const void *data, uint8_t len)
CMD(BT_HCI_CMD_LE_CREATE_CONN, cmd_le_create_conn, \
cmd_le_create_conn_complete), \
CMD(BT_HCI_CMD_LE_READ_WHITE_LIST_SIZE, cmd_read_wl_size, NULL), \
- CMD(BT_HCI_CMD_LE_CLEAR_WHITE_LIST, cmd_clear_wl, NULL), \
+ CMD(BT_HCI_CMD_LE_CLEAR_WHITE_LIST, cmd_wl_clear, NULL), \
+ CMD(BT_HCI_CMD_LE_ADD_TO_WHITE_LIST, cmd_add_wl, NULL), \
+ CMD(BT_HCI_CMD_LE_REMOVE_FROM_WHITE_LIST, cmd_remove_wl, NULL), \
CMD(BT_HCI_CMD_LE_CONN_UPDATE, cmd_conn_update, \
cmd_conn_update_complete), \
CMD(BT_HCI_CMD_LE_READ_REMOTE_FEATURES, cmd_le_read_remote_features, \
@@ -4910,6 +5001,8 @@ static void set_le_commands(struct btdev *btdev)
btdev->commands[26] |= 0x10; /* LE Create Connection */
btdev->commands[26] |= 0x40; /* LE Read White List Size */
btdev->commands[26] |= 0x80; /* LE Clear White List */
+ btdev->commands[27] |= 0x01; /* LE Add Device to White List */
+ btdev->commands[27] |= 0x02; /* LE Remove Device from White List */
btdev->commands[27] |= 0x04; /* LE Connection Update */
btdev->commands[27] |= 0x20; /* LE Read Remote Used Features */
btdev->commands[27] |= 0x40; /* LE Encrypt */
@@ -5207,6 +5300,8 @@ static void set_le_states(struct btdev *btdev)
btdev->le_states[3] = 0xff;
btdev->le_states[4] = 0xff;
btdev->le_states[5] = 0x03;
+
+ wl_clear(btdev);
}
static void set_amp_features(struct btdev *btdev)
@@ -5421,9 +5516,10 @@ static const struct btdev_cmd *default_cmd(struct btdev *btdev, uint16_t opcode,
}
}
-failed:
util_debug(btdev->debug_callback, btdev->debug_data,
"Unsupported command 0x%4.4x\n", opcode);
+
+failed:
cmd_status(btdev, status, opcode);
return NULL;
--
2.30.2
