Send the important frames first

The TX queue was arrival-ordered, and extended advertising is set-and-repeat: a
frame that leaves first holds the radio for a whole burst. So a position update
queued ahead of an ack went out first and the ack waited behind it, which is
exactly backwards for the one a sender is timing out waiting on.

It is now a bag rather than a ring. runOnce picks the highest-priority slot and
closes the gap; equal priorities still leave oldest-first, because priority is
meant to order the queue, not to reorder within itself. A full queue makes room
only for something strictly more important, the same trade
MeshPacketQueue::replaceLowerPriorityPacket makes for LoRa, and the slot it
displaces is the newest of the least important, so a frame that has already
waited its turn is not the one thrown away. Refusals are counted in
txDroppedQueueFull, kept separate from txDroppedTooLarge: one says the bearer
cannot carry this packet at all, the other that it could not carry it right now.

The slot carries the priority because the air copy no longer does - strippedForAir
drops it, being local scheduling the receiver overwrites.

src/mesh/BLEMeshHandler.cpp

@@ -131,19 +131,60 @@ bool BLEMeshHandler::onSend(const meshtastic_MeshPacket *mp)
     slot.from = mp->from;
     slot.id = mp->id;
 
+    slot.priority = (uint8_t)mp->priority;
+
     if (txCount >= BLE_MESH_TX_QUEUE_SIZE) {
-        LOG_WARN("BLE mesh: TX queue full, dropping 0x%08x", mp->id);
-        return false;
+        // Make room only by displacing something strictly less important, the same trade
+        // MeshPacketQueue::replaceLowerPriorityPacket makes for the LoRa queue. A queue full of work
+        // at least as important refuses the newcomer rather than shuffling equals.
+        const size_t worst = lowestPrioritySlot();
+        if (txQueue[worst].priority >= slot.priority) {
+            txDroppedQueueFull++;
+            LOG_WARN("BLE mesh: TX queue full of priority >= %u, dropping 0x%08x", (unsigned)slot.priority, mp->id);
+            return false;
+        }
+        LOG_WARN("BLE mesh: dropping queued 0x%08x (priority %u) for 0x%08x (priority %u)", txQueue[worst].id,
+                 (unsigned)txQueue[worst].priority, mp->id, (unsigned)slot.priority);
+        txDroppedQueueFull++;
+        removeSlot(worst);
     }
-    txQueue[txTail] = slot;
-    txTail = (txTail + 1) % BLE_MESH_TX_QUEUE_SIZE;
-    txCount++;
+    txQueue[txCount++] = slot;
 
     setIntervalFromNow(0);
     concurrency::mainDelay.interrupt();
     return true;
 }
 
+/// The slot that should go out next: highest priority, oldest first within a priority.
+size_t BLEMeshHandler::highestPrioritySlot() const
+{
+    size_t best = 0;
+    for (size_t i = 1; i < txCount; i++) {
+        if (txQueue[i].priority > txQueue[best].priority)
+            best = i;
+    }
+    return best;
+}
+
+/// The slot to displace when a more important frame arrives: lowest priority, newest first, so a
+/// frame that has already waited is not the one thrown away.
+size_t BLEMeshHandler::lowestPrioritySlot() const
+{
+    size_t worst = 0;
+    for (size_t i = 1; i < txCount; i++) {
+        if (txQueue[i].priority <= txQueue[worst].priority)
+            worst = i;
+    }
+    return worst;
+}
+
+void BLEMeshHandler::removeSlot(size_t index)
+{
+    for (size_t i = index + 1; i < txCount; i++)
+        txQueue[i - 1] = txQueue[i];
+    txCount--;
+}
+
 int32_t BLEMeshHandler::runOnce()
 {
     if (!isRunning || !platformReady())
@@ -163,9 +204,9 @@ int32_t BLEMeshHandler::runOnce()
 
     if (txCount == 0)
         return 100;
-    AdvSlot slot = txQueue[txHead];
-    txHead = (txHead + 1) % BLE_MESH_TX_QUEUE_SIZE;
-    txCount--;
+    const size_t next = highestPrioritySlot();
+    AdvSlot slot = txQueue[next];
+    removeSlot(next);
 
     if (platformBeginAdvertising(slot.data.data(), slot.len)) {
         advertising = true;
@@ -184,21 +225,18 @@ bool BLEMeshHandler::onCancelSending(meshtastic_MeshPacket_TransportMechanism me
 
     bool canceled = false;
 
-    // Compact the ring in place, keeping order. A cancel is rare and the ring is eight deep, so a
-    // copy costs less than threading a tombstone through runOnce().
+    // Compact in place, keeping arrival order so equal priorities still leave oldest-first.
     size_t kept = 0;
     for (size_t i = 0; i < txCount; i++) {
-        const AdvSlot &slot = txQueue[(txHead + i) % BLE_MESH_TX_QUEUE_SIZE];
-        if (slot.from == from && slot.id == id) {
+        if (txQueue[i].from == from && txQueue[i].id == id) {
             canceled = true;
             continue;
         }
         if (kept != i)
-            txQueue[(txHead + kept) % BLE_MESH_TX_QUEUE_SIZE] = slot;
+            txQueue[kept] = txQueue[i];
         kept++;
     }
     txCount = kept;
-    txTail = (txHead + kept) % BLE_MESH_TX_QUEUE_SIZE;
 
     // Cut a burst already on air short too. Extended advertising repeats one payload for
     // BLE_MESH_ADV_EVENTS events, so the copies still to come are exactly what the overhear says are

src/mesh/BLEMeshHandler.h

@@ -83,6 +83,11 @@ class BLEMeshHandler : private concurrency::OSThread, public MeshTransportBase
     /// ciphertext is that minus the envelope - well under LoRa's MAX_RADIO_PAYLOAD_LEN.
     uint32_t txDroppedTooLarge = 0;
 
+    /// Frames refused because the queue was full of work at least as important. Distinct from
+    /// txDroppedTooLarge: that one says the bearer cannot carry this packet at all, this one says it
+    /// could not carry it right now.
+    uint32_t txDroppedQueueFull = 0;
+
     /// Called from Router::send(). Encodes and queues; never transmits inline.
     bool onSend(const meshtastic_MeshPacket *mp) override;
 
@@ -92,12 +97,15 @@ class BLEMeshHandler : private concurrency::OSThread, public MeshTransportBase
 
   protected:
     /// One queued outbound frame, already built into a complete AD payload. `from`/`id` are kept
-    /// alongside the bytes so a cancel does not have to decode its own queue back out again.
+    /// alongside the bytes so a cancel does not have to decode its own queue back out again, and
+    /// `priority` because the air copy no longer carries it - strippedForAir() drops it, since it is
+    /// a local scheduling property that the receiver overwrites anyway.
     struct AdvSlot {
         std::array<uint8_t, BLE_MESH_ADV_TOTAL_MAX> data;
         uint8_t len;
         NodeNum from;
         PacketId id;
+        uint8_t priority;
     };
 
     // --- platform hooks ---------------------------------------------------------------------
@@ -114,6 +122,11 @@ class BLEMeshHandler : private concurrency::OSThread, public MeshTransportBase
 
     int32_t runOnce() override;
 
+    /// Queue order, kept out of runOnce() so the policy is one readable thing.
+    size_t highestPrioritySlot() const;
+    size_t lowestPrioritySlot() const;
+    void removeSlot(size_t index);
+
     /// Decode a received advertisement payload and enqueue it into the router.
     void deliverToRouter(const uint8_t *data, size_t len, int8_t rssi);
 
@@ -134,9 +147,11 @@ class BLEMeshHandler : private concurrency::OSThread, public MeshTransportBase
     //
     // <mutex>/<atomic> are also actively harmful here: they pull in <chrono>, which does not survive
     // Arduino's round()/abs() macros on the nRF52 arm-none-eabi toolchain.
+    // A bag, not a ring. Frames leave in priority order rather than arrival order, so there is no
+    // head to advance: runOnce() picks the best slot and closes the gap. Eight slots of 256 bytes
+    // makes the worst-case compaction under 2 KB on the main task, once per burst, which is cheaper
+    // than the index arithmetic an ordered ring would need.
     std::array<AdvSlot, BLE_MESH_TX_QUEUE_SIZE> txQueue{};
-    size_t txHead = 0;
-    size_t txTail = 0;
     size_t txCount = 0;
     bool advertising = false;
 

test/test_ble_mesh/BleMesh.cpp

@@ -75,6 +75,14 @@ meshtastic_MeshPacket encryptedPacket(uint32_t from = 0x3061b02e, uint32_t id =
     return p;
 }
 
+/// A packet at a chosen priority, for the queue-order tests.
+meshtastic_MeshPacket packetAt(meshtastic_MeshPacket_Priority priority, uint32_t id)
+{
+    meshtastic_MeshPacket p = encryptedPacket(0x3061b02e, id);
+    p.priority = priority;
+    return p;
+}
+
 /// What Router::send actually hands a transport, as opposed to the minimal fixture above.
 ///
 /// fixPriority() runs before encryption and never leaves priority UNSET (Router.cpp:562), and
@@ -266,6 +274,98 @@ void test_an_oversized_packet_is_counted_not_just_logged(void)
     TEST_ASSERT_EQUAL_UINT32_MESSAGE(1, h.txDroppedTooLarge, "the loss is countable");
 }
 
+void test_an_urgent_frame_overtakes_one_already_queued(void)
+{
+    FakeBLEMesh h;
+    h.start();
+    auto background = packetAt(meshtastic_MeshPacket_Priority_BACKGROUND, 0x11111111);
+    auto ack = packetAt(meshtastic_MeshPacket_Priority_ACK, 0x22222222);
+    TEST_ASSERT_TRUE(h.onSend(&background));
+    TEST_ASSERT_TRUE(h.onSend(&ack));
+
+    // Extended advertising is set-and-repeat, so whatever leaves first holds the radio for a whole
+    // burst. Arrival order would put a position update ahead of an ack that a sender is timing out
+    // waiting for.
+    uint8_t expected[BLE_MESH_ADV_TOTAL_MAX];
+    const uint8_t len = h.build(&ack, expected, sizeof(expected));
+
+    h.pump();
+    TEST_ASSERT_EQUAL_MESSAGE(1, h.sent.size(), "one frame on air");
+    TEST_ASSERT_EQUAL_UINT8_ARRAY(expected, h.sent[0].data(), len);
+}
+
+void test_equal_priorities_leave_in_arrival_order(void)
+{
+    FakeBLEMesh h;
+    h.start();
+    auto first = packetAt(meshtastic_MeshPacket_Priority_DEFAULT, 0x11111111);
+    auto second = packetAt(meshtastic_MeshPacket_Priority_DEFAULT, 0x22222222);
+    TEST_ASSERT_TRUE(h.onSend(&first));
+    TEST_ASSERT_TRUE(h.onSend(&second));
+
+    uint8_t expected[BLE_MESH_ADV_TOTAL_MAX];
+    const uint8_t len = h.build(&first, expected, sizeof(expected));
+
+    // Priority orders the queue; it does not reorder within a priority. Without this a burst of
+    // same-priority traffic would leave in an order nothing defines.
+    h.pump();
+    TEST_ASSERT_EQUAL_UINT8_ARRAY(expected, h.sent[0].data(), len);
+}
+
+void test_a_full_queue_makes_room_only_for_something_more_important(void)
+{
+    FakeBLEMesh h;
+    h.start();
+    for (size_t i = 0; i < BLE_MESH_TX_QUEUE_SIZE; i++) {
+        auto p = packetAt(meshtastic_MeshPacket_Priority_BACKGROUND, (uint32_t)(0x1000 + i));
+        TEST_ASSERT_TRUE(h.onSend(&p));
+    }
+
+    // Full of the least important work there is, so an ack displaces one.
+    auto ack = packetAt(meshtastic_MeshPacket_Priority_ACK, 0x22222222);
+    TEST_ASSERT_TRUE_MESSAGE(h.onSend(&ack), "an ack gets in");
+    TEST_ASSERT_EQUAL_UINT32_MESSAGE(1, h.txDroppedQueueFull, "and the displacement is counted");
+
+    // Now full again, and a second frame of the same priority as the rest has nothing to displace:
+    // shuffling equals would only change which packet is lost.
+    auto peer = packetAt(meshtastic_MeshPacket_Priority_BACKGROUND, 0x33333333);
+    TEST_ASSERT_FALSE_MESSAGE(h.onSend(&peer), "an equal is refused");
+    TEST_ASSERT_EQUAL_UINT32(2, h.txDroppedQueueFull);
+
+    uint8_t expected[BLE_MESH_ADV_TOTAL_MAX];
+    const uint8_t len = h.build(&ack, expected, sizeof(expected));
+    h.pump();
+    TEST_ASSERT_EQUAL_UINT8_ARRAY(expected, h.sent[0].data(), len);
+}
+
+void test_the_frame_displaced_is_the_newest_of_the_least_important(void)
+{
+    FakeBLEMesh h;
+    h.start();
+    auto oldest = packetAt(meshtastic_MeshPacket_Priority_BACKGROUND, 0x11111111);
+    TEST_ASSERT_TRUE(h.onSend(&oldest));
+    for (size_t i = 1; i < BLE_MESH_TX_QUEUE_SIZE; i++) {
+        auto p = packetAt(meshtastic_MeshPacket_Priority_BACKGROUND, (uint32_t)(0x2000 + i));
+        TEST_ASSERT_TRUE(h.onSend(&p));
+    }
+    auto ack = packetAt(meshtastic_MeshPacket_Priority_ACK, 0x33333333);
+    TEST_ASSERT_TRUE(h.onSend(&ack));
+
+    // A frame that has already waited its turn is not the one to throw away, so the displaced slot
+    // is the newest of the least important rather than the first one found.
+    uint8_t expectedAck[BLE_MESH_ADV_TOTAL_MAX];
+    uint8_t expectedOldest[BLE_MESH_ADV_TOTAL_MAX];
+    const uint8_t ackLen = h.build(&ack, expectedAck, sizeof(expectedAck));
+    const uint8_t oldestLen = h.build(&oldest, expectedOldest, sizeof(expectedOldest));
+
+    h.pump();
+    h.advertising = false;
+    h.pump();
+    TEST_ASSERT_EQUAL_MESSAGE(2, h.sent.size(), "two frames went out");
+    TEST_ASSERT_EQUAL_UINT8_ARRAY(expectedAck, h.sent[0].data(), ackLen);
+    TEST_ASSERT_EQUAL_UINT8_ARRAY(expectedOldest, h.sent[1].data(), oldestLen);
+}
+
 void test_a_dupe_heard_on_ble_cancels_our_queued_copy(void)
 {
     FakeBLEMesh h;
@@ -522,6 +622,10 @@ void setup()
     RUN_TEST(test_canceling_cuts_a_burst_already_on_air);
     RUN_TEST(test_send_queues_rather_than_transmitting);
     RUN_TEST(test_tx_queue_is_bounded);
+    RUN_TEST(test_an_urgent_frame_overtakes_one_already_queued);
+    RUN_TEST(test_equal_priorities_leave_in_arrival_order);
+    RUN_TEST(test_a_full_queue_makes_room_only_for_something_more_important);
+    RUN_TEST(test_the_frame_displaced_is_the_newest_of_the_least_important);
     RUN_TEST(test_a_relayed_packet_is_re_advertised);
     RUN_TEST(test_ingress_accepts_a_well_formed_frame);
     RUN_TEST(test_ingress_drops_a_frame_with_no_sender);