From: Luiz Augusto von Dentz <luiz.von.dentz@xxxxxxxxx>
This adds the initial support for MSFT vendor commands and enable them
when in btvirt:
< HCI Command: Microsoft Ex.. (0x3f|0x001e) plen 1
Read Supported Features (0x00)
> HCI Event: Command Complete (0x0e) plen 14
Microsoft Extension (0x3f|0x001e) ncmd 1
Read Supported Features (0x00)
Status: Success (0x00)
Features: 0x3f 0x00 0x00 0x00 0x00 0x00 0x00 0x00
RSSI Monitoring feature for BR/EDR
RSSI Monitoring feature for LE connections
RSSI Monitoring of LE advertisements
Advertising Monitoring of LE advertisements
Verifying the validity of P-192 and P-256 keys
Continuous Advertising Monitoring
Event prefix length: 0
Event prefix:
< HCI Command: Microsoft Ex.. (0x3f|0x001e) plen 2
LE Set Advertisement Filter Enable (0x05)
Enable: All filter conditions (0x01)
> HCI Event: Command Complete (0x0e) plen 5
Microsoft Extension (0x3f|0x001e) ncmd 1
LE Set Advertisement Filter Enable (0x05)
Status: Success (0x00)
---
emulator/btdev.c | 245 ++++++++++++++++++++++++++++++++++++++++++-----
emulator/main.c | 2 +
2 files changed, 223 insertions(+), 24 deletions(-)
diff --git a/emulator/btdev.c b/emulator/btdev.c
index 18aefed11..096ba2da4 100644
--- a/emulator/btdev.c
+++ b/emulator/btdev.c
@@ -33,6 +33,7 @@
#include "src/shared/ecc.h"
#include "src/shared/queue.h"
#include "monitor/bt.h"
+#include "monitor/msft.h"
#include "btdev.h"
#define AL_SIZE 16
@@ -139,6 +140,7 @@ struct btdev {
uint8_t le_states[8];
const struct btdev_cmd *cmds;
uint16_t msft_opcode;
+ const struct btdev_cmd *msft_cmds;
bool aosp_capable;
uint16_t default_link_policy;
@@ -6440,41 +6442,76 @@ static void num_completed_packets(struct btdev *btdev, uint16_t handle)
}
}
+static const struct btdev_cmd *run_cmd(struct btdev *btdev,
+ const struct btdev_cmd *cmd,
+ const void *data, uint8_t len)
+{
+ uint8_t status = BT_HCI_ERR_UNKNOWN_COMMAND;
+ int err;
+
+ err = cmd->func(btdev, data, len);
+ switch (err) {
+ case 0:
+ return cmd;
+ case -ENOTSUP:
+ status = BT_HCI_ERR_UNKNOWN_COMMAND;
+ break;
+ case -EINVAL:
+ status = BT_HCI_ERR_INVALID_PARAMETERS;
+ break;
+ case -EPERM:
+ status = BT_HCI_ERR_COMMAND_DISALLOWED;
+ break;
+ default:
+ status = BT_HCI_ERR_UNSPECIFIED_ERROR;
+ break;
+ }
+
+ cmd_status(btdev, status, cmd->opcode);
+
+ return NULL;
+}
+
+static const struct btdev_cmd *msft_cmd(struct btdev *btdev, const void *data,
+ uint8_t len)
+{
+ const struct btdev_cmd *cmd;
+
+ for (cmd = btdev->msft_cmds; cmd->func; cmd++) {
+ if (cmd->opcode != ((uint8_t *)data)[0])
+ continue;
+
+ return run_cmd(btdev, cmd, data, len);
+ }
+
+ util_debug(btdev->debug_callback, btdev->debug_data,
+ "Unsupported MSFT subcommand 0x%2.2x\n",
+ ((uint8_t *)data)[0]);
+
+ cmd_status(btdev, BT_HCI_ERR_UNKNOWN_COMMAND, btdev->msft_opcode);
+
+ return NULL;
+}
+
static const struct btdev_cmd *default_cmd(struct btdev *btdev, uint16_t opcode,
const void *data, uint8_t len)
{
const struct btdev_cmd *cmd;
- uint8_t status = BT_HCI_ERR_UNKNOWN_COMMAND;
- int err;
+
+ if (btdev->msft_opcode == opcode)
+ return msft_cmd(btdev, data, len);
for (cmd = btdev->cmds; cmd->func; cmd++) {
if (cmd->opcode != opcode)
continue;
- err = cmd->func(btdev, data, len);
- switch (err) {
- case 0:
- return cmd;
- case -ENOTSUP:
- status = BT_HCI_ERR_UNKNOWN_COMMAND;
- goto failed;
- case -EINVAL:
- status = BT_HCI_ERR_INVALID_PARAMETERS;
- goto failed;
- case -EPERM:
- status = BT_HCI_ERR_COMMAND_DISALLOWED;
- goto failed;
- default:
- status = BT_HCI_ERR_UNSPECIFIED_ERROR;
- goto failed;
- }
+ return run_cmd(btdev, cmd, data, len);
}
util_debug(btdev->debug_callback, btdev->debug_data,
"Unsupported command 0x%4.4x\n", opcode);
-failed:
- cmd_status(btdev, status, opcode);
+ cmd_status(btdev, BT_HCI_ERR_UNKNOWN_COMMAND, opcode);
return NULL;
}
@@ -6719,14 +6756,174 @@ bool btdev_del_hook(struct btdev *btdev, enum btdev_hook_type type,
return false;
}
+static int cmd_msft_read_features(struct btdev *dev, const void *data,
+ uint8_t len)
+{
+ struct msft_rsp_read_supported_features rsp;
+
+ memset(&rsp, 0, sizeof(rsp));
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.subcmd = MSFT_SUBCMD_READ_SUPPORTED_FEATURES;
+ rsp.features[0] = MSFT_MONITOR_BREDR_RSSI | MSFT_MONITOR_LE_RSSI |
+ MSFT_MONITOR_LE_LEGACY_RSSI |
+ MSFT_MONITOR_LE_ADV |
+ MSFT_MONITOR_SSP_VALIDATION |
+ MSFT_MONITOR_LE_ADV_CONTINUOS;
+
+ cmd_complete(dev, dev->msft_opcode, &rsp, sizeof(rsp));
+
+ return 0;
+}
+
+static int cmd_msft_monitor_rssi(struct btdev *dev, const void *data,
+ uint8_t len)
+{
+ const struct msft_cmd_monitor_rssi *cmd = data;
+ struct msft_rsp_monitor_rssi rsp;
+ struct btdev_conn *conn;
+
+ memset(&rsp, 0, sizeof(rsp));
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.subcmd = MSFT_SUBCMD_MONITOR_RSSI;
+
+ conn = queue_find(dev->conns, match_handle,
+ UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+ if (!conn)
+ rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
+
+ cmd_complete(dev, dev->msft_opcode, &rsp, sizeof(rsp));
+
+ return 0;
+}
+
+static int cmd_msft_cancel_monitor_rssi(struct btdev *dev, const void *data,
+ uint8_t len)
+{
+ const struct msft_cmd_cancel_monitor_rssi *cmd = data;
+ struct msft_rsp_cancel_monitor_rssi rsp;
+ struct btdev_conn *conn;
+
+ memset(&rsp, 0, sizeof(rsp));
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.subcmd = MSFT_SUBCMD_CANCEL_MONITOR_RSSI;
+
+ conn = queue_find(dev->conns, match_handle,
+ UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+ if (!conn)
+ rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
+
+ cmd_complete(dev, dev->msft_opcode, &rsp, sizeof(rsp));
+
+ return 0;
+}
+
+static int cmd_msft_le_monitor_adv(struct btdev *dev, const void *data,
+ uint8_t len)
+{
+ const struct msft_cmd_le_monitor_adv *cmd = data;
+ struct msft_rsp_le_monitor_adv rsp;
+ static uint8_t handle;
+
+ memset(&rsp, 0, sizeof(rsp));
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.subcmd = MSFT_SUBCMD_LE_MONITOR_ADV;
+
+ switch (cmd->type) {
+ case MSFT_LE_MONITOR_ADV_PATTERN:
+ case MSFT_LE_MONITOR_ADV_UUID:
+ case MSFT_LE_MONITOR_ADV_IRK:
+ case MSFT_LE_MONITOR_ADV_ADDR:
+ rsp.handle = handle++;
+ break;
+ default:
+ rsp.status = BT_HCI_ERR_INVALID_PARAMETERS;
+ break;
+ }
+
+ cmd_complete(dev, dev->msft_opcode, &rsp, sizeof(rsp));
+
+ return 0;
+}
+
+static int cmd_msft_le_cancel_monitor_adv(struct btdev *dev, const void *data,
+ uint8_t len)
+{
+ struct msft_rsp_le_cancel_monitor_adv rsp;
+
+ memset(&rsp, 0, sizeof(rsp));
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.subcmd = MSFT_SUBCMD_LE_CANCEL_MONITOR_ADV;
+
+ cmd_complete(dev, dev->msft_opcode, &rsp, sizeof(rsp));
+
+ return 0;
+}
+
+static int cmd_msft_le_monitor_adv_enable(struct btdev *dev, const void *data,
+ uint8_t len)
+{
+ struct msft_rsp_le_cancel_monitor_adv rsp;
+
+ memset(&rsp, 0, sizeof(rsp));
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.subcmd = MSFT_SUBCMD_LE_MONITOR_ADV_ENABLE;
+
+ cmd_complete(dev, dev->msft_opcode, &rsp, sizeof(rsp));
+
+ return 0;
+}
+
+static int cmd_msft_read_abs_rssi(struct btdev *dev, const void *data,
+ uint8_t len)
+{
+ struct msft_rsp_read_abs_rssi rsp;
+
+ memset(&rsp, 0, sizeof(rsp));
+ rsp.status = BT_HCI_ERR_SUCCESS;
+ rsp.subcmd = MSFT_SUBCMD_READ_ABS_RSSI;
+
+ cmd_complete(dev, dev->msft_opcode, &rsp, sizeof(rsp));
+
+ return 0;
+}
+
+#define CMD_MSFT \
+ CMD(MSFT_SUBCMD_READ_SUPPORTED_FEATURES, cmd_msft_read_features, \
+ NULL), \
+ CMD(MSFT_SUBCMD_MONITOR_RSSI, cmd_msft_monitor_rssi, NULL), \
+ CMD(MSFT_SUBCMD_CANCEL_MONITOR_RSSI, cmd_msft_cancel_monitor_rssi, \
+ NULL), \
+ CMD(MSFT_SUBCMD_LE_MONITOR_ADV, cmd_msft_le_monitor_adv, NULL), \
+ CMD(MSFT_SUBCMD_LE_CANCEL_MONITOR_ADV, cmd_msft_le_cancel_monitor_adv, \
+ NULL), \
+ CMD(MSFT_SUBCMD_LE_MONITOR_ADV_ENABLE, cmd_msft_le_monitor_adv_enable, \
+ NULL), \
+ CMD(MSFT_SUBCMD_READ_ABS_RSSI, cmd_msft_read_abs_rssi, NULL)
+
+static const struct btdev_cmd cmd_msft[] = {
+ CMD_MSFT,
+ {}
+};
+
int btdev_set_msft_opcode(struct btdev *btdev, uint16_t opcode)
{
if (!btdev)
return -EINVAL;
- btdev->msft_opcode = opcode;
-
- return 0;
+ switch (btdev->type) {
+ case BTDEV_TYPE_BREDRLE:
+ case BTDEV_TYPE_BREDRLE50:
+ case BTDEV_TYPE_BREDRLE52:
+ btdev->msft_opcode = opcode;
+ btdev->msft_cmds = cmd_msft;
+ return 0;
+ case BTDEV_TYPE_BREDR:
+ case BTDEV_TYPE_LE:
+ case BTDEV_TYPE_AMP:
+ case BTDEV_TYPE_BREDR20:
+ default:
+ return -ENOTSUP;
+ }
}
int btdev_set_aosp_capable(struct btdev *btdev, bool enable)
diff --git a/emulator/main.c b/emulator/main.c
index f64d46a5e..3c215efbc 100644
--- a/emulator/main.c
+++ b/emulator/main.c
@@ -192,6 +192,8 @@ int main(int argc, char *argv[])
if (debug_enabled)
vhci_set_debug(vhci, vhci_debug, UINT_TO_PTR(i), NULL);
+
+ vhci_set_msft_opcode(vhci, 0xfc1e);
}
if (serial_enabled) {
--
2.31.1
