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 <anuj01.jain@samsung.com>
Signed-off-by: Ayush Garg <ayush.garg@samsung.com>

diff --git a/emulator/btdev.c b/emulator/btdev.c
index 3506124..956dcee 100755 (executable)
--- 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 (executable)
--- 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) {