From 9e78666ae36b4c42b79eb889122cef4e24e8535b Mon Sep 17 00:00:00 2001
From: Rahul Tank <rahul.tank@espressif.com>
Date: Wed, 14 Aug 2024 15:05:37 +0530
Subject: [PATCH] nimble/host: Add support for ext adv param v2 HCI command

---
 nimble/host/include/host/ble_gap.h | 13 +++++++++++++
 nimble/host/src/ble_gap.c          | 30 +++++++++++++++++++++++++++---
 nimble/host/src/ble_hs_hci_priv.h  |  2 +-
 nimble/host/src/ble_hs_startup.c   |  5 ++---
 nimble/include/nimble/hci_common.h | 21 +++++++++++++++++++++
 5 files changed, 64 insertions(+), 7 deletions(-)

diff --git a/nimble/host/include/host/ble_gap.h b/nimble/host/include/host/ble_gap.h
index e842ae29a7..03d3a0729f 100644
--- a/nimble/host/include/host/ble_gap.h
+++ b/nimble/host/include/host/ble_gap.h
@@ -1582,6 +1582,19 @@ struct ble_gap_ext_adv_params {
 
     /** Advertising Set ID */
     uint8_t sid;
+
+    /** Version of HCI command to be used
+     *  0 : No version set, fallback to v1
+     *  1 : V1
+     *  2 : V2
+     */
+    unsigned int version:2;
+
+    /** Primary phy options */
+    uint8_t pri_phy_opt;
+
+    /** Secondary phy options */
+    uint8_t sec_phy_opt;
 };
 
 /**
diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c
index 38da71200b..3b456e569d 100644
--- a/nimble/host/src/ble_gap.c
+++ b/nimble/host/src/ble_gap.c
@@ -3129,10 +3129,18 @@ ble_gap_ext_adv_params_tx(uint8_t instance,
                           int8_t *selected_tx_power)
 
 {
-    struct ble_hci_le_set_ext_adv_params_cp cmd;
     struct ble_hci_le_set_ext_adv_params_rp rsp;
+    uint16_t opcode;
     int rc;
 
+    if (params->version == 0x2) {
+        struct ble_hs_hci_sup_cmd sup_cmd;
+        struct ble_hci_le_set_ext_adv_params_v2_cp cmd;
+    }
+    else {
+        struct ble_hci_le_set_ext_adv_params_cp cmd;
+    }
+
     memset(&cmd, 0, sizeof(cmd));
 
     cmd.adv_handle = instance;
@@ -3207,8 +3215,24 @@ ble_gap_ext_adv_params_tx(uint8_t instance,
     cmd.sid = params->sid;
     cmd.scan_req_notif = params->scan_req_notif;
 
-    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
-                                      BLE_HCI_OCF_LE_SET_EXT_ADV_PARAM),
+    if (params->version == 0x2) {
+	/* Check if controller has support for V2 */
+	sup_cmd = ble_hs_hci_get_hci_supported_cmd();
+
+        if ((sup_cmd.commands[46] & 0x04 ) != 0) {
+            cmd.pri_phy_opt = params->pri_phy_opt;
+            cmd.sec_phy_opt = params->sec_phy_opt;
+
+            opcode = BLE_HCI_OCF_LE_SET_EXT_ADV_PARAM_V2;
+	} else {
+	    /* Version V2 is not supported by controller. So fail */
+            return BLE_HS_EINVAL;
+	}
+    }
+    else {
+        opcode = BLE_HCI_OCF_LE_SET_EXT_ADV_PARAM;
+    }
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, opcode),
                            &cmd, sizeof(cmd), &rsp, sizeof(rsp));
 
     if (rc != 0) {
diff --git a/nimble/host/src/ble_hs_hci_priv.h b/nimble/host/src/ble_hs_hci_priv.h
index 78621e046a..211f3f931c 100644
--- a/nimble/host/src/ble_hs_hci_priv.h
+++ b/nimble/host/src/ble_hs_hci_priv.h
@@ -80,7 +80,7 @@ struct hci_periodic_adv_params
 #endif
 
 struct ble_hs_hci_sup_cmd {
-    unsigned event_mask2 : 1; /** Indicates whether the controller supports the set event mask page 2 command */
+    uint8_t commands[64];
 };
 
 extern uint16_t ble_hs_hci_avail_pkts;
diff --git a/nimble/host/src/ble_hs_startup.c b/nimble/host/src/ble_hs_startup.c
index c23579547c..2f95880be9 100644
--- a/nimble/host/src/ble_hs_startup.c
+++ b/nimble/host/src/ble_hs_startup.c
@@ -82,8 +82,7 @@ ble_hs_startup_read_sup_cmd_tx(void)
         return rc;
     }
 
-    /* Only check support for Set Event ­Mask ­Page ­2 command */
-    sup_cmd.event_mask2 = (rsp.commands[22] & 0x04) != 0;
+    memcpy(&sup_cmd.commands, &rsp.commands, sizeof(sup_cmd));
     ble_hs_hci_set_hci_supported_cmd(sup_cmd);
 
     return 0;
@@ -373,7 +372,7 @@ ble_hs_startup_set_evmask_tx(void)
         return rc;
     }
 
-    if ((version >= BLE_HCI_VER_BCS_4_1) && sup_cmd.event_mask2) {
+    if ((version >= BLE_HCI_VER_BCS_4_1) && ((sup_cmd.commands[22] & 0x04) != 0)) {
         /**
          * Enable the following events:
          *     0x0000000000800000 Authenticated Payload Timeout Event
diff --git a/nimble/include/nimble/hci_common.h b/nimble/include/nimble/hci_common.h
index b1f6261266..5667eef319 100644
--- a/nimble/include/nimble/hci_common.h
+++ b/nimble/include/nimble/hci_common.h
@@ -597,6 +597,27 @@ struct ble_hci_le_set_ext_adv_params_rp {
     int8_t  tx_power;
 } __attribute__((packed));
 
+#define BLE_HCI_OCF_LE_SET_EXT_ADV_PARAM_V2             (0x007F)
+struct ble_hci_le_set_ext_adv_params_v2_cp {
+    uint8_t adv_handle;
+    uint16_t props;
+    uint8_t pri_itvl_min[3];
+    uint8_t pri_itvl_max[3];
+    uint8_t pri_chan_map;
+    uint8_t own_addr_type;
+    uint8_t peer_addr_type;
+    uint8_t peer_addr[6];
+    uint8_t filter_policy;
+    int8_t tx_power;
+    uint8_t pri_phy;
+    uint8_t sec_max_skip;
+    uint8_t sec_phy;
+    uint8_t sid;
+    uint8_t scan_req_notif;
+    uint8_t pri_phy_opt;
+    uint8_t sec_phy_opt;
+} __attribute__((packed));
+
 #define BLE_HCI_OCF_LE_SET_EXT_ADV_DATA             (0x0037)
 struct ble_hci_le_set_ext_adv_data_cp {
     uint8_t adv_handle;