[BlueZ] btdev: Add support for Add/Remove White List

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;