Fix packet pool leak when rx queue is full

PacketQueue::add() silently dropped packets when the queue was at
capacity. The packet pointer was lost — never enqueued, never returned
to the unused pool. Each occurrence permanently shrank the 32-packet
pool until allocNew() returned NULL and the node went deaf. Return bool
from add() and free the packet back to the pool on failure.

src/helpers/StaticPoolPacketManager.cpp

@@ -55,15 +55,15 @@ mesh::Packet* PacketQueue::removeByIdx(int i) {
   return item;
 }
 
-void PacketQueue::add(mesh::Packet* packet, uint8_t priority, uint32_t scheduled_for) {
+bool PacketQueue::add(mesh::Packet* packet, uint8_t priority, uint32_t scheduled_for) {
   if (_num == _size) {
-    // TODO: log "FATAL: queue is full!"
+    return false;
-    return;
   }
   _table[_num] = packet;
   _pri_table[_num] = priority;
   _schedule_table[_num] = scheduled_for;
   _num++;
+  return true;
 }
 
 StaticPoolPacketManager::StaticPoolPacketManager(int pool_size): unused(pool_size), send_queue(pool_size), rx_queue(pool_size) {
@@ -82,7 +82,10 @@ void StaticPoolPacketManager::free(mesh::Packet* packet) {
 }
 
 void StaticPoolPacketManager::queueOutbound(mesh::Packet* packet, uint8_t priority, uint32_t scheduled_for) {
-  send_queue.add(packet, priority, scheduled_for);
+  if (!send_queue.add(packet, priority, scheduled_for)) {
+    MESH_DEBUG_PRINTLN("queueOutbound: send queue full, dropping packet");
+    free(packet);
+  }
 }
 
 mesh::Packet* StaticPoolPacketManager::getNextOutbound(uint32_t now) {
@@ -106,7 +109,10 @@ mesh::Packet* StaticPoolPacketManager::removeOutboundByIdx(int i) {
 }
 
 void StaticPoolPacketManager::queueInbound(mesh::Packet* packet, uint32_t scheduled_for) {
-  rx_queue.add(packet, 0, scheduled_for);
+  if (!rx_queue.add(packet, 0, scheduled_for)) {
+    MESH_DEBUG_PRINTLN("queueInbound: rx queue full, dropping packet");
+    free(packet);
+  }
 }
 mesh::Packet* StaticPoolPacketManager::getNextInbound(uint32_t now) {
   return rx_queue.get(now);

src/helpers/StaticPoolPacketManager.h

@@ -11,7 +11,7 @@ class PacketQueue {
 public:
   PacketQueue(int max_entries);
   mesh::Packet* get(uint32_t now);
-  void add(mesh::Packet* packet, uint8_t priority, uint32_t scheduled_for);
+  bool add(mesh::Packet* packet, uint8_t priority, uint32_t scheduled_for);
   int count() const { return _num; }
   int countBefore(uint32_t now) const;
   mesh::Packet* itemAt(int i) const { return _table[i]; }