Abandon std::sort in NodeDB, and associated fixes (#7175)

src/mesh/NodeDB.cpp

@@ -1000,7 +1000,8 @@ void NodeDB::cleanupMeshDB()
                     meshNodes->at(i).user.public_key.size = 0;
                 }
             }
-            meshNodes->at(newPos++) = meshNodes->at(i);
+            if (newPos != i)
+                meshNodes->at(newPos++) = meshNodes->at(i);
         } else {
             removed++;
         }
@@ -1087,8 +1088,8 @@ LoadFileResult NodeDB::loadProto(const char *filename, size_t protoSize, size_t
     if (f) {
         LOG_INFO("Load %s", filename);
         pb_istream_t stream = {&readcb, &f, protoSize};
-
-        memset(dest_struct, 0, objSize);
+        if (fields != &meshtastic_NodeDatabase_msg) // contains a vector object
+            memset(dest_struct, 0, objSize);
         if (!pb_decode(&stream, fields, dest_struct)) {
             LOG_ERROR("Error: can't decode protobuf %s", PB_GET_ERROR(&stream));
             state = LoadFileResult::DECODE_FAILED;
@@ -1156,7 +1157,7 @@ void NodeDB::loadFromDisk()
         LOG_WARN("Node count %d exceeds MAX_NUM_NODES %d, truncating", numMeshNodes, MAX_NUM_NODES);
         numMeshNodes = MAX_NUM_NODES;
     }
-    meshNodes->resize(MAX_NUM_NODES + 1); // The rp2040, rp2035, and maybe other targets, have a problem doing a sort() when full
+    meshNodes->resize(MAX_NUM_NODES);
 
     // static DeviceState scratch; We no longer read into a tempbuf because this structure is 15KB of valuable RAM
     state = loadProto(deviceStateFileName, meshtastic_DeviceState_size, sizeof(meshtastic_DeviceState),
@@ -1694,22 +1695,20 @@ void NodeDB::sortMeshDB()
 {
     if (!Throttle::isWithinTimespanMs(lastSort, 1000 * 5)) {
         lastSort = millis();
-        std::sort(meshNodes->begin(), meshNodes->begin() + numMeshNodes,
-                  [](const meshtastic_NodeInfoLite &a, const meshtastic_NodeInfoLite &b) {
-                      if (a.num == myNodeInfo.my_node_num && b.num == myNodeInfo.my_node_num) // in theory impossible
-                          return false;
-                      if (a.num == myNodeInfo.my_node_num) {
-                          return true;
-                      }
-                      if (b.num == myNodeInfo.my_node_num) {
-                          return false;
-                      }
-                      bool aFav = a.is_favorite;
-                      bool bFav = b.is_favorite;
-                      if (aFav != bFav)
-                          return aFav;
-                      return a.last_heard > b.last_heard;
-                  });
+        bool changed = true;
+        while (changed) { // dumb reverse bubble sort, but probably not bad for what we're doing
+            changed = false;
+            for (int i = numMeshNodes - 1; i > 1; i--) { // lowest case this should examine is i == 2
+                if (meshNodes->at(i).is_favorite && !meshNodes->at(i - 1).is_favorite) {
+                    std::swap(meshNodes->at(i), meshNodes->at(i - 1));
+                    changed = true;
+                } else if (meshNodes->at(i).last_heard > meshNodes->at(i - 1).last_heard) {
+                    std::swap(meshNodes->at(i), meshNodes->at(i - 1));
+                    changed = true;
+                }
+            }
+        }
+        LOG_INFO("Sort took %u milliseconds", millis() - lastSort);
     }
 }
 

src/mesh/NodeDB.h

@@ -208,9 +208,6 @@ class NodeDB
     their denial?)
     */
 
-    /// pick a provisional nodenum we hope no one is using
-    void pickNewNodeNum();
-
     // get channel channel index we heard a nodeNum on, defaults to 0 if not found
     uint8_t getMeshNodeChannel(NodeNum n);
 
@@ -286,6 +283,9 @@ class NodeDB
     /// Find a node in our DB, create an empty NodeInfoLite if missing
     meshtastic_NodeInfoLite *getOrCreateMeshNode(NodeNum n);
 
+    /// pick a provisional nodenum we hope no one is using
+    void pickNewNodeNum();
+
     /// Notify observers of changes to the DB
     void notifyObservers(bool forceUpdate = false)
     {

src/nimble/NimbleBluetooth.cpp

@@ -9,6 +9,7 @@
 #include "mesh/mesh-pb-constants.h"
 #include "sleep.h"
 #include <NimBLEDevice.h>
+#include <mutex>
 
 NimBLECharacteristic *fromNumCharacteristic;
 NimBLECharacteristic *BatteryCharacteristic;
@@ -17,8 +18,36 @@ NimBLEServer *bleServer;
 
 static bool passkeyShowing;
 
-class BluetoothPhoneAPI : public PhoneAPI
+class BluetoothPhoneAPI : public PhoneAPI, public concurrency::OSThread
 {
+  public:
+    BluetoothPhoneAPI() : concurrency::OSThread("NimbleBluetooth") { nimble_queue.resize(3); }
+    std::vector<NimBLEAttValue> nimble_queue;
+    std::mutex nimble_mutex;
+    uint8_t queue_size = 0;
+    bool has_fromRadio = false;
+    uint8_t fromRadioBytes[meshtastic_FromRadio_size] = {0};
+    size_t numBytes = 0;
+    bool hasChecked = false;
+
+  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());
+            }
+            LOG_WARN("Queue_size %u", queue_size);
+            queue_size = 0;
+        }
+        if (hasChecked == false) {
+            numBytes = getFromRadio(fromRadioBytes);
+            hasChecked = true;
+        }
+
+        return 100;
+    }
     /**
      * Subclasses can use this as a hook to provide custom notifications for their transport (i.e. bluetooth notifies)
      */
@@ -51,15 +80,16 @@ class NimbleBluetoothToRadioCallback : public NimBLECharacteristicCallbacks
 {
     virtual void onWrite(NimBLECharacteristic *pCharacteristic)
     {
-        LOG_DEBUG("To Radio onwrite");
         auto val = pCharacteristic->getValue();
 
         if (memcmp(lastToRadio, val.data(), val.length()) != 0) {
-            LOG_DEBUG("New ToRadio packet");
-            memcpy(lastToRadio, val.data(), val.length());
-            bluetoothPhoneAPI->handleToRadio(val.data(), val.length());
-        } else {
-            LOG_DEBUG("Drop dup ToRadio packet we just saw");
+            if (bluetoothPhoneAPI->queue_size < 3) {
+                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++;
+                bluetoothPhoneAPI->setIntervalFromNow(0);
+            }
         }
     }
 };
@@ -68,12 +98,19 @@ class NimbleBluetoothFromRadioCallback : public NimBLECharacteristicCallbacks
 {
     virtual void onRead(NimBLECharacteristic *pCharacteristic)
     {
-        uint8_t fromRadioBytes[meshtastic_FromRadio_size];
-        size_t numBytes = bluetoothPhoneAPI->getFromRadio(fromRadioBytes);
-
-        std::string fromRadioByteString(fromRadioBytes, fromRadioBytes + numBytes);
-
+        while (!bluetoothPhoneAPI->hasChecked) {
+            bluetoothPhoneAPI->setIntervalFromNow(0);
+            delay(20);
+        }
+        std::lock_guard<std::mutex> guard(bluetoothPhoneAPI->nimble_mutex);
+        std::string fromRadioByteString(bluetoothPhoneAPI->fromRadioBytes,
+                                        bluetoothPhoneAPI->fromRadioBytes + bluetoothPhoneAPI->numBytes);
         pCharacteristic->setValue(fromRadioByteString);
+
+        if (bluetoothPhoneAPI->numBytes != 0) // if we did send something, queue it up right away to reload
+            bluetoothPhoneAPI->setIntervalFromNow(0);
+        bluetoothPhoneAPI->numBytes = 0;
+        bluetoothPhoneAPI->hasChecked = false;
     }
 };