Change RF95 to deliver packets straight from ISR and no polling for anything

src/CustomRF95.cpp

@@ -0,0 +1,168 @@
+#include "CustomRF95.h"
+#include <pb_encode.h>
+#include <pb_decode.h>
+#include "configuration.h"
+#include "assert.h"
+#include "NodeDB.h"
+
+#define MAX_TX_QUEUE 8 // max number of packets which can be waiting for transmission
+
+/// A temporary buffer used for sending/receving packets, sized to hold the biggest buffer we might need
+#define MAX_RHPACKETLEN 251
+static uint8_t radiobuf[MAX_RHPACKETLEN];
+
+CustomRF95::CustomRF95(MemoryPool<MeshPacket> &_pool, PointerQueue<MeshPacket> &_rxDest)
+    : RH_RF95(NSS_GPIO, DIO0_GPIO),
+      pool(_pool),
+      rxDest(_rxDest),
+      txQueue(MAX_TX_QUEUE),
+      sendingPacket(NULL)
+{
+}
+
+bool CustomRF95::init()
+{
+    bool ok = RH_RF95::init();
+
+    return ok;
+}
+
+/// Send a packet (possibly by enquing in a private fifo).  This routine will
+/// later free() the packet to pool.  This routine is not allowed to stall because it is called from
+/// bluetooth comms code.  If the txmit queue is empty it might return an error
+ErrorCode CustomRF95::send(MeshPacket *p)
+{
+    // FIXME - we currently just slam over into send mode if the RF95 is in RX mode.  This is _probably_ safe given
+    // how quiet our network is, bu it would be better to wait _if_ we are partially though receiving a packet (rather than
+    // just merely waiting for one).
+    // This is doubly bad because not only do we drop the packet that was on the way in, we almost certainly guarantee no one
+    // outside will like the packet we are sending.
+    if (_mode == RHModeIdle || _mode == RHModeRx)
+    {
+        // if the radio is idle, we can send right away
+        DEBUG_MSG("immedate send on mesh (txGood=%d,rxGood=%d,rxBad=%d)\n", txGood(), rxGood(), rxBad());
+        startSend(p);
+        return ERRNO_OK;
+    }
+    else
+    {
+        DEBUG_MSG("enquing packet for send from=0x%x, to=0x%x\n", p->from, p->to);
+        return txQueue.enqueue(p, 0) ? ERRNO_OK : ERRNO_UNKNOWN; // nowait
+    }
+}
+
+// After doing standard behavior, check to see if a new packet arrived or one was sent and start a new send or receive as necessary
+void CustomRF95::handleInterrupt()
+{
+    RH_RF95::handleInterrupt();
+
+    BaseType_t higherPriWoken = false;
+    if (_mode == RHModeIdle) // We are now done sending or receiving
+    {
+        if (sendingPacket) // Were we sending?
+        {
+            // We are done sending that packet, release it
+            pool.releaseFromISR(sendingPacket, &higherPriWoken);
+            sendingPacket = NULL;
+            // DEBUG_MSG("Done with send\n");
+        }
+
+        // If we just finished receiving a packet, forward it into a queue
+        if (_rxBufValid)
+        {
+            // We received a packet
+
+            // Skip the 4 headers that are at the beginning of the rxBuf
+            size_t payloadLen = _bufLen - RH_RF95_HEADER_LEN;
+            uint8_t *payload = _buf + RH_RF95_HEADER_LEN;
+
+            // FIXME - throws exception if called in ISR context: frequencyError() - probably the floating point math
+            int32_t freqerr = -1, snr = lastSNR();
+            //DEBUG_MSG("Received packet from mesh src=0x%x,dest=0x%x,id=%d,len=%d rxGood=%d,rxBad=%d,freqErr=%d,snr=%d\n",
+            //          srcaddr, destaddr, id, rxlen, rf95.rxGood(), rf95.rxBad(), freqerr, snr);
+
+            MeshPacket *mp = pool.allocZeroed();
+
+            SubPacket *p = &mp->payload;
+
+            mp->from = _rxHeaderFrom;
+            mp->to = _rxHeaderTo;
+            //_rxHeaderId = _buf[2];
+            //_rxHeaderFlags = _buf[3];
+
+            // If we already have an entry in the DB for this nodenum, goahead and hide the snr/freqerr info there.
+            // Note: we can't create it at this point, because it might be a bogus User node allocation.  But odds are we will
+            // already have a record we can hide this debugging info in.
+            NodeInfo *info = nodeDB.getNode(mp->from);
+            if (info)
+            {
+                info->snr = snr;
+                info->frequency_error = freqerr;
+            }
+
+            if (!pb_decode_from_bytes(payload, payloadLen, SubPacket_fields, p))
+            {
+                pool.releaseFromISR(mp, &higherPriWoken);
+            }
+            else
+            {
+                // parsing was successful, queue for our recipient
+                mp->has_payload = true;
+
+                int res = rxDest.enqueueFromISR(mp, &higherPriWoken); // NOWAIT - fixme, if queue is full, delete older messages
+                assert(res == pdTRUE);
+            }
+
+            clearRxBuf(); // This message accepted and cleared
+        }
+
+        higherPriWoken |= handleIdleISR();
+    }
+
+    // If we call this _IT WILL NOT RETURN_
+    if (higherPriWoken)
+        portYIELD_FROM_ISR();
+}
+
+/// Return true if a higher pri task has woken
+bool CustomRF95::handleIdleISR()
+{
+    BaseType_t higherPriWoken = false;
+
+    // First send any outgoing packets we have ready
+    MeshPacket *txp = txQueue.dequeuePtrFromISR(0);
+    if (txp)
+        startSend(txp);
+    else
+    {
+        // Nothing to send, let's switch back to receive mode
+        setModeRx();
+    }
+
+    return higherPriWoken;
+}
+
+/// This routine might be called either from user space or ISR
+void CustomRF95::startSend(MeshPacket *txp)
+{
+    assert(!sendingPacket);
+
+    // DEBUG_MSG("sending queued packet on mesh (txGood=%d,rxGood=%d,rxBad=%d)\n", rf95.txGood(), rf95.rxGood(), rf95.rxBad());
+    assert(txp->has_payload);
+
+    size_t numbytes = pb_encode_to_bytes(radiobuf, sizeof(radiobuf), SubPacket_fields, &txp->payload);
+
+    sendingPacket = txp;
+
+    setHeaderTo(txp->to);
+    setHeaderFrom(nodeDB.getNodeNum()); // We must do this before each send, because we might have just changed our nodenum
+
+    // setHeaderId(0);
+
+    assert(numbytes <= 251); // Make sure we don't overflow the tiny max packet size
+
+    // uint32_t start = millis(); // FIXME, store this in the class
+
+    int res = RH_RF95::send(radiobuf, numbytes);
+    assert(res);
+}

src/CustomRF95.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <RH_RF95.h>
+#include <RHMesh.h>
+#include "MemoryPool.h"
+#include "mesh.pb.h"
+#include "PointerQueue.h"
+#include "MeshTypes.h"
+
+/**
+ * A version of the RF95 driver which is smart enough to manage packets via queues (no polling or blocking in user threads!)
+ */
+class CustomRF95 : public RH_RF95
+{
+    MemoryPool<MeshPacket> &pool;
+    PointerQueue<MeshPacket> &rxDest;
+    PointerQueue<MeshPacket> txQueue;
+
+    MeshPacket *sendingPacket; // The packet we are currently sending
+public:
+    /** pool is the pool we will alloc our rx packets from
+     * rxDest is where we will send any rx packets, it becomes receivers responsibility to return packet to the pool
+     */
+    CustomRF95(MemoryPool<MeshPacket> &pool, PointerQueue<MeshPacket> &rxDest);
+
+    /// Send a packet (possibly by enquing in a private fifo).  This routine will
+    /// later free() the packet to pool.  This routine is not allowed to stall because it is called from
+    /// bluetooth comms code.  If the txmit queue is empty it might return an error
+    ErrorCode send(MeshPacket *p);
+
+    bool init();
+
+protected:
+    // After doing standard behavior, check to see if a new packet arrived or one was sent and start a new send or receive as necessary
+    virtual void handleInterrupt();
+
+private:
+    /// Send a new packet - this low level call can be called from either ISR or userspace
+    void startSend(MeshPacket *txp);
+
+    /// Return true if a higher pri task has woken
+    bool handleIdleISR();
+};

src/MemoryPool.h

@@ -1,4 +1,4 @@
-#pragma once 
+#pragma once
 
 #include <Arduino.h>
 #include <assert.h>
@@ -10,58 +10,72 @@
  * 
  * Eventually this routine will even be safe for ISR use...
  */
-template <class T> class MemoryPool {
+template <class T>
+class MemoryPool
+{
     PointerQueue<T> dead;
 
     T *buf; // our large raw block of memory
 
     size_t maxElements;
+
 public:
-    MemoryPool(size_t _maxElements): dead(_maxElements), maxElements(_maxElements) {
+    MemoryPool(size_t _maxElements) : dead(_maxElements), maxElements(_maxElements)
+    {
         buf = new T[maxElements];
-        
+
         // prefill dead
-        for(int i = 0; i < maxElements; i++)
+        for (int i = 0; i < maxElements; i++)
             release(&buf[i]);
     }
 
-    ~MemoryPool() {
+    ~MemoryPool()
+    {
         delete[] buf;
     }
 
     /// Return a queable object which has been prefilled with zeros.  Panic if no buffer is available
-    T *allocZeroed() {
+    T *allocZeroed()
+    {
         T *p = allocZeroed(0);
-        
+
         assert(p); // FIXME panic instead
         return p;
     }
 
-
     /// Return a queable object which has been prefilled with zeros - allow timeout to wait for available buffers (you probably don't want this version)
-    T *allocZeroed(TickType_t maxWait) {
+    T *allocZeroed(TickType_t maxWait)
+    {
         T *p = dead.dequeuePtr(maxWait);
-        
-        if(p)
+
+        if (p)
             memset(p, 0, sizeof(T));
         return p;
     }
 
     /// Return a queable object which is a copy of some other object
-    T *allocCopy(const T &src, TickType_t maxWait = portMAX_DELAY) {
+    T *allocCopy(const T &src, TickType_t maxWait = portMAX_DELAY)
+    {
         T *p = dead.dequeuePtr(maxWait);
-        
-        if(p)
+
+        if (p)
             *p = src;
         return p;
     }
 
-
     /// Return a buffer for use by others
-    void release(T *p) {
+    void release(T *p)
+    {
         int res = dead.enqueue(p, 0);
         assert(res == pdTRUE);
         assert(p >= buf && (p - buf) < maxElements); // sanity check to make sure a programmer didn't free something that didn't come from this pool
     }
-};
 
+    /// Return a buffer from an ISR, if higherPriWoken is set to true you have some work to do ;-)
+    void releaseFromISR(T *p, BaseType_t *higherPriWoken)
+    {
+        int res = dead.enqueueFromISR(p, higherPriWoken);
+        assert(res == pdTRUE);
+        assert(p >= buf && (p - buf) < maxElements); // sanity check to make sure a programmer didn't free something that didn't come from this pool
+    }
+};

src/MeshRadio.cpp

@@ -11,9 +11,8 @@
 
 #define DEFAULT_CHANNEL_NUM 3 // we randomly pick one
 
-
 /// 16 bytes of random PSK for our _public_ default channel that all devices power up on
-static const uint8_t defaultpsk[] = { 0xd4, 0xf1, 0xbb, 0x3a, 0x20, 0x29, 0x07, 0x59, 0xf0, 0xbc, 0xff, 0xab, 0xcf, 0x4e, 0x69, 0xbf };
+static const uint8_t defaultpsk[] = {0xd4, 0xf1, 0xbb, 0x3a, 0x20, 0x29, 0x07, 0x59, 0xf0, 0xbc, 0xff, 0xab, 0xcf, 0x4e, 0x69, 0xbf};
 
 /**
  * ## LoRaWAN for North America
@@ -27,11 +26,8 @@ Channel zero starts at 903.08 MHz center frequency.
 */
 
 MeshRadio::MeshRadio(MemoryPool<MeshPacket> &_pool, PointerQueue<MeshPacket> &_rxDest)
-    : rf95(NSS_GPIO, DIO0_GPIO),
-      manager(rf95),
-      pool(_pool),
-      rxDest(_rxDest),
-      txQueue(MAX_TX_QUEUE)
+    : rf95(_pool, _rxDest),
+      manager(rf95)
 {
   myNodeInfo.num_channels = NUM_CHANNELS;
 
@@ -81,15 +77,15 @@ bool MeshRadio::init()
 
 void MeshRadio::reloadConfig()
 {
-  rf95.setModeIdle();
+  rf95.setModeIdle(); // Need to be idle before doing init
 
   // Set up default configuration
   // No Sync Words in LORA mode.
   rf95.setModemConfig((RH_RF95::ModemConfigChoice)channelSettings.modem_config); // Radio default
-                                                                             //    setModemConfig(Bw125Cr48Sf4096); // slow and reliable?
+                                                                                 //    setModemConfig(Bw125Cr48Sf4096); // slow and reliable?
   // rf95.setPreambleLength(8);           // Default is 8
 
-  assert(channelSettings.channel_num < NUM_CHANNELS); // If the phone tries to tell us to use an illegal channel then panic 
+  assert(channelSettings.channel_num < NUM_CHANNELS); // If the phone tries to tell us to use an illegal channel then panic
 
   // Defaults after init are 434.0MHz, modulation GFSK_Rb250Fd250, +13dbM
   float center_freq = CH0 + CH_SPACING * channelSettings.channel_num;
@@ -108,11 +104,14 @@ void MeshRadio::reloadConfig()
   rf95.setTxPower(channelSettings.tx_power, false);
 
   DEBUG_MSG("Set radio: name=%s. config=%u, ch=%d, txpower=%d\n", channelSettings.name, channelSettings.modem_config, channelSettings.channel_num, channelSettings.tx_power);
+
+  // Done with init tell radio to start receiving
+  rf95.setModeRx();
 }
 
-
-void MeshRadio::sleep() {
-  // we no longer care about interrupts from this device 
+void MeshRadio::sleep()
+{
+  // we no longer care about interrupts from this device
   rf95.prepareDeepSleep();
 
   // FIXME - leave the device state in rx mode instead
@@ -121,10 +120,15 @@ void MeshRadio::sleep() {
 
 ErrorCode MeshRadio::send(MeshPacket *p)
 {
+#if 1
+  return rf95.send(p);
+#else
   DEBUG_MSG("enquing packet for send from=0x%x, to=0x%x\n", p->from, p->to);
   return txQueue.enqueue(p, 0); // nowait
+#endif
 }
 
+#if 0
 ErrorCode MeshRadio::sendTo(NodeNum dest, const uint8_t *buf, size_t len)
 {
   // We must do this before each send, because we might have just changed our nodenum
@@ -154,6 +158,7 @@ void MeshRadio::handleReceive(MeshPacket *mp)
   int res = rxDest.enqueue(mp, 0); // NOWAIT - fixme, if queue is full, delete older messages
   assert(res == pdTRUE);
 }
+#endif
 
 void MeshRadio::loop()
 {
@@ -168,8 +173,9 @@ static int16_t packetnum = 0;  // packet counter, we increment per xmission
   assert(sendTo(NODENUM_BROADCAST, (uint8_t *)radiopacket, sizeof(radiopacket)) == ERRNO_OK);
 #endif
 
+#if 0
   /// A temporary buffer used for sending/receving packets, sized to hold the biggest buffer we might need
-  #define MAX_RHPACKETLEN 251
+#define MAX_RHPACKETLEN 251
   static uint8_t radiobuf[MAX_RHPACKETLEN];
   uint8_t rxlen;
   uint8_t srcaddr, destaddr, id, flags;
@@ -213,7 +219,9 @@ static int16_t packetnum = 0;  // packet counter, we increment per xmission
       handleReceive(mp);
     }
   }
+#endif
 
+#if 0
   // Poll to see if we need to send any packets
   MeshPacket *txp = txQueue.dequeuePtr(0); // nowait
   if (txp)
@@ -234,4 +242,5 @@ static int16_t packetnum = 0;  // packet counter, we increment per xmission
 
     DEBUG_MSG("Done with send\n");
   }
+#endif
 }

src/MeshRadio.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include <RH_RF95.h>
+#include "CustomRF95.h"
 #include <RHMesh.h>
 #include "MemoryPool.h"
 #include "mesh.pb.h"
@@ -80,17 +80,13 @@ public:
     void reloadConfig();
 
 private:
-    RH_RF95 rf95; // the raw radio interface
+    CustomRF95 rf95; // the raw radio interface
 
     // RHDatagram manager;
     // RHReliableDatagram manager; // don't use mesh yet
     RHMesh manager;
     // MeshRXHandler rxHandler;
 
-    MemoryPool<MeshPacket> &pool;
-    PointerQueue<MeshPacket> &rxDest;
-    PointerQueue<MeshPacket> txQueue;
-
     /// low level send, might block for mutiple seconds
     ErrorCode sendTo(NodeNum dest, const uint8_t *buf, size_t len);
 

src/MeshService.cpp

@@ -218,7 +218,7 @@ void MeshService::handleToRadio(std::string s)
 void MeshService::sendToMesh(MeshPacket *p)
 {
     nodeDB.updateFrom(*p); // update our local DB for this packet (because phone might have sent position packets etc...)
-    assert(radio.send(p) == pdTRUE);
+    assert(radio.send(p) == ERRNO_OK);
 }
 
 MeshPacket *MeshService::allocForSending()

src/NodeDB.cpp

@@ -292,6 +292,7 @@ void NodeDB::updateFrom(const MeshPacket &mp)
 }
 
 /// Find a node in our DB, return null for missing
+/// NOTE: This function might be called from an ISR
 NodeInfo *NodeDB::getNode(NodeNum n)
 {
     for (int i = 0; i < *numNodes; i++)

src/PointerQueue.h

@@ -1,21 +1,31 @@
-#pragma once 
+#pragma once
 
 #include "TypedQueue.h"
 
-
 /**
  * A wrapper for freertos queues that assumes each element is a pointer
  */
-template <class T> class PointerQueue: public TypedQueue<T *> {
+template <class T>
+class PointerQueue : public TypedQueue<T *>
+{
 public:
-    PointerQueue(int maxElements) : TypedQueue<T *>(maxElements) {
+    PointerQueue(int maxElements) : TypedQueue<T *>(maxElements)
+    {
     }
 
-    // preturns a ptr or null if the queue was empty
-    T *dequeuePtr(TickType_t maxWait = portMAX_DELAY) {
+    // returns a ptr or null if the queue was empty
+    T *dequeuePtr(TickType_t maxWait = portMAX_DELAY)
+    {
         T *p;
-        
-        return this->dequeue(&p, maxWait)  == pdTRUE ? p : NULL;
+
+        return this->dequeue(&p, maxWait) == pdTRUE ? p : NULL;
+    }
+
+    // returns a ptr or null if the queue was empty
+    T *dequeuePtrFromISR(BaseType_t *higherPriWoken)
+    {
+        T *p;
+
+        return this->dequeueFromISR(&p, higherPriWoken) == pdTRUE ? p : NULL;
     }
 };
-

src/TypedQueue.h

@@ -1,4 +1,4 @@
-#pragma once 
+#pragma once
 
 #include <Arduino.h>
 #include <assert.h>
@@ -7,29 +7,47 @@
  * A wrapper for freertos queues.  Note: each element object must be quite small, so T should be only
  * pointer types or ints
  */
-template <class T> class TypedQueue {
+template <class T>
+class TypedQueue
+{
     QueueHandle_t h;
+
 public:
-    TypedQueue(int maxElements) {
+    TypedQueue(int maxElements)
+    {
         h = xQueueCreate(maxElements, sizeof(T));
         assert(h);
     }
 
-    ~TypedQueue() {
+    ~TypedQueue()
+    {
         vQueueDelete(h);
     }
 
-    int numFree() {
+    int numFree()
+    {
         return uxQueueSpacesAvailable(h);
     }
 
     // pdTRUE for success else failure
-    BaseType_t enqueue(T x, TickType_t maxWait = portMAX_DELAY) {
+    BaseType_t enqueue(T x, TickType_t maxWait = portMAX_DELAY)
+    {
         return xQueueSendToBack(h, &x, maxWait);
     }
 
+    BaseType_t enqueueFromISR(T x, BaseType_t *higherPriWoken)
+    {
+        return xQueueSendToBackFromISR(h, &x, higherPriWoken);
+    }
+
     // pdTRUE for success else failure
-    BaseType_t dequeue(T *p, TickType_t maxWait = portMAX_DELAY) {
+    BaseType_t dequeue(T *p, TickType_t maxWait = portMAX_DELAY)
+    {
         return xQueueReceive(h, p, maxWait);
     }
+
+    BaseType_t dequeueFromISR(T *p, BaseType_t *higherPriWoken)
+    {
+        return xQueueReceiveFromISR(h, p, higherPriWoken);
+    }
 };

src/configuration.h

@@ -43,8 +43,8 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 // Select which board is being used. If the outside build environment has sent a choice, just use that
 #if !defined(T_BEAM_V10) && !defined(HELTEC_LORA32)
-#define T_BEAM_V10  // AKA Rev1 (second board released)
-//#define HELTEC_LORA32
+//#define T_BEAM_V10  // AKA Rev1 (second board released)
+#define HELTEC_LORA32
 
 #define HW_VERSION_US // We encode the hardware freq range in the hw version string, so sw update can eventually install the correct build
 #endif