diff --git a/components/bt/bluedroid/stack/btm/btm_ble_gap.c b/components/bt/bluedroid/stack/btm/btm_ble_gap.c
index 445cf0357..5540728ad 100644
--- a/components/bt/bluedroid/stack/btm/btm_ble_gap.c
+++ b/components/bt/bluedroid/stack/btm/btm_ble_gap.c
@@ -708,6 +708,37 @@ extern void BTM_BleReadControllerFeatures(tBTM_BLE_CTRL_FEATURES_CBACK  *p_vsc_c
     return ;
 }
 
+void BTM_VendorHciEchoCmdCallback(tBTM_VSC_CMPL *p1)
+{
+    if (!p1) {
+        return;
+    }
+    uint8_t *p = p1->p_param_buf;
+    uint8_t status, echo;
+    STREAM_TO_UINT8  (status, p);
+    STREAM_TO_UINT8  (echo, p);
+    BTM_TRACE_DEBUG("%s status 0x%x echo 0x%x", __func__, status, echo);
+}
+
+/******************************************************************************
+**
+** Function         BTM_VendorHciEchoCmdTest
+**
+** Description      vendor common echo hci cmd test, controller will return status and echo
+**
+** Parameters:      echo : echo value
+**
+** Returns          void
+**
+*******************************************************************************/
+void BTM_VendorHciEchoCmdTest(uint8_t echo)
+{
+    BTM_VendorSpecificCommand (HCI_VENDOR_COMMON_ECHO_CMD_OPCODE,
+                                1,
+                                &echo, 
+                                BTM_VendorHciEchoCmdCallback);
+}
+
 /*******************************************************************************
 **
 ** Function         BTM_BleEnableMixedPrivacyMode
diff --git a/components/bt/bluedroid/stack/include/stack/hcidefs.h b/components/bt/bluedroid/stack/include/stack/hcidefs.h
index 0169ba8cc..486cce8ab 100644
--- a/components/bt/bluedroid/stack/include/stack/hcidefs.h
+++ b/components/bt/bluedroid/stack/include/stack/hcidefs.h
@@ -45,7 +45,7 @@
 #define HCI_GRP_VENDOR_SPECIFIC         (0x3F << 10)            /* 0xFC00 */
 
 /* Group occupies high 6 bits of the HCI command rest is opcode itself */
-#define HCI_OGF(p)  (UINT8)((0xFC00 & (p)) >> 10)
+#define HCI_OGF(p)  (UINT8)(0x003F & (p >> 10))
 #define HCI_OCF(p)  ( 0x3FF & (p))
 
 /*
@@ -352,27 +352,65 @@
 #define HCI_BLE_READ_RESOLVABLE_ADDR_LOCAL  (0x002C | HCI_GRP_BLE_CMDS)
 #define HCI_BLE_SET_ADDR_RESOLUTION_ENABLE  (0x002D | HCI_GRP_BLE_CMDS)
 #define HCI_BLE_SET_RAND_PRIV_ADDR_TIMOUT   (0x002E | HCI_GRP_BLE_CMDS)
+// Vendor OGF define
+#define HCI_VENDOR_OGF          0x3F
 
-/* LE Get Vendor Capabilities Command OCF */
-#define HCI_BLE_VENDOR_CAP_OCF    (0x0153 | HCI_GRP_VENDOR_SPECIFIC)
+// ESP vendor group define
+#define HCI_ESP_GROUP_COMMON    0x01
+#define HCI_ESP_GROUP_BLE       0x02
+#define HCI_ESP_GROUP_BT        0x03
+#define HCI_ESP_GROUP_END       0x07
 
+//ESP common subcode define
+#define HCI_SUBCODE_COMMON_INIT 0x00
+#define HCI_SUBCODE_COMMON_ECHO 0x01
+#define HCI_SUBCODE_COMMON_MAX  0x7F
+
+//ESP BLE subcode define
+#define HCI_SUBCODE_BLE_INIT                 0x00
+#define HCI_SUBCODE_BLE_MULTI_ADV            0x01
+#define HCI_SUBCODE_BLE_BATCH_SCAN           0x02
+#define HCI_SUBCODE_BLE_ADV_FILTER           0x03
+#define HCI_SUBCODE_BLE_TRACK_ADV            0x04
+#define HCI_SUBCODE_BLE_ENERGY_INFO          0x05
+#define HCI_SUBCODE_BLE_EXTENDED_SCAN_PARAMS 0x06
+#define HCI_SUBCODE_BLE_MAX     0x7F
+
+//ESP BT subcode define
+#define HCI_SUBCODE_BT_INIT     0x00
+#define HCI_SUBCODE_BT_MAX      0x7F
+
+#define HCI_ESP_VENDOR_OPCODE_BUILD(ogf, group, subcode) ((ogf << 10) | (group <<7) | (subcode << 0))
+/*
+* |     OGF     | VENDIOR GROUP | GROUP SUBCODE |
+* | 1 1 1 1 1 1 |     0 0 0     | X X X X X X X | Already Exist
+* | 1 1 1 1 1 1 |     0 0 1     | X X X X X X X | ESP VENDOR COMMON HCI CMD
+* | 1 1 1 1 1 1 |     0 1 0     | X X X X X X X | ESP VENDOR BLE HCI CMD
+* | 1 1 1 1 1 1 |     0 1 1     | X X X X X X X | ESP VENDOR BT HCI CMD
+* | 1 1 1 1 1 1 |     1 0 0     | X X X X X X X | RESERVED FOR FUTURE USE
+* | 1 1 1 1 1 1 |     1 0 1     | X X X X X X X | RESERVED FOR FUTURE USE
+* | 1 1 1 1 1 1 |     1 1 0     | X X X X X X X | RESERVED FOR FUTURE USE
+* | 1 1 1 1 1 1 |     1 1 1     | X X X X X X X | RESERVED FOR FUTURE USE
+*/
+
+// ESP COMMON HCI CMD
+#define HCI_VENDOR_COMMON_ECHO_CMD_OPCODE HCI_ESP_VENDOR_OPCODE_BUILD(HCI_VENDOR_OGF, HCI_ESP_GROUP_COMMON, HCI_SUBCODE_COMMON_ECHO)
+
+//ESP BLE HCI CMD
 /* Multi adv OCF */
-#define HCI_BLE_MULTI_ADV_OCF     (0x0154 | HCI_GRP_VENDOR_SPECIFIC)
-
+#define HCI_BLE_MULTI_ADV_OCF             HCI_ESP_VENDOR_OPCODE_BUILD(HCI_VENDOR_OGF, HCI_ESP_GROUP_COMMON, HCI_SUBCODE_BLE_MULTI_ADV)
 /* Batch scan OCF */
-#define HCI_BLE_BATCH_SCAN_OCF    (0x0156 | HCI_GRP_VENDOR_SPECIFIC)
-
+#define HCI_BLE_BATCH_SCAN_OCF            HCI_ESP_VENDOR_OPCODE_BUILD(HCI_VENDOR_OGF, HCI_ESP_GROUP_COMMON, HCI_SUBCODE_BLE_BATCH_SCAN)
 /* ADV filter OCF */
-#define HCI_BLE_ADV_FILTER_OCF    (0x0157 | HCI_GRP_VENDOR_SPECIFIC)
-
+#define HCI_BLE_ADV_FILTER_OCF            HCI_ESP_VENDOR_OPCODE_BUILD(HCI_VENDOR_OGF, HCI_ESP_GROUP_COMMON, HCI_SUBCODE_BLE_ADV_FILTER)
 /* Tracking OCF */
-#define HCI_BLE_TRACK_ADV_OCF     (0x0158 | HCI_GRP_VENDOR_SPECIFIC)
-
+#define HCI_BLE_TRACK_ADV_OCF             HCI_ESP_VENDOR_OPCODE_BUILD(HCI_VENDOR_OGF, HCI_ESP_GROUP_COMMON, HCI_SUBCODE_BLE_TRACK_ADV)
 /* Energy info OCF */
-#define HCI_BLE_ENERGY_INFO_OCF   (0x0159 | HCI_GRP_VENDOR_SPECIFIC)
-
+#define HCI_BLE_ENERGY_INFO_OCF           HCI_ESP_VENDOR_OPCODE_BUILD(HCI_VENDOR_OGF, HCI_ESP_GROUP_COMMON, HCI_SUBCODE_BLE_ENERGY_INFO)
 /* Extended BLE Scan parameters OCF */
-#define HCI_BLE_EXTENDED_SCAN_PARAMS_OCF  (0x0160 | HCI_GRP_VENDOR_SPECIFIC)
+#define HCI_BLE_EXTENDED_SCAN_PARAMS_OCF  HCI_ESP_VENDOR_OPCODE_BUILD(HCI_VENDOR_OGF, HCI_ESP_GROUP_COMMON, HCI_SUBCODE_BLE_EXTENDED_SCAN_PARAMS)
+
+//ESP BT HCI CMD
 
 /* subcode for multi adv feature */
 #define BTM_BLE_MULTI_ADV_SET_PARAM                     0x01
diff --git a/components/esp32/ld/esp32.rom.ld b/components/esp32/ld/esp32.rom.ld
index 7b3dc08a4..c1e3e94bc 100644
--- a/components/esp32/ld/esp32.rom.ld
+++ b/components/esp32/ld/esp32.rom.ld
@@ -57,6 +57,7 @@ PROVIDE ( _bss_start_btdm = 0x3ffb8000);
 PROVIDE ( _bss_end_btdm = 0x3ffbff70);
 PROVIDE ( _daylight = 0x3ffae0a4 );
 PROVIDE ( dbg_default_handler = 0x3ff97218 );
+PROVIDE ( dbg_default_state = 0x3ff97220 );
 PROVIDE ( dbg_state = 0x3ffb8d5d );
 PROVIDE ( DebugE256PublicKey_x = 0x3ff97428 );
 PROVIDE ( DebugE256PublicKey_y = 0x3ff97408 );