From 07f4ae07bd758f86250d2af7b3dd429171de157e Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 20 Oct 2021 11:41:46 -0700 Subject: [PATCH] emulator: Add initial support for MSFT vendor commands 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) Signed-off-by: Anuj Jain Signed-off-by: Ayush Garg --- 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 3506124..956dcee 100755 --- 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; @@ -6398,41 +6400,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; } @@ -6677,14 +6714,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 f64d46a..3c215ef 100755 --- 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.7.4