emulator: Add basic PHY command handling to LE only controller

1 files changed, 52 insertions, 5 deletions

diff --git a/emulator/le.c b/emulator/le.c
index 6cfbe638d..0f71a1800 100644
--- a/emulator/le.c
+++ b/emulator/le.c
@@ -281,9 +281,9 @@ static void reset_defaults(struct bt_le *hci)
 	hci->commands[35] |= 0x02;	/* LE Set Address Resolution Enable */
 	hci->commands[35] |= 0x04;	/* LE Set Resolvable Private Address Timeout */
 	hci->commands[35] |= 0x08;	/* LE Read Maximum Data Length */
-	//hci->commands[35] |= 0x10;	/* LE Read PHY */
-	//hci->commands[35] |= 0x20;	/* LE Set Default PHY */
-	//hci->commands[35] |= 0x40;	/* LE Set PHY */
+	hci->commands[35] |= 0x10;	/* LE Read PHY */
+	hci->commands[35] |= 0x20;	/* LE Set Default PHY */
+	hci->commands[35] |= 0x40;	/* LE Set PHY */
 	//hci->commands[35] |= 0x80;	/* LE Enhanced Receiver Test */
 	//hci->commands[36] |= 0x01;	/* LE Enhanced Transmitter Test */
 	//hci->commands[36] |= 0x02;	/* LE Set Advertising Set Random Address */
@@ -353,13 +353,13 @@ static void reset_defaults(struct bt_le *hci)
 	hci->le_features[0] |= 0x20;	/* LE Data Packet Length Extension */
 	hci->le_features[0] |= 0x40;	/* LL Privacy */
 	hci->le_features[0] |= 0x80;	/* Extended Scanner Filter Policies */
-	//hci->le_features[1] |= 0x01;	/* LE 2M PHY */
+	hci->le_features[1] |= 0x01;	/* LE 2M PHY */
 	//hci->le_features[1] |= 0x02;	/* Stable Modulation Index - Transmitter */
 	//hci->le_features[1] |= 0x04;	/* Stable Modulation Index - Receiver */
 	//hci->le_features[1] |= 0x08;	/* LE Coded PHY */
 	//hci->le_features[1] |= 0x10;	/* LE Extended Advertising */
 	//hci->le_features[1] |= 0x20;	/* LE Periodic Advertising */
-	//hci->le_features[1] |= 0x40;	/* Channel Selection Algorithm #2 */
+	hci->le_features[1] |= 0x40;	/* Channel Selection Algorithm #2 */
 	//hci->le_features[1] |= 0x80;	/* LE Power Class 1 */
 	//hci->le_features[2] |= 0x01;	/* Minimum Number of Used Channels Procedure */
 
@@ -1694,6 +1694,47 @@ static void cmd_le_read_max_data_length(struct bt_le *hci,
 							&rsp, sizeof(rsp));
 }
 
+static void cmd_le_read_phy(struct bt_le *hci, const void *data, uint8_t size)
+{
+	const struct bt_hci_cmd_le_read_phy *cmd = data;
+	struct bt_hci_rsp_le_read_phy rsp;
+
+	rsp.status = BT_HCI_ERR_SUCCESS;
+	rsp.handle = cmd->handle;
+	rsp.tx_phy = 0x01;	/* LE 1M */
+	rsp.rx_phy = 0x01;	/* LE 1M */
+
+	cmd_complete(hci, BT_HCI_CMD_LE_READ_PHY, &rsp, sizeof(rsp));
+}
+
+static void cmd_le_set_default_phy(struct bt_le *hci,
+						const void *data, uint8_t size)
+{
+	//const struct bt_hci_cmd_le_set_default_phy *cmd = data;
+	uint8_t status;
+
+	status = BT_HCI_ERR_SUCCESS;
+	cmd_complete(hci, BT_HCI_CMD_LE_SET_DEFAULT_PHY,
+						&status, sizeof(status));
+}
+
+static void cmd_le_set_phy(struct bt_le *hci, const void *data, uint8_t size)
+{
+	const struct bt_hci_cmd_le_set_phy *cmd = data;
+	struct bt_hci_evt_le_phy_update_complete evt;
+
+	cmd_status(hci, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_SET_PHY);
+
+	evt.status = BT_HCI_ERR_SUCCESS;
+	evt.handle = cmd->handle;
+	evt.tx_phy = 0x01;	/* LE 1M */
+	evt.rx_phy = 0x01;	/* LE 1M */
+
+	if (hci->le_event_mask[1] & 0x08)
+		le_meta_event(hci, BT_HCI_EVT_LE_PHY_UPDATE_COMPLETE,
+							&evt, sizeof(evt));
+}
+
 static const struct {
 	uint16_t opcode;
 	void (*func) (struct bt_le *hci, const void *data, uint8_t size);
@@ -1781,6 +1822,12 @@ static const struct {
 				cmd_le_set_resolv_timeout, 2, true },
 	{ BT_HCI_CMD_LE_READ_MAX_DATA_LENGTH,
 				cmd_le_read_max_data_length, 0, true },
+	{ BT_HCI_CMD_LE_READ_PHY,
+				cmd_le_read_phy, 2, true },
+	{ BT_HCI_CMD_LE_SET_DEFAULT_PHY,
+				cmd_le_set_default_phy, 3, true },
+	{ BT_HCI_CMD_LE_SET_PHY,
+				cmd_le_set_phy, 7, true },
 
 	{ }
 };