From a73a8dc3af84e3fac4c523e25e2ac902a61b292c Mon Sep 17 00:00:00 2001
From: "Earle F. Philhower, III" <earlephilhower@yahoo.com>
Date: Mon, 27 Oct 2025 13:17:13 -0700
Subject: [PATCH] Ensure connectBLE doesn't return before HCI conn

gap_connect() is async, so we need to pause until either it connects or
we timeout so that we can ger an accurate connection state.

Fixes #3221 (again)
---
 libraries/BluetoothHCI/src/BluetoothHCI.h       |  4 ++++
 .../src/BluetoothHIDMaster.cpp                  | 17 ++++++++++++++++-
 2 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/libraries/BluetoothHCI/src/BluetoothHCI.h b/libraries/BluetoothHCI/src/BluetoothHCI.h
index 6d5146dda..2550c3497 100644
--- a/libraries/BluetoothHCI/src/BluetoothHCI.h
+++ b/libraries/BluetoothHCI/src/BluetoothHCI.h
@@ -49,6 +49,10 @@ class BluetoothHCI {
 
     void scanFree(); // Free allocated scan buffers
 
+    bool connected() {
+        return _hciConn != HCI_CON_HANDLE_INVALID;
+    }
+
     friend class BluetoothHIDMaster;
 
 protected:
diff --git a/libraries/BluetoothHIDMaster/src/BluetoothHIDMaster.cpp b/libraries/BluetoothHIDMaster/src/BluetoothHIDMaster.cpp
index d55ed8fc7..045b68ddc 100644
--- a/libraries/BluetoothHIDMaster/src/BluetoothHIDMaster.cpp
+++ b/libraries/BluetoothHIDMaster/src/BluetoothHIDMaster.cpp
@@ -225,7 +225,22 @@ bool BluetoothHIDMaster::connectBLE(const uint8_t *addr, int addrType) {
     }
     uint8_t a[6];
     memcpy(a, addr, sizeof(a));
-    return ERROR_CODE_SUCCESS == gap_connect(a, (bd_addr_type_t)addrType);
+    if (ERROR_CODE_SUCCESS != gap_connect(a, (bd_addr_type_t)addrType)) {
+        return false;
+    }
+    // GAP connection running async.  Wait for HCI connect
+    uint32_t now = millis();
+    while (millis() - now < 5000) {
+        if (_hci.connected()) {
+            break;
+        }
+        delay(25);
+    }
+    if (!_hci.connected()) {
+        gap_connect_cancel();
+        return false;
+    }
+    return _hci.connected();
 }
 
 bool BluetoothHIDMaster::connectBLE() {