Rework radio message processing

lib/comms/comms.cpp

@@ -265,12 +265,18 @@ bool ReadlineComms::send(Message &m)
         p = _scan_result_str(m.payload.dump);
         break;
     case MessageType::CONFIG_TASK:
-        p = m.payload.config.is_set ? "SET " : "GET ";
+        ConfigTaskType ctt = m.payload.config.task_type;
+        p = ctt == GET              ? "GET "
+            : ctt == SET            ? "SET "
+            : ctt == GETSET_SUCCESS ? "Success: "
+                                    : "Failed to set: ";
         p += *m.payload.config.key;
-        if (m.payload.config.is_set)
+        if (ctt == SET || ctt == GETSET_SUCCESS)
         {
             p += " " + *m.payload.config.value;
         }
+
+        p += "\n";
         break;
     }
 
@@ -368,7 +374,7 @@ Message *_parsePacket(String p)
     {
         Message *m = new Message();
         m->type = MessageType::CONFIG_TASK;
-        m->payload.config.is_set = false;
+        m->payload.config.task_type = GET;
         m->payload.config.key = new String(_stringParam(p, ""));
         m->payload.config.value = NULL;
         return m;
@@ -378,7 +384,7 @@ Message *_parsePacket(String p)
     {
         Message *m = new Message();
         m->type = MessageType::CONFIG_TASK;
-        m->payload.config.is_set = true;
+        m->payload.config.task_type = SET;
         m->payload.config.key = new String(_stringParam(p, ""));
         m->payload.config.value = new String(_stringParam(p, ""));
 
@@ -430,7 +436,8 @@ Message::~Message()
     {
         delete payload.config.key;
 
-        if (payload.config.is_set)
+        ConfigTaskType ctt = payload.config.task_type;
+        if (ctt == GETSET_SUCCESS || ctt == SET)
         {
             delete payload.config.value;
         }

lib/comms/comms.h

@@ -22,6 +22,15 @@ enum MessageType
     _MAX_MESSAGE_TYPE = CONFIG_TASK
 };
 
+enum ConfigTaskType
+{
+    GET = 0,
+    SET,
+    GETSET_SUCCESS,
+    SET_FAIL,
+    _MAX_CONFIG_TASK_TYPE = SET_FAIL
+};
+
 struct Wrapper
 {
     int32_t length;
@@ -45,7 +54,7 @@ struct ConfigTask
 {
     String *key;
     String *value;
-    bool is_set;
+    ConfigTaskType task_type;
 };
 
 struct Message

lib/comms/radio_comms.cpp

@@ -48,24 +48,25 @@ size_t _write(uint8_t *m, size_t sz, size_t p, uint8_t *v, size_t v_sz)
 #define RSSI_HI -80
 #define DETAIL_RSSIS 8
 #define RSSI_LO (RSSI_HI - 1 - DETAIL_RSSIS)
-int16_t RadioComms::send(Message &m)
+
+uint8_t *_serialize_scan_result(Message &m, size_t &p, uint8_t *msg)
 {
     if (m.type != SCAN_RESULT)
     {
-        return RADIOLIB_ERR_INVALID_FUNCTION;
+        return NULL;
     }
 
-    uint8_t msg[MAX_MSG];
     size_t dump_sz = m.payload.dump.sz;
-    size_t p = _write(msg, MAX_MSG, 0, (uint8_t)m.type);
+    size_t max_msg = p;
+    p = _write(msg, max_msg, 0, (uint8_t)m.type);
 
     // first cut: dump the RSSI as-is
     // optimize the message size later
-    p = _write(msg, MAX_MSG, p, (uint8_t *)&m.payload.dump.freqs_khz[0], 4);
-    p = _write(msg, MAX_MSG, p, (uint8_t *)&m.payload.dump.freqs_khz[dump_sz - 1], 4);
-    p = _write(msg, MAX_MSG, p, (uint8_t *)&dump_sz, 2);
+    p = _write(msg, max_msg, p, (uint8_t *)&m.payload.dump.freqs_khz[0], 4);
+    p = _write(msg, max_msg, p, (uint8_t *)&m.payload.dump.freqs_khz[dump_sz - 1], 4);
+    p = _write(msg, max_msg, p, (uint8_t *)&dump_sz, 2);
 
-    size_t rem = MAX_MSG - p;
+    size_t rem = max_msg - p;
     if (rem > dump_sz)
         rem = dump_sz;
 
@@ -75,7 +76,7 @@ int16_t RadioComms::send(Message &m)
     {
         if (i * rem / dump_sz > p - pp)
         {
-            p = _write(msg, MAX_MSG, p, bits);
+            p = _write(msg, max_msg, p, bits);
             bits = 0;
         }
         int16_t v = m.payload.dump.rssis[i];
@@ -97,10 +98,79 @@ int16_t RadioComms::send(Message &m)
 
     if (dump_sz > 0)
     {
-        p = _write(msg, MAX_MSG, p, bits);
+        p = _write(msg, max_msg, p, bits);
     }
 
-    return radio.transmit(msg, p);
+    return msg;
+}
+
+uint8_t *_serialize_config_task(Message &m, size_t &p, uint8_t *msg)
+{
+    if (m.type != CONFIG_TASK)
+    {
+        return NULL;
+    }
+    size_t max_msg = p;
+    ConfigTaskType ctt = m.payload.config.task_type;
+    p = _write(msg, max_msg, 0, (uint8_t)m.type);
+    p = _write(msg, max_msg, p, (uint8_t)ctt);
+
+    int key_len = m.payload.config.key->length();
+    if (max_msg - p < key_len + 1)
+    {
+        return NULL;
+    }
+
+    p = _write(msg, max_msg, p, (uint8_t)key_len);
+    p = _write(msg, max_msg, p, (uint8_t *)m.payload.config.key->c_str(), key_len);
+
+    if (ctt == GET || ctt == SET_FAIL)
+    {
+        return msg;
+    }
+
+    int v_len = m.payload.config.value->length();
+
+    if (max_msg - p < v_len + 1)
+    {
+        return NULL;
+    }
+
+    p = _write(msg, max_msg, p, (uint8_t)v_len);
+    p = _write(msg, max_msg, p, (uint8_t *)m.payload.config.value->c_str(), v_len);
+
+    return msg;
+}
+
+int16_t RadioComms::send(Message &m)
+{
+    uint8_t msg_buf[MAX_MSG];
+    size_t p = MAX_MSG;
+    uint8_t *msg = NULL;
+
+    if (m.type == SCAN_RESULT)
+    {
+        msg = _serialize_scan_result(m, p, msg_buf);
+    }
+    else if (m.type == MessageType::CONFIG_TASK)
+    {
+        msg = _serialize_config_task(m, p, msg_buf);
+    }
+
+    if (msg == NULL)
+    {
+        Serial.printf("Failed to encode message\n");
+        return RADIOLIB_ERR_INVALID_FUNCTION;
+    }
+
+    int16_t status = radio.transmit(msg, p);
+
+    if (msg != msg_buf)
+    {
+        delete[] msg;
+    }
+
+    return status;
 }
 
 size_t _read(uint8_t *buf, size_t sz, size_t p, uint8_t *v, size_t len)
@@ -118,6 +188,96 @@ size_t _read(uint8_t *buf, size_t sz, size_t p, uint8_t *v)
     return _read(buf, sz, p, v, 1);
 }
 
+Message *_deserialize_scan_result(size_t len, size_t &p, uint8_t *packet)
+{
+    Message *message = new Message();
+    message->type = SCAN_RESULT;
+
+    uint32_t s, e;
+    size_t dump_sz = 0;
+    p = _read(packet, len, p, (uint8_t *)&s, 4);
+    p = _read(packet, len, p, (uint8_t *)&e, 4);
+    p = _read(packet, len, p, (uint8_t *)&dump_sz, 2);
+    size_t rem = len - p;
+
+    message->payload.dump.sz = dump_sz;
+    if (dump_sz > 0)
+    {
+        message->payload.dump.rssis = new int16_t[dump_sz];
+        message->payload.dump.freqs_khz = new uint32_t[dump_sz];
+        message->payload.dump.freqs_khz[0] = s;
+        message->payload.dump.freqs_khz[dump_sz - 1] = e;
+
+        for (int i = 1; i < dump_sz - 1; i++)
+        {
+            uint32_t incr = (e - s) / (dump_sz - i);
+            s += incr;
+            message->payload.dump.freqs_khz[i] = s;
+        }
+
+        for (int i = 0, k = 0; i < rem; i++)
+        {
+            int j = (i + 1) * dump_sz / rem;
+            int16_t rssi = 0;
+            p = _read(packet, len, p, (uint8_t *)&rssi);
+            rssi -= 255;
+            for (; k < j; k++)
+            {
+                message->payload.dump.rssis[k] = rssi;
+            }
+        }
+    }
+
+    return message;
+}
+
+Message *_deserialize_config_task(size_t len, size_t &p, uint8_t *packet)
+{
+    Message *message = new Message();
+    message->type = CONFIG_TASK;
+
+    ConfigTaskType ctt = GET;
+    p = _read(packet, len, p, (uint8_t *)&ctt);
+    message->payload.config.task_type = ctt;
+
+    size_t key_len = 0;
+    size_t p1 = _read(packet, len, p, (uint8_t *)&key_len);
+    memmove(packet + p, packet + p + 1, key_len);
+    packet[p + key_len] = 0;
+    String *key = new String((char *)packet + p);
+    message->payload.config.key = key;
+
+    p = p1 + key_len;
+
+    if (key->length() != key_len)
+    {
+        delete message;
+        return NULL;
+    }
+
+    if (ctt == GET || ctt == SET_FAIL)
+    {
+        return message;
+    }
+
+    size_t v_len = 0;
+    p1 = _read(packet, len, p, (uint8_t *)&v_len);
+    memmove(packet + p, packet + p + 1, v_len);
+    packet[p + v_len] = 0;
+    String *value = new String((char *)packet + p);
+    message->payload.config.value = value;
+
+    p = p1 + v_len;
+
+    if (value->length() != v_len)
+    {
+        delete message;
+        return NULL;
+    }
+
+    return message;
+}
+
 volatile bool _received = false;
 void _rcv() { _received = true; }
 
@@ -184,43 +344,11 @@ Message *RadioComms::receive(uint16_t timeout_ms)
     Message *message = NULL;
     if (b == SCAN_RESULT)
     {
-        message = new Message();
-        message->type = SCAN_RESULT;
-
-        uint32_t s, e;
-        size_t dump_sz = 0;
-        p = _read(packet, len, p, (uint8_t *)&s, 4);
-        p = _read(packet, len, p, (uint8_t *)&e, 4);
-        p = _read(packet, len, p, (uint8_t *)&dump_sz, 2);
-        size_t rem = len - p;
-
-        message->payload.dump.sz = dump_sz;
-        if (dump_sz > 0)
-        {
-            message->payload.dump.rssis = new int16_t[dump_sz];
-            message->payload.dump.freqs_khz = new uint32_t[dump_sz];
-            message->payload.dump.freqs_khz[0] = s;
-            message->payload.dump.freqs_khz[dump_sz - 1] = e;
-
-            for (int i = 1; i < dump_sz - 1; i++)
-            {
-                uint32_t incr = (e - s) / (dump_sz - i);
-                s += incr;
-                message->payload.dump.freqs_khz[i] = s;
-            }
-
-            for (int i = 0, k = 0; i < rem; i++)
-            {
-                int j = (i + 1) * dump_sz / rem;
-                int16_t rssi = 0;
-                p = _read(packet, len, p, (uint8_t *)&rssi);
-                rssi -= 255;
-                for (; k < j; k++)
-                {
-                    message->payload.dump.rssis[k] = rssi;
-                }
-            }
-        }
+        message = _deserialize_scan_result(len, p, packet);
+    }
+    else if (b == CONFIG_TASK)
+    {
+        message = _deserialize_config_task(len, p, packet);
     }
     else
     {

lib/config/config.h

@@ -3,6 +3,16 @@
 
 #include <SD.h>
 
+template <typename L, typename R> struct Result
+{
+    bool is_ok;
+    union
+    {
+        L not_ok;
+        R ok;
+    };
+};
+
 struct ScanRange
 {
     uint64_t start_khz;

src/main.cpp

@@ -1425,7 +1425,13 @@ void checkComms()
             Comms1->send(*m); // forward to peer
             break;
         case MessageType::CONFIG_TASK:
-            if (m->payload.config.is_set)
+            ConfigTaskType ctt = m->payload.config.task_type;
+            if (ctt == GET)
+            {
+                Serial.printf("GET config: %s = %s\n", m->payload.config.key->c_str(),
+                              config.getConfig(*m->payload.config.key).c_str());
+            }
+            else if (ctt == SET)
             {
                 String v = config.getConfig(*m->payload.config.key);
                 bool r =
@@ -1494,7 +1500,8 @@ void doScan();
 
 void reportScan(RadioComms &c);
 
-int16_t checkRadio(RadioComms &c);
+Result<int16_t, Message *> checkRadio(RadioComms &c);
+int16_t sendMessage(RadioComms &c, Message &m);
 
 void loop(void)
 {
@@ -1504,27 +1511,43 @@ void loop(void)
     drone_detected_frequency_start = 0;
 
     checkComms();
+    // NB: swapping the use of Tx and Rx comms, so a pair of modules
+    //     with identical rx/tx_lora config can talk
+    RadioComms *rx = config.is_host ? TxComms : RxComms;
+    RadioComms *tx = config.is_host ? RxComms : TxComms;
 
-    if (config.is_host)
+    if (rx != NULL && (config.is_host || config.lora_enabled))
     {
-        if (TxComms != NULL)
+        Result<int16_t, Message *> res = checkRadio(*rx);
+
+        if (!res.is_ok)
         {
-            // NB: swapping the use of Tx and Rx comms, so a pair of modules
-            //     with identical rx/tx_lora config can talk
-            int16_t status = checkRadio(*TxComms);
+            int16_t status = res.not_ok;
             if (status != RADIOLIB_ERR_NONE)
             {
                 Serial.printf("Error getting a message: %d\n", status);
             }
         }
+        else if (res.ok != NULL)
+        {
+            if (config.is_host || tx == NULL)
+            {
+                HostComms->send(*res.ok);
+            }
+            else
+            {
+                sendMessage(*tx, *res.ok);
+            }
+
+            delete res.ok;
+        }
     }
-    else
+
+    if (!config.is_host)
     {
         doScan();
-        if (TxComms != NULL && config.lora_enabled)
-            reportScan(*TxComms);
-        if (RxComms != NULL && config.lora_enabled)
-            checkRadio(*RxComms);
+        if (tx != NULL && config.lora_enabled)
+            reportScan(*tx);
     }
 }
 
@@ -1961,56 +1984,87 @@ void doScan()
 #endif
 }
 
-int16_t checkRadio(RadioComms &comms)
+Result<int16_t, Message *> checkRadio(RadioComms &comms)
 {
     radioIsScan = false;
-    int16_t status = comms.configureRadio();
-    if (status != RADIOLIB_ERR_NONE)
-        return status;
+    Result<int16_t, Message *> ret;
+    ret.is_ok = false;
+
+    ret.not_ok = comms.configureRadio();
+    if (ret.not_ok != RADIOLIB_ERR_NONE)
+        return ret;
+
+    ret.is_ok = true;
 
     Message *msg = comms.receive(
         config.is_host
             ? 2000
             : 200); // 200ms should be enough to receive 500 bytes at SF 7 and BW 500
+    ret.ok = msg;
     if (msg == NULL)
     {
-        return status;
+        return ret;
     }
 
-    if (msg->type == SCAN_RESULT)
+    if (msg->type == CONFIG_TASK)
     {
-        HostComms->send(*msg);
-    }
-    else
-    {
-        Serial.printf("Received a message of unsupported type: %d\n", msg->type);
+        ConfigTaskType ctt = msg->payload.config.task_type;
+
+        if (ctt == ConfigTaskType::GET || ctt == ConfigTaskType::SET)
+        {
+            // must be GET or SET - both require sending back a response
+
+            String old_v = config.getConfig(*msg->payload.config.key);
+            bool success = true;
+            if (ctt == ConfigTaskType::SET)
+            {
+                success = config.updateConfig(*msg->payload.config.key,
+                                              *msg->payload.config.value);
+                delete msg->payload.config.value;
+            }
+
+            if (success)
+            {
+                msg->payload.config.task_type = GETSET_SUCCESS;
+                msg->payload.config.value = new String(old_v);
+            }
+            else
+            {
+                msg->payload.config.task_type = SET_FAIL;
+            }
+        }
     }
 
-    delete msg;
-
-    return status;
+    return ret;
 }
 
-void reportScan(RadioComms &comms)
+int16_t sendMessage(RadioComms &comms, Message &msg)
 {
     radioIsScan = false;
     int16_t status = comms.configureRadio();
     if (status != RADIOLIB_ERR_NONE)
     {
         Serial.printf("Failed to configure Radio: %d\n", status);
-        return;
+        return status;
     }
 
-    Message m;
-    m.type = SCAN_RESULT;
-    m.payload.dump = frequency_scan_result.dump;
-    status = comms.send(m);
-
-    m.payload.dump.sz = 0; // dump is shared, so should not delete underlying arrays
+    status = comms.send(msg);
 
     if (status != RADIOLIB_ERR_NONE)
     {
-        Serial.printf("Failed to send scan result: %d\n", status);
-        return;
+        Serial.printf("Failed to send message of type %d: %d\n", msg.type, status);
+        return status;
     }
+
+    return status;
+}
+
+void reportScan(RadioComms &comms)
+{
+    Message m;
+    m.type = SCAN_RESULT;
+    m.payload.dump = frequency_scan_result.dump;
+    int16_t status = sendMessage(comms, m);
+
+    m.payload.dump.sz = 0; // dump is shared, so should not delete underlying arrays
 }