emulator: Improved handling of Set Default PHY command

1 files changed, 69 insertions, 5 deletions

diff --git a/emulator/le.c b/emulator/le.c
index 33abbd31e..1c8ba280b 100644
--- a/emulator/le.c
+++ b/emulator/le.c
@@ -58,6 +58,10 @@
 #define MAX_RX_LEN		0x00fb
 #define MAX_RX_TIME		0x0848
 
+#define DEFAULT_ALL_PHYS	0x03
+#define DEFAULT_TX_PHYS		0x00
+#define DEFAULT_RX_PHYS		0x00
+
 struct bt_peer {
 	uint8_t  addr_type;
 	uint8_t  addr[6];
@@ -123,6 +127,10 @@ struct bt_le {
 	uint8_t  le_resolv_enable;
 	uint16_t le_resolv_timeout;
 
+	uint8_t  le_default_all_phys;
+	uint8_t  le_default_tx_phys;
+	uint8_t  le_default_rx_phys;
+
 	struct bt_peer scan_cache[SCAN_CACHE_SIZE];
 	uint8_t scan_cache_count;
 };
@@ -354,9 +362,9 @@ static void reset_defaults(struct bt_le *hci)
 	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] |= 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] |= 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 */
@@ -418,6 +426,10 @@ static void reset_defaults(struct bt_le *hci)
 	clear_resolv_list(hci);
 	hci->le_resolv_enable = 0x00;
 	hci->le_resolv_timeout = 0x0384;	/* 900 secs or 15 minutes */
+
+	hci->le_default_all_phys = DEFAULT_ALL_PHYS;
+	hci->le_default_tx_phys = DEFAULT_TX_PHYS;
+	hci->le_default_rx_phys = DEFAULT_RX_PHYS;
 }
 
 static void clear_scan_cache(struct bt_le *hci)
@@ -1710,8 +1722,60 @@ static void cmd_le_read_phy(struct bt_le *hci, const void *data, uint8_t size)
 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;
+	const struct bt_hci_cmd_le_set_default_phy *cmd = data;
+	uint8_t status, tx_phys, rx_phys;
+	uint8_t phys_mask;
+
+	phys_mask = (true << 0) | ((!!(hci->le_features[1] & 0x01)) << 1)
+				| ((!!(hci->le_features[1] & 0x08)) << 2);
+
+	if (cmd->all_phys > 0x03) {
+		cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
+					BT_HCI_CMD_LE_SET_DEFAULT_PHY);
+		return;
+	}
+
+	/* Transmitter PHYs preferences */
+	if (!(cmd->all_phys & 0x01)) {
+		/* At least one preference bit shall be set to 1 */
+		if (!cmd->tx_phys) {
+			cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
+					BT_HCI_CMD_LE_SET_DEFAULT_PHY);
+			return;
+		}
+
+		if (cmd->tx_phys & ~phys_mask) {
+			cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
+					BT_HCI_CMD_LE_SET_DEFAULT_PHY);
+			return;
+		}
+
+		tx_phys = cmd->tx_phys;
+	} else
+		tx_phys = 0x00;
+
+	/* Transmitter PHYs preferences */
+	if (!(cmd->all_phys & 0x02)) {
+		/* At least one preference bit shall be set to 1 */
+		if (!cmd->rx_phys) {
+			cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
+					BT_HCI_CMD_LE_SET_DEFAULT_PHY);
+			return;
+		}
+
+		if (cmd->rx_phys & ~phys_mask) {
+			cmd_status(hci, BT_HCI_ERR_INVALID_PARAMETERS,
+					BT_HCI_CMD_LE_SET_DEFAULT_PHY);
+			return;
+		}
+
+		rx_phys = cmd->rx_phys;
+	} else
+		rx_phys = 0x00;
+
+	hci->le_default_all_phys = cmd->all_phys;
+	hci->le_default_tx_phys = tx_phys;
+	hci->le_default_rx_phys = rx_phys;
 
 	status = BT_HCI_ERR_SUCCESS;
 	cmd_complete(hci, BT_HCI_CMD_LE_SET_DEFAULT_PHY,