diff --git a/src/mesh/BLEMeshHandler.cpp b/src/mesh/BLEMeshHandler.cpp index 6e130aab47..0da3b6c9ad 100644 --- a/src/mesh/BLEMeshHandler.cpp +++ b/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 diff --git a/src/mesh/BLEMeshHandler.h b/src/mesh/BLEMeshHandler.h index 2b7c68f49c..67779d2d6c 100644 --- a/src/mesh/BLEMeshHandler.h +++ b/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 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 // // / are also actively harmful here: they pull in , 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 txQueue{}; - size_t txHead = 0; - size_t txTail = 0; size_t txCount = 0; bool advertising = false; diff --git a/test/test_ble_mesh/BleMesh.cpp b/test/test_ble_mesh/BleMesh.cpp index 255fbd4e63..388c7bfa4f 100644 --- a/test/test_ble_mesh/BleMesh.cpp +++ b/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);