emulator: Improved handling of Set Default PHY command
diff --git a/emulator/le.c b/emulator/le.c
index 33abbd3..1c8ba28 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 @@
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 @@
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 @@
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_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,
