Fix NimbleBluetooth reliability and performance (#8385)

* Initial work to get NimbleBluetooth working reliably, and cross-task mutexes cleaned up

* Pre-fill toPhoneQueue when safe (during config/nodeinfo): runOnceToPhoneCanPreloadNextPacket

* Handle 0-byte responses breaking clients during initial config phases

* requestLowerPowerConnection

* PhoneAPI: onConfigStart and onConfigComplete callbacks for subclasses

* NimbleBluetooth: switch to high-throughput BLE mode during config, then lower-power BLE mode for steady-state

* Add some documentation to NimbleBluetooth.cpp

* make cppcheck happier

* Allow runOnceHandleToPhoneQueue to tell runOnce to shouldBreakAndRetryLater, so we don't busy-loop forever in runOnce

* Gating some logging behind DEBUG_NIMBLE_ON_READ_TIMING ifdef again; bump retry count

* Add check for connected state in NimBLE onRead()

---------

Co-authored-by: Jonathan Bennett <jbennett@incomsystems.biz>

src/mesh/PhoneAPI.cpp

@@ -57,6 +57,9 @@ void PhoneAPI::handleStartConfig()
 #endif
     }
 
+    // Allow subclasses to prepare for high-throughput config traffic
+    onConfigStart();
+
     // even if we were already connected - restart our state machine
     if (config_nonce == SPECIAL_NONCE_ONLY_NODES) {
         // If client only wants node info, jump directly to sending nodes
@@ -71,7 +74,7 @@ void PhoneAPI::handleStartConfig()
     spiLock->unlock();
     LOG_DEBUG("Got %d files in manifest", filesManifest.size());
 
-    LOG_INFO("Start API client config");
+    LOG_INFO("Start API client config millis=%u", millis());
     // Protect against concurrent BLE callbacks: they run in NimBLE's FreeRTOS task and also touch nodeInfoQueue.
     {
         concurrency::LockGuard guard(&nodeInfoMutex);
@@ -453,7 +456,10 @@ size_t PhoneAPI::getFromRadio(uint8_t *buf)
         break;
 
     case STATE_SEND_OTHER_NODEINFOS: {
-        LOG_DEBUG("Send known nodes");
+        if (readIndex == 2) { //  readIndex==2 will be true for the first non-us node
+            LOG_INFO("Start sending nodeinfos millis=%u", millis());
+        }
+
         meshtastic_NodeInfo infoToSend = {};
         {
             concurrency::LockGuard guard(&nodeInfoMutex);
@@ -470,13 +476,22 @@ size_t PhoneAPI::getFromRadio(uint8_t *buf)
         if (infoToSend.num != 0) {
             // Just in case we stored a different user.id in the past, but should never happen going forward
             sprintf(infoToSend.user.id, "!%08x", infoToSend.num);
-            LOG_DEBUG("nodeinfo: num=0x%x, lastseen=%u, id=%s, name=%s", infoToSend.num, infoToSend.last_heard,
-                      infoToSend.user.id, infoToSend.user.long_name);
+
+            // Logging this really slows down sending nodes on initial connection because the serial console is so slow, so only
+            // uncomment if you really need to:
+            // LOG_INFO("nodeinfo: num=0x%x, lastseen=%u, id=%s, name=%s", nodeInfoForPhone.num, nodeInfoForPhone.last_heard,
+            // nodeInfoForPhone.user.id, nodeInfoForPhone.user.long_name);
+
+            // Occasional progress logging. (readIndex==2 will be true for the first non-us node)
+            if (readIndex == 2 || readIndex % 20 == 0) {
+                LOG_DEBUG("nodeinfo: %d/%d", readIndex, nodeDB->getNumMeshNodes());
+            }
+
             fromRadioScratch.which_payload_variant = meshtastic_FromRadio_node_info_tag;
             fromRadioScratch.node_info = infoToSend;
             prefetchNodeInfos();
         } else {
-            LOG_DEBUG("Done sending nodeinfo");
+            LOG_DEBUG("Done sending %d of %d nodeinfos millis=%u", readIndex, nodeDB->getNumMeshNodes(), millis());
             concurrency::LockGuard guard(&nodeInfoMutex);
             nodeInfoQueue.clear();
             state = STATE_SEND_FILEMANIFEST;
@@ -558,11 +573,15 @@ size_t PhoneAPI::getFromRadio(uint8_t *buf)
 
 void PhoneAPI::sendConfigComplete()
 {
-    LOG_INFO("Config Send Complete");
+    LOG_INFO("Config Send Complete millis=%u", millis());
     fromRadioScratch.which_payload_variant = meshtastic_FromRadio_config_complete_id_tag;
     fromRadioScratch.config_complete_id = config_nonce;
     config_nonce = 0;
     state = STATE_SEND_PACKETS;
+
+    // Allow subclasses to know we've entered steady-state so they can lower power consumption
+    onConfigComplete();
+
     pauseBluetoothLogging = false;
 }
 

src/mesh/PhoneAPI.h

@@ -136,6 +136,7 @@ class PhoneAPI
     bool available();
 
     bool isConnected() { return state != STATE_SEND_NOTHING; }
+    bool isSendingPackets() { return state == STATE_SEND_PACKETS; }
 
   protected:
     /// Our fromradio packet while it is being assembled
@@ -158,6 +159,11 @@ class PhoneAPI
      */
     virtual void onNowHasData(uint32_t fromRadioNum) {}
 
+    /// Subclasses can use these lifecycle hooks for transport-specific behavior around config/steady-state
+    /// (i.e. BLE connection params)
+    virtual void onConfigStart() {}
+    virtual void onConfigComplete() {}
+
     /// begin a new connection
     void handleStartConfig();
 

src/nimble/NimbleBluetooth.cpp

@@ -3,12 +3,15 @@
 #include "BluetoothCommon.h"
 #include "NimbleBluetooth.h"
 #include "PowerFSM.h"
+#include "StaticPointerQueue.h"
 
+#include "concurrency/OSThread.h"
 #include "main.h"
 #include "mesh/PhoneAPI.h"
 #include "mesh/mesh-pb-constants.h"
 #include "sleep.h"
 #include <NimBLEDevice.h>
+#include <atomic>
 #include <mutex>
 
 #ifdef NIMBLE_TWO
@@ -32,45 +35,288 @@ constexpr uint16_t kPreferredBleTxTimeUs = (kPreferredBleTxOctets + 14) * 8;
 } // namespace
 #endif
 
+// Debugging options: careful, they slow things down quite a bit!
+// #define DEBUG_NIMBLE_ON_READ_TIMING  // uncomment to time onRead duration
+// #define DEBUG_NIMBLE_ON_WRITE_TIMING // uncomment to time onWrite duration
+// #define DEBUG_NIMBLE_NOTIFY          // uncomment to enable notify logging
+
+#define NIMBLE_BLUETOOTH_TO_PHONE_QUEUE_SIZE 3
+#define NIMBLE_BLUETOOTH_FROM_PHONE_QUEUE_SIZE 3
+
 NimBLECharacteristic *fromNumCharacteristic;
 NimBLECharacteristic *BatteryCharacteristic;
 NimBLECharacteristic *logRadioCharacteristic;
 NimBLEServer *bleServer;
 
 static bool passkeyShowing;
+static std::atomic<int32_t> nimbleBluetoothConnHandle{-1}; // actual handles are uint16_t, so -1 means "no connection"
 
 class BluetoothPhoneAPI : public PhoneAPI, public concurrency::OSThread
 {
+    /*
+      CAUTION: There's a lot going on here and lots of room to break things.
+
+      This NimbleBluetooth.cpp file does some tricky synchronization between the NimBLE FreeRTOS task (which runs the onRead and
+      onWrite callbacks) and the main task (which runs runOnce and the rest of PhoneAPI).
+
+      The main idea is to add a little bit of synchronization here to make it so that the rest of the codebase doesn't have to
+      know about concurrency and mutexes, and can just run happily ever after as a cooperative multitasking OSThread system, where
+      locking isn't something that anyone has to worry about too much! :)
+
+      We achieve this by having some queues and mutexes in this file only, and ensuring that all calls to getFromRadio and
+      handleToRadio are only made from the main FreeRTOS task. This way, the rest of the codebase doesn't have to worry about
+      being run concurrently, which would make everything else much much much more complicated.
+
+      PHONE -> RADIO:
+        - [NimBLE FreeRTOS task:] onWrite callback holds fromPhoneMutex and pushes received packets into fromPhoneQueue.
+        - [Main task:] runOnceHandleFromPhoneQueue in main task holds fromPhoneMutex, pulls packets from fromPhoneQueue, and calls
+      handleToRadio **in main task**.
+
+      RADIO -> PHONE:
+        - [NimBLE FreeRTOS task:] onRead callback sets onReadCallbackIsWaitingForData flag and polls in a busy loop. (unless
+      there's already a packet waiting in toPhoneQueue)
+        - [Main task:] runOnceHandleToPhoneQueue sees onReadCallbackIsWaitingForData flag, calls getFromRadio **in main task** to
+      get packets from radio, holds toPhoneMutex, pushes the packet into toPhoneQueue, and clears the
+      onReadCallbackIsWaitingForData flag.
+        - [NimBLE FreeRTOS task:] onRead callback sees that the onReadCallbackIsWaitingForData flag cleared, holds toPhoneMutex,
+      pops the packet from toPhoneQueue, and returns it to NimBLE.
+
+      MUTEXES:
+        - fromPhoneMutex protects fromPhoneQueue and fromPhoneQueueSize
+        - toPhoneMutex protects toPhoneQueue, toPhoneQueueByteSizes, and toPhoneQueueSize
+
+      ATOMICS:
+        - fromPhoneQueueSize is only increased by onWrite, and only decreased by runOnceHandleFromPhoneQueue (or onDisconnect).
+        - toPhoneQueueSize is only increased by runOnceHandleToPhoneQueue, and only decreased by onRead (or onDisconnect).
+        - onReadCallbackIsWaitingForData is a flag. It's only set by onRead, and only cleared by runOnceHandleToPhoneQueue (or
+      onDisconnect).
+
+      PRELOADING: see comments in runOnceToPhoneCanPreloadNextPacket about when it's safe to preload packets from getFromRadio.
+
+      BLE CONNECTION PARAMS:
+        - During config, we request a high-throughput, low-latency BLE connection for speed.
+        - After config, we switch to a lower-power BLE connection for steady-state use to extend battery life.
+
+      MEMORY MANAGEMENT:
+        - We keep packets on the stack and do not allocate heap.
+        - We use std::array for fromPhoneQueue and toPhoneQueue to avoid mallocs and frees across FreeRTOS tasks.
+        - Yes, we have to do some copy operations on pop because of this, but it's worth it to avoid cross-task memory management.
+
+      NOTIFY IS BROKEN:
+        - Adding NIMBLE_PROPERTY::NOTIFY to FromRadioCharacteristic appears to break things. It is NOT backwards compatible.
+
+      ZERO-SIZE READS:
+        - Returning a zero-size read from onRead breaks some clients during the config phase. So we have to block onRead until we
+      have data.
+        - During the STATE_SEND_PACKETS phase, it's totally OK to return zero-size reads, as clients are expected to do reads
+      until they get a 0-byte response.
+
+      CROSS-TASK WAKEUP:
+        - If you call: bluetoothPhoneAPI->setIntervalFromNow(0); to schedule immediate processing of new data,
+        - Then you should also call: concurrency::mainDelay.interrupt(); to wake up the main loop if it's sleeping.
+        - Otherwise, you're going to wait ~100ms or so until the main loop wakes up from some other cause.
+    */
+
   public:
-    BluetoothPhoneAPI() : concurrency::OSThread("NimbleBluetooth") { nimble_queue.resize(3); }
-    std::vector<NimBLEAttValue> nimble_queue;
-    std::mutex nimble_mutex;
-    uint8_t queue_size = 0;
-    uint8_t fromRadioBytes[meshtastic_FromRadio_size] = {0};
-    size_t numBytes = 0;
-    bool hasChecked = false;
-    bool phoneWants = false;
+    BluetoothPhoneAPI() : concurrency::OSThread("NimbleBluetooth") {}
+
+    /* Packets from phone (BLE onWrite callback) */
+    std::mutex fromPhoneMutex;
+    std::atomic<size_t> fromPhoneQueueSize{0};
+    // We use array here (and pay the cost of memcpy) to avoid dynamic memory allocations and frees across FreeRTOS tasks.
+    std::array<NimBLEAttValue, NIMBLE_BLUETOOTH_FROM_PHONE_QUEUE_SIZE> fromPhoneQueue{};
+
+    /* Packets to phone (BLE onRead callback) */
+    std::mutex toPhoneMutex;
+    std::atomic<size_t> toPhoneQueueSize{0};
+    // We use array here (and pay the cost of memcpy) to avoid dynamic memory allocations and frees across FreeRTOS tasks.
+    std::array<std::array<uint8_t, meshtastic_FromRadio_size>, NIMBLE_BLUETOOTH_TO_PHONE_QUEUE_SIZE> toPhoneQueue{};
+    std::array<size_t, NIMBLE_BLUETOOTH_TO_PHONE_QUEUE_SIZE> toPhoneQueueByteSizes{};
+    // The onReadCallbackIsWaitingForData flag provides synchronization between the NimBLE task's onRead callback and our main
+    // task's runOnce. It's only set by onRead, and only cleared by runOnce.
+    std::atomic<bool> onReadCallbackIsWaitingForData{false};
+
+    /* Statistics/logging helpers */
+    std::atomic<int32_t> readCount{0};
+    std::atomic<int32_t> notifyCount{0};
+    std::atomic<int32_t> writeCount{0};
 
   protected:
     virtual int32_t runOnce() override
     {
-        std::lock_guard<std::mutex> guard(nimble_mutex);
-        if (queue_size > 0) {
-            for (uint8_t i = 0; i < queue_size; i++) {
-                handleToRadio(nimble_queue.at(i).data(), nimble_queue.at(i).length());
+        bool shouldBreakAndRetryLater = false;
+
+        while (runOnceHasWorkToDo()) {
+            // Important that we service onRead first, because the onRead callback blocks NimBLE until we clear
+            // onReadCallbackIsWaitingForData.
+            shouldBreakAndRetryLater = runOnceHandleToPhoneQueue(); // push data from getFromRadio to onRead
+            runOnceHandleFromPhoneQueue();                          // pull data from onWrite to handleToRadio
+
+            if (shouldBreakAndRetryLater) {
+                // onRead still wants data, but it's not available yet. Return so we can try again when a packet may be ready.
+#ifdef DEBUG_NIMBLE_ON_READ_TIMING
+                LOG_INFO("BLE runOnce breaking to retry later (leaving onRead waiting)");
+#endif
+                return 100; // try again in 100ms
             }
-            LOG_DEBUG("Queue_size %u", queue_size);
-            queue_size = 0;
-        }
-        if (!hasChecked && phoneWants) {
-            // Pull fresh data while we're outside of the NimBLE callback context.
-            numBytes = getFromRadio(fromRadioBytes);
-            hasChecked = true;
         }
 
         // the run is triggered via NimbleBluetoothToRadioCallback and NimbleBluetoothFromRadioCallback
         return INT32_MAX;
     }
+
+    virtual void onConfigStart() override
+    {
+        LOG_INFO("BLE onConfigStart");
+
+        // Prefer high throughput during config/setup, at the cost of high power consumption (for a few seconds)
+        if (bleServer && isConnected()) {
+            int32_t conn_handle = nimbleBluetoothConnHandle.load();
+            if (conn_handle != -1) {
+                requestHighThroughputConnection(static_cast<uint16_t>(conn_handle));
+            }
+        }
+    }
+
+    virtual void onConfigComplete() override
+    {
+        LOG_INFO("BLE onConfigComplete");
+
+        // Switch to lower power consumption BLE connection params for steady-state use after config/setup is complete
+        if (bleServer && isConnected()) {
+            int32_t conn_handle = nimbleBluetoothConnHandle.load();
+            if (conn_handle != -1) {
+                requestLowerPowerConnection(static_cast<uint16_t>(conn_handle));
+            }
+        }
+    }
+
+    bool runOnceHasWorkToDo() { return runOnceHasWorkToPhone() || runOnceHasWorkFromPhone(); }
+
+    bool runOnceHasWorkToPhone() { return onReadCallbackIsWaitingForData || runOnceToPhoneCanPreloadNextPacket(); }
+
+    bool runOnceToPhoneCanPreloadNextPacket()
+    {
+        /*
+         * PRELOADING getFromRadio RESPONSES:
+         *
+         * It's not safe to preload packets if we're in STATE_SEND_PACKETS, because there may be a while between the time we call
+         * getFromRadio and when the client actually reads it. If the connection drops in that time, we might lose that packet
+         * forever. In STATE_SEND_PACKETS, if we wait for onRead before we call getFromRadio, we minimize the time window where
+         * the client might disconnect before completing the read.
+         *
+         * However, if we're in the setup states (sending config, nodeinfo, etc), it's safe and beneficial to preload packets into
+         * toPhoneQueue because the client will just reconnect after a disconnect, losing nothing.
+         */
+
+        if (!isConnected()) {
+            return false;
+        } else if (isSendingPackets()) {
+            // If we're in STATE_SEND_PACKETS, we must wait for onRead before calling getFromRadio.
+            return false;
+        } else {
+            // In other states, we can preload as long as there's space in the toPhoneQueue.
+            return toPhoneQueueSize < NIMBLE_BLUETOOTH_TO_PHONE_QUEUE_SIZE;
+        }
+    }
+
+    bool runOnceHandleToPhoneQueue()
+    {
+        // Returns false normally.
+        // Returns true if we should break out of runOnce and retry later, such as setup states where getFromRadio returns 0
+        // bytes.
+
+        // Stack buffer for getFromRadio packet
+        uint8_t fromRadioBytes[meshtastic_FromRadio_size] = {0};
+        size_t numBytes = 0;
+
+        if (onReadCallbackIsWaitingForData || runOnceToPhoneCanPreloadNextPacket()) {
+            numBytes = getFromRadio(fromRadioBytes);
+
+            if (numBytes == 0) {
+                // Client expected a read, but we have nothing to send.
+                // Returning a 0-byte packet breaks clients during the config phase, so we have to block onRead until there's a
+                // packet ready.
+                if (isSendingPackets()) {
+                    // In STATE_SEND_PACKETS, it is 100% OK to return a 0-byte response, as we expect clients to do read beyond
+                    // notifies regularly, to make sure they have nothing else to read.
+#ifdef DEBUG_NIMBLE_ON_READ_TIMING
+                    LOG_DEBUG("BLE getFromRadio returned numBytes=0, but in STATE_SEND_PACKETS, so clearing "
+                              "onReadCallbackIsWaitingForData flag");
+#endif
+                } else {
+                    // In other states, this breaks clients.
+                    // Return early, leaving onReadCallbackIsWaitingForData==true so onRead knows to try again.
+                    // This gives runOnce a chance to handleToRadio and produce a response.
+#ifdef DEBUG_NIMBLE_ON_READ_TIMING
+                    LOG_DEBUG("BLE getFromRadio returned numBytes=0. Blocking onRead until we have data");
+#endif
+
+                    // Return true to tell runOnce to shouldBreakAndRetryLater, so we don't busy-loop in runOnce even though
+                    // onRead is still waiting!
+                    return true;
+                }
+            } else {
+                // Push to toPhoneQueue, protected by toPhoneMutex. Hold the mutex as briefly as possible.
+                if (toPhoneQueueSize < NIMBLE_BLUETOOTH_TO_PHONE_QUEUE_SIZE) {
+                    // Note: the comparison above is safe without a mutex because we are the only method that *increases*
+                    // toPhoneQueueSize. (It's okay if toPhoneQueueSize *decreases* in the NimBLE task meanwhile.)
+
+                    { // scope for toPhoneMutex mutex
+                        std::lock_guard<std::mutex> guard(toPhoneMutex);
+                        size_t storeAtIndex = toPhoneQueueSize.load();
+                        memcpy(toPhoneQueue[storeAtIndex].data(), fromRadioBytes, numBytes);
+                        toPhoneQueueByteSizes[storeAtIndex] = numBytes;
+                        toPhoneQueueSize++;
+                    }
+#ifdef DEBUG_NIMBLE_ON_READ_TIMING
+                    LOG_DEBUG("BLE getFromRadio returned numBytes=%u, pushed toPhoneQueueSize=%u", numBytes,
+                              toPhoneQueueSize.load());
+#endif
+                } else {
+                    // Shouldn't happen because the onRead callback shouldn't be waiting if the queue is full!
+                    LOG_ERROR("Shouldn't happen! Drop FromRadio packet, toPhoneQueue full (%u bytes)", numBytes);
+                }
+            }
+
+            // Clear the onReadCallbackIsWaitingForData flag so onRead knows it can proceed.
+            onReadCallbackIsWaitingForData = false; // only clear this flag AFTER the push
+        }
+
+        return false;
+    }
+
+    bool runOnceHasWorkFromPhone() { return fromPhoneQueueSize > 0; }
+
+    void runOnceHandleFromPhoneQueue()
+    {
+        // Handle packets we received from onWrite from the phone.
+        if (fromPhoneQueueSize > 0) {
+            // Note: the comparison above is safe without a mutex because we are the only method that *decreases*
+            // fromPhoneQueueSize. (It's okay if fromPhoneQueueSize *increases* in the NimBLE task meanwhile.)
+
+            LOG_DEBUG("NimbleBluetooth: handling ToRadio packet, fromPhoneQueueSize=%u", fromPhoneQueueSize.load());
+
+            // Pop the front of fromPhoneQueue, holding the mutex only briefly while we pop.
+            NimBLEAttValue val;
+            { // scope for fromPhoneMutex mutex
+                std::lock_guard<std::mutex> guard(fromPhoneMutex);
+                val = fromPhoneQueue[0];
+
+                // Shift the rest of the queue down
+                for (uint8_t i = 1; i < fromPhoneQueueSize; i++) {
+                    fromPhoneQueue[i - 1] = fromPhoneQueue[i];
+                }
+
+                // Safe decrement due to onDisconnect
+                if (fromPhoneQueueSize > 0)
+                    fromPhoneQueueSize--;
+            }
+
+            handleToRadio(val.data(), val.length());
+        }
+    }
+
     /**
      * Subclasses can use this as a hook to provide custom notifications for their transport (i.e. bluetooth notifies)
      */
@@ -78,14 +324,22 @@ class BluetoothPhoneAPI : public PhoneAPI, public concurrency::OSThread
     {
         PhoneAPI::onNowHasData(fromRadioNum);
 
+        int currentNotifyCount = notifyCount.fetch_add(1);
+
         uint8_t cc = bleServer->getConnectedCount();
-        LOG_DEBUG("BLE notify fromNum: %d connections: %d", fromRadioNum, cc);
+
+#ifdef DEBUG_NIMBLE_NOTIFY
+        // This logging slows things down when there are lots of packets going to the phone, like initial connection:
+        LOG_DEBUG("BLE notify(%d) fromNum: %d connections: %d", currentNotifyCount, fromRadioNum, cc);
+#endif
 
         uint8_t val[4];
         put_le32(val, fromRadioNum);
 
         fromNumCharacteristic->setValue(val, sizeof(val));
 #ifdef NIMBLE_TWO
+        // NOTE: I don't have any NIMBLE_TWO devices, but this line makes me suspicious, and I suspect it needs to just be
+        // notify().
         fromNumCharacteristic->notify(val, sizeof(val), BLE_HS_CONN_HANDLE_NONE);
 #else
         fromNumCharacteristic->notify();
@@ -94,6 +348,54 @@ class BluetoothPhoneAPI : public PhoneAPI, public concurrency::OSThread
 
     /// Check the current underlying physical link to see if the client is currently connected
     virtual bool checkIsConnected() { return bleServer && bleServer->getConnectedCount() > 0; }
+
+    void requestHighThroughputConnection(uint16_t conn_handle)
+    {
+        /* Request a lower-latency, higher-throughput BLE connection.
+
+        This comes at the cost of higher power consumption, so we may want to only use this for initial setup, and then switch to
+        a slower mode.
+
+        See https://developer.apple.com/library/archive/qa/qa1931/_index.html for formulas to calculate values, iOS/macOS
+        constraints, and recommendations. (Android doesn't have specific constraints, but seems to be compatible with the Apple
+        recommendations.)
+
+        Selected settings:
+            minInterval (units of 1.25ms): 7.5ms = 6 (lower than the Apple recommended minimum, but allows faster when the client
+        supports it.)
+            maxInterval (units of 1.25ms): 15ms = 12
+            latency: 0 (don't allow peripheral to skip any connection events)
+            timeout (units of 10ms): 6 seconds = 600 (supervision timeout)
+
+        These are intentionally aggressive to prioritize speed over power consumption, but are only used for a few seconds at
+        setup. Not worth adjusting much.
+        */
+        LOG_INFO("BLE requestHighThroughputConnection");
+        bleServer->updateConnParams(conn_handle, 6, 12, 0, 600);
+    }
+
+    void requestLowerPowerConnection(uint16_t conn_handle)
+    {
+        /* Request a lower power consumption (but higher latency, lower throughput) BLE connection.
+
+        This is suitable for steady-state operation after initial setup is complete.
+
+        See https://developer.apple.com/library/archive/qa/qa1931/_index.html for formulas to calculate values, iOS/macOS
+        constraints, and recommendations. (Android doesn't have specific constraints, but seems to be compatible with the Apple
+        recommendations.)
+
+        Selected settings:
+            minInterval (units of 1.25ms): 30ms = 24
+            maxInterval (units of 1.25ms): 50ms = 40
+            latency: 2 (allow peripheral to skip up to 2 consecutive connection events to save power)
+            timeout (units of 10ms): 6 seconds = 600 (supervision timeout)
+
+        There's an opportunity for tuning here if anyone wants to do some power measurements, but these should allow 10-20 packets
+        per second.
+        */
+        LOG_INFO("BLE requestLowerPowerConnection");
+        bleServer->updateConnParams(conn_handle, 24, 40, 2, 600);
+    }
 };
 
 static BluetoothPhoneAPI *bluetoothPhoneAPI;
@@ -113,18 +415,45 @@ class NimbleBluetoothToRadioCallback : public NimBLECharacteristicCallbacks
 
 #endif
     {
+        // CAUTION: This callback runs in the NimBLE task!!! Don't do anything except communicate with the main task's runOnce.
+        // Assumption: onWrite is serialized by NimBLE, so we don't need to lock here against multiple concurrent onWrite calls.
+
+        int currentWriteCount = bluetoothPhoneAPI->writeCount.fetch_add(1);
+
+#ifdef DEBUG_NIMBLE_ON_WRITE_TIMING
+        int startMillis = millis();
+        LOG_DEBUG("BLE onWrite(%d): start millis=%d", currentWriteCount, startMillis);
+#endif
+
         auto val = pCharacteristic->getValue();
 
         if (memcmp(lastToRadio, val.data(), val.length()) != 0) {
-            if (bluetoothPhoneAPI->queue_size < 3) {
+            if (bluetoothPhoneAPI->fromPhoneQueueSize < NIMBLE_BLUETOOTH_FROM_PHONE_QUEUE_SIZE) {
+                // Note: the comparison above is safe without a mutex because we are the only method that *increases*
+                // fromPhoneQueueSize. (It's okay if fromPhoneQueueSize *decreases* in the main task meanwhile.)
                 memcpy(lastToRadio, val.data(), val.length());
-                std::lock_guard<std::mutex> guard(bluetoothPhoneAPI->nimble_mutex);
-                bluetoothPhoneAPI->nimble_queue.at(bluetoothPhoneAPI->queue_size) = val;
-                bluetoothPhoneAPI->queue_size++;
+
+                { // scope for fromPhoneMutex mutex
+                    // Append to fromPhoneQueue, protected by fromPhoneMutex. Hold the mutex as briefly as possible.
+                    std::lock_guard<std::mutex> guard(bluetoothPhoneAPI->fromPhoneMutex);
+                    bluetoothPhoneAPI->fromPhoneQueue.at(bluetoothPhoneAPI->fromPhoneQueueSize) = val;
+                    bluetoothPhoneAPI->fromPhoneQueueSize++;
+                }
+
+                // After releasing the mutex, schedule immediate processing of the new packet.
                 bluetoothPhoneAPI->setIntervalFromNow(0);
+                concurrency::mainDelay.interrupt(); // wake up main loop if sleeping
+
+#ifdef DEBUG_NIMBLE_ON_WRITE_TIMING
+                int finishMillis = millis();
+                LOG_DEBUG("BLE onWrite(%d): append to fromPhoneQueue took %u ms. numBytes=%d", currentWriteCount,
+                          finishMillis - startMillis, val.length());
+#endif
+            } else {
+                LOG_WARN("BLE onWrite(%d): Drop ToRadio packet, fromPhoneQueue full (%u bytes)", currentWriteCount, val.length());
             }
         } else {
-            LOG_DEBUG("Drop duplicate ToRadio packet (%u bytes)", val.length());
+            LOG_DEBUG("BLE onWrite(%d): Drop duplicate ToRadio packet (%u bytes)", currentWriteCount, val.length());
         }
     }
 };
@@ -137,32 +466,111 @@ class NimbleBluetoothFromRadioCallback : public NimBLECharacteristicCallbacks
     virtual void onRead(NimBLECharacteristic *pCharacteristic)
 #endif
     {
-        bluetoothPhoneAPI->phoneWants = true;
-        bluetoothPhoneAPI->setIntervalFromNow(0);
-        std::lock_guard<std::mutex> guard(bluetoothPhoneAPI->nimble_mutex); // BLE callbacks run in NimBLE task
+        // In some cases, it seems a new connection starts with a read.
+        // The API has no bytes to send, leading to a timeout. This short-circuits this problem.
+        if (!bluetoothPhoneAPI->isConnected())
+            return;
+        // CAUTION: This callback runs in the NimBLE task!!! Don't do anything except communicate with the main task's runOnce.
 
-        if (!bluetoothPhoneAPI->hasChecked) {
-            // Fetch payload on demand; prefetch keeps this fast for the first read.
-            bluetoothPhoneAPI->numBytes = bluetoothPhoneAPI->getFromRadio(bluetoothPhoneAPI->fromRadioBytes);
-            bluetoothPhoneAPI->hasChecked = true;
-        }
+        int currentReadCount = bluetoothPhoneAPI->readCount.fetch_add(1);
+        int tries = 0;
+        int startMillis = millis();
 
-        pCharacteristic->setValue(bluetoothPhoneAPI->fromRadioBytes, bluetoothPhoneAPI->numBytes);
-
-        if (bluetoothPhoneAPI->numBytes != 0) {
-#ifdef NIMBLE_TWO
-            // Notify immediately so subscribed clients see the packet without an extra read.
-            pCharacteristic->notify(bluetoothPhoneAPI->fromRadioBytes, bluetoothPhoneAPI->numBytes, BLE_HS_CONN_HANDLE_NONE);
-#else
-            pCharacteristic->notify();
+#ifdef DEBUG_NIMBLE_ON_READ_TIMING
+        LOG_DEBUG("BLE onRead(%d): start millis=%d", currentReadCount, startMillis);
 #endif
+
+        // Is there a packet ready to go, or do we have to ask the main task to get one for us?
+        if (bluetoothPhoneAPI->toPhoneQueueSize > 0) {
+            // Note: the comparison above is safe without a mutex because we are the only method that *decreases*
+            // toPhoneQueueSize. (It's okay if toPhoneQueueSize *increases* in the main task meanwhile.)
+
+            // There's already a packet queued. Great! We don't need to wait for onReadCallbackIsWaitingForData.
+#ifdef DEBUG_NIMBLE_ON_READ_TIMING
+            LOG_DEBUG("BLE onRead(%d): packet already waiting, no need to set onReadCallbackIsWaitingForData", currentReadCount);
+#endif
+        } else {
+            // Tell the main task that we'd like a packet.
+            bluetoothPhoneAPI->onReadCallbackIsWaitingForData = true;
+
+            // Wait for the main task to produce a packet for us, up to about 20 seconds.
+            // It normally takes just a few milliseconds, but at initial startup, etc, the main task can get blocked for longer
+            // doing various setup tasks.
+            while (bluetoothPhoneAPI->onReadCallbackIsWaitingForData && tries < 4000) {
+                // Schedule the main task runOnce to run ASAP.
+                bluetoothPhoneAPI->setIntervalFromNow(0);
+                concurrency::mainDelay.interrupt(); // wake up main loop if sleeping
+
+                if (!bluetoothPhoneAPI->onReadCallbackIsWaitingForData) {
+                    // we may be able to break even before a delay, if the call to interrupt woke up the main loop and it ran
+                    // already
+#ifdef DEBUG_NIMBLE_ON_READ_TIMING
+                    LOG_DEBUG("BLE onRead(%d): broke before delay after %u ms, %d tries", currentReadCount,
+                              millis() - startMillis, tries);
+#endif
+                    break;
+                }
+
+                // This delay happens in the NimBLE FreeRTOS task, which really can't do anything until we get a value back.
+                // No harm in polling pretty frequently.
+                delay(tries < 20 ? 1 : 5);
+                tries++;
+
+                if (tries == 4000) {
+                    LOG_WARN(
+                        "BLE onRead(%d): timeout waiting for data after %u ms, %d tries, giving up and returning 0-size response",
+                        currentReadCount, millis() - startMillis, tries);
+                }
+            }
         }
 
-        if (bluetoothPhoneAPI->numBytes != 0) // if we did send something, queue it up right away to reload
+        // Pop from toPhoneQueue, protected by toPhoneMutex. Hold the mutex as briefly as possible.
+        uint8_t fromRadioBytes[meshtastic_FromRadio_size] = {0}; // Stack buffer for getFromRadio packet
+        size_t numBytes = 0;
+        { // scope for toPhoneMutex mutex
+            std::lock_guard<std::mutex> guard(bluetoothPhoneAPI->toPhoneMutex);
+            size_t toPhoneQueueSize = bluetoothPhoneAPI->toPhoneQueueSize.load();
+            if (toPhoneQueueSize > 0) {
+                // Copy from the front of the toPhoneQueue
+                memcpy(fromRadioBytes, bluetoothPhoneAPI->toPhoneQueue[0].data(), bluetoothPhoneAPI->toPhoneQueueByteSizes[0]);
+                numBytes = bluetoothPhoneAPI->toPhoneQueueByteSizes[0];
+
+                // Shift the rest of the queue down
+                for (uint8_t i = 1; i < toPhoneQueueSize; i++) {
+                    memcpy(bluetoothPhoneAPI->toPhoneQueue[i - 1].data(), bluetoothPhoneAPI->toPhoneQueue[i].data(),
+                           bluetoothPhoneAPI->toPhoneQueueByteSizes[i]);
+                    // The above line is similar to:
+                    //   bluetoothPhoneAPI->toPhoneQueue[i - 1] = bluetoothPhoneAPI->toPhoneQueue[i]
+                    // but is usually faster because it doesn't have to copy all the trailing bytes beyond
+                    // toPhoneQueueByteSizes[i].
+                    //
+                    // We deliberately use an array here (and pay the CPU cost of some memcpy) to avoid synchronizing dynamic
+                    // memory allocations and frees across FreeRTOS tasks.
+
+                    bluetoothPhoneAPI->toPhoneQueueByteSizes[i - 1] = bluetoothPhoneAPI->toPhoneQueueByteSizes[i];
+                }
+
+                // Safe decrement due to onDisconnect
+                if (bluetoothPhoneAPI->toPhoneQueueSize > 0)
+                    bluetoothPhoneAPI->toPhoneQueueSize--;
+            } else {
+                // nothing in the toPhoneQueue; that's fine, and we'll just have numBytes=0.
+            }
+        }
+
+#ifdef DEBUG_NIMBLE_ON_READ_TIMING
+        int finishMillis = millis();
+        LOG_DEBUG("BLE onRead(%d): onReadCallbackIsWaitingForData took %u ms, %d tries. numBytes=%d", currentReadCount,
+                  finishMillis - startMillis, tries, numBytes);
+#endif
+
+        pCharacteristic->setValue(fromRadioBytes, numBytes);
+
+        // If we sent something, wake up the main loop if it's sleeping in case there are more packets ready to enqueue.
+        if (numBytes != 0) {
             bluetoothPhoneAPI->setIntervalFromNow(0);
-        bluetoothPhoneAPI->numBytes = 0;
-        bluetoothPhoneAPI->hasChecked = false;
-        bluetoothPhoneAPI->phoneWants = false;
+            concurrency::mainDelay.interrupt(); // wake up main loop if sleeping
+        }
     }
 };
 
@@ -244,6 +652,13 @@ class NimbleBluetoothServerCallback : public NimBLEServerCallbacks
             if (screen)
                 screen->endAlert();
         }
+
+        // Store the connection handle for future use
+#ifdef NIMBLE_TWO
+        nimbleBluetoothConnHandle = connInfo.getConnHandle();
+#else
+        nimbleBluetoothConnHandle = desc->conn_handle;
+#endif
     }
 
 #ifdef NIMBLE_TWO
@@ -290,16 +705,29 @@ class NimbleBluetoothServerCallback : public NimBLEServerCallbacks
         bluetoothStatus->updateStatus(&newStatus);
 
         if (bluetoothPhoneAPI) {
-            std::lock_guard<std::mutex> guard(bluetoothPhoneAPI->nimble_mutex);
             bluetoothPhoneAPI->close();
-            bluetoothPhoneAPI->numBytes = 0;
-            bluetoothPhoneAPI->queue_size = 0;
-            bluetoothPhoneAPI->hasChecked = false;
-            bluetoothPhoneAPI->phoneWants = false;
+
+            { // scope for fromPhoneMutex mutex
+                std::lock_guard<std::mutex> guard(bluetoothPhoneAPI->fromPhoneMutex);
+                bluetoothPhoneAPI->fromPhoneQueueSize = 0;
+            }
+
+            bluetoothPhoneAPI->onReadCallbackIsWaitingForData = false;
+            { // scope for toPhoneMutex mutex
+                std::lock_guard<std::mutex> guard(bluetoothPhoneAPI->toPhoneMutex);
+                bluetoothPhoneAPI->toPhoneQueueSize = 0;
+            }
+
+            bluetoothPhoneAPI->readCount = 0;
+            bluetoothPhoneAPI->notifyCount = 0;
+            bluetoothPhoneAPI->writeCount = 0;
         }
 
         // Clear the last ToRadio packet buffer to avoid rejecting first packet from new connection
         memset(lastToRadio, 0, sizeof(lastToRadio));
+
+        nimbleBluetoothConnHandle = -1; // -1 means "no connection"
+
 #ifdef NIMBLE_TWO
         // Restart Advertising
         ble->startAdvertising();
@@ -436,17 +864,15 @@ void NimbleBluetooth::setupService()
     if (config.bluetooth.mode == meshtastic_Config_BluetoothConfig_PairingMode_NO_PIN) {
         ToRadioCharacteristic = bleService->createCharacteristic(TORADIO_UUID, NIMBLE_PROPERTY::WRITE);
         // Allow notifications so phones can stream FromRadio without polling.
-        FromRadioCharacteristic =
-            bleService->createCharacteristic(FROMRADIO_UUID, NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY);
+        FromRadioCharacteristic = bleService->createCharacteristic(FROMRADIO_UUID, NIMBLE_PROPERTY::READ);
         fromNumCharacteristic = bleService->createCharacteristic(FROMNUM_UUID, NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::READ);
         logRadioCharacteristic =
             bleService->createCharacteristic(LOGRADIO_UUID, NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::READ, 512U);
     } else {
         ToRadioCharacteristic = bleService->createCharacteristic(
             TORADIO_UUID, NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::WRITE_AUTHEN | NIMBLE_PROPERTY::WRITE_ENC);
-        FromRadioCharacteristic =
-            bleService->createCharacteristic(FROMRADIO_UUID, NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::READ_AUTHEN |
-                                                                 NIMBLE_PROPERTY::READ_ENC | NIMBLE_PROPERTY::NOTIFY);
+        FromRadioCharacteristic = bleService->createCharacteristic(
+            FROMRADIO_UUID, NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::READ_AUTHEN | NIMBLE_PROPERTY::READ_ENC);
         fromNumCharacteristic =
             bleService->createCharacteristic(FROMNUM_UUID, NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::READ |
                                                                NIMBLE_PROPERTY::READ_AUTHEN | NIMBLE_PROPERTY::READ_ENC);