init

User/meshcore/packets/ack.c

@@ -0,0 +1,43 @@
+#include "ack.h"
+#include "lib/cifra/sha2.h"
+#include "lib/config.h"
+#include "meshcore/meshframing.h"
+#include <string.h>
+
+#define TAG "Ack"
+
+void sendDiscreteAck (uint8_t *data, const uint8_t len, uint8_t *senderPubKey) {
+    FrameStruct frame;
+    frame.path.pathLen = 0;
+    memset (&frame, 0, sizeof (frame));
+
+    // 1. Header
+    frame.header =
+        ROUTE_TYPE_FLOOD |  // currently flood
+        PAYLOAD_TYPE_ACK |
+        PAYLOAD_VERSION_0;
+
+    // Buffer for the digest
+    uint8_t hash[CF_SHA256_HASHSZ];
+
+    // Context
+    cf_sha256_context ctx;
+
+    // 1. Initialize
+    cf_sha256_init (&ctx);
+
+    // 2. Feed in your data
+    cf_sha256_update (&ctx, data, len);
+
+    cf_sha256_update (&ctx, senderPubKey, sizeof (persistent.pubkey));
+
+    // 3. Compute digest
+    cf_sha256_digest (&ctx, hash);
+
+    memcpy (frame.payload, hash, 4);
+    // 5. Finalize
+    frame.payloadLen = 4;
+
+
+    LoRaTransmit (&frame);
+}

User/meshcore/packets/ack.h

@@ -0,0 +1,8 @@
+#ifndef ACK_HEADER
+#define ACK_HEADER
+
+#include "meshcore/packetstructs.h"
+
+void sendDiscreteAck (uint8_t *data, const uint8_t len, uint8_t *senderPubKey);
+
+#endif

User/meshcore/packets/advert.c

@@ -0,0 +1,302 @@
+#include "ch32v20x_rtc.h"
+#include "lib/config.h"
+#include "lib/monocypher/monocypher-ed25519.h"
+#include "meshcore/meshframing.h"
+#include "meshcore/packetstructs.h"
+#include <string.h>
+#include "advert.h"
+#include "util/hexdump.h"
+#include "lib/base64.h"
+#include "util/log.h"
+
+#define TAG "Advert"
+
+static void build_ad_message (const FrameStruct *frame,
+                              uint8_t *out,
+                              size_t *out_len) {
+    size_t idx = 0;
+
+    // pubkey + timestamp (your 36 bytes)
+    memcpy (out + idx, frame->payload, 36);
+    idx += 36;
+
+    // rest of payload after signature region
+    memcpy (out + idx,
+            &frame->payload[100],
+            frame->payloadLen - 100);
+
+    idx += (frame->payloadLen - 100);
+
+    *out_len = idx;
+}
+
+void ed25519_sign_ad (FrameStruct *frame) {
+    uint8_t *signature = &frame->payload[36];
+
+    uint8_t message[256];
+    size_t m = 0;
+
+    // pubkey + timestamp (0..35)
+    memcpy (message + m, frame->payload, 36);
+    m += 36;
+
+    // skip signature [36..99]
+
+    // payload after signature
+    memcpy (message + m,
+            frame->payload + 100,
+            frame->payloadLen - 100);
+    m += frame->payloadLen - 100;
+
+    crypto_ed25519_sign (signature,
+                         persistent.privkey,
+                         message,
+                         m);
+}
+
+int ed25519_verify_ad (const FrameStruct *frame) {
+    const uint8_t *signature = &frame->payload[36];
+
+    uint8_t message[256];
+    size_t m = 0;
+
+    if (signature[63] & 224)
+        return 0;
+
+    // pubkey + timestamp
+    memcpy (message + m, frame->payload, 36);
+    m += 36;
+
+    // skip signature [36..99]
+
+    // payload after signature
+    memcpy (message + m,
+            frame->payload + 100,
+            frame->payloadLen - 100);
+    m += frame->payloadLen - 100;
+
+    return crypto_ed25519_check (signature,
+                                 frame->payload,  // pubkey at start
+                                 message,
+                                 m) == 0;
+}
+
+void sendAdvert (uint8_t shouldFlood) {
+    FrameStruct frame;
+    size_t offset = 0;
+
+    frame.header = (shouldFlood ? ROUTE_TYPE_FLOOD : ROUTE_TYPE_DIRECT) | PAYLOAD_TYPE_ADVERT | PAYLOAD_VERSION_0;
+
+    /* ---- public key ---- */
+    memcpy (frame.payload + offset, persistent.pubkey, 32);
+    offset += 32;
+
+    /* ---- timestamp ---- */
+    uint32_t timestamp = RTC_GetCounter();
+    memcpy (frame.payload + offset, &timestamp, sizeof (timestamp));
+    offset += sizeof (timestamp);
+
+    /* ---- reserve signature ---- */
+    uint8_t *signature_pos = frame.payload + offset;
+    offset += 64;
+
+    /* ---- build app data directly into payload ---- */
+    size_t app_start = offset;
+
+    uint8_t dataFlags = ADVERTISEMENT_FLAG_HAS_NAME;
+    if (persistent.nodeType == NODE_TYPE_CHAT_NODE)
+        dataFlags |= ADVERTISEMENT_FLAG_IS_CHAT_NODE;
+    else if (persistent.nodeType == NODE_TYPE_REPEATER)
+        dataFlags |= ADVERTISEMENT_FLAG_IS_REAPEATER;
+    else if (persistent.nodeType == NODE_TYPE_ROOM_SERVER)
+        dataFlags |= ADVERTISEMENT_FLAG_IS_ROOM_SERVER;
+    else if (persistent.nodeType == NODE_TYPE_SENSOR)
+        dataFlags |= ADVERTISEMENT_FLAG_IS_SENSOR;
+
+    frame.payload[offset++] = dataFlags;
+
+    if (dataFlags & ADVERTISEMENT_FLAG_HAS_LOCATION) {
+        memcpy (frame.payload + offset, &persistent.latitude, sizeof (persistent.latitude));
+        offset += sizeof (persistent.latitude);
+
+        memcpy (frame.payload + offset, &persistent.longitude, sizeof (persistent.longitude));
+        offset += sizeof (persistent.longitude);
+    }
+    /*
+
+    if (dataFlags & ADVERTISEMENT_FLAG_RFU1) {
+        memcpy(frame.payload + offset, &persistent.rfu1, sizeof(persistent.rfu1));
+        offset += sizeof(persistent.rfu1);
+    }
+
+    if (dataFlags & ADVERTISEMENT_FLAG_RFU2) {
+        memcpy(frame.payload + offset, &persistent.rfu2, sizeof(persistent.rfu2));
+        offset += sizeof(persistent.rfu2);
+    }
+    */
+
+    if (dataFlags & ADVERTISEMENT_FLAG_HAS_NAME) {
+        size_t nameLen = strlen (persistent.nodeName);
+        memcpy (frame.payload + offset, persistent.nodeName, nameLen);
+        offset += nameLen;
+    }
+
+    size_t app_len = offset - app_start;
+
+    frame.payloadLen = offset;
+    /* ---- sign directly over payload ---- */
+    ed25519_sign_ad (&frame);
+
+    /* ---- debug ---- */
+    hexdump ("Public key", frame.payload, 32);
+    hexdump ("Signature", signature_pos, 64);
+    hexdump ("Appdata", frame.payload + app_start, app_len);
+    iprintf ("Timestamp is %lu\n", timestamp);
+    iprintf ("NodeName %s\n", persistent.nodeName);
+
+    /* ---- send ---- */
+    frame.payloadLen = offset;
+    frame.path.pathLen = 0;
+    LoRaTransmit (&frame);
+}
+
+void decodeAdvertisement (const FrameStruct *frame) {
+    AdvertisementPayload advert;
+    memset (&advert, 0, sizeof (advert));
+
+    advert.valid = 0;
+    if (frame->payloadLen < 101) {
+        MESH_LOGW (TAG, "Advertisement frame too short (%d < 101)", frame->payloadLen);
+        return;
+    }
+
+    if (!ed25519_verify_ad (frame)) {
+        MESH_LOGW (TAG, "Incorrect signature");
+        return;
+    } else {
+        MESH_LOGI (TAG, "Advertisement signature ok.");
+    }
+    advert.valid = 1;
+
+    unsigned char index = 0;
+
+    memcpy (advert.pubKey, frame->payload + index, 32);
+    index += 32;
+
+    memcpy (&advert.timestamp, frame->payload + index, 4);
+    index += 4;
+
+    memcpy (advert.signature, frame->payload + index, 64);
+    index += 64;
+
+    advert.dataFlags = frame->payload[index++];
+
+    uint8_t expectedLen = 101;
+
+    if (advert.dataFlags & ADVERTISEMENT_FLAG_HAS_LOCATION) {
+        expectedLen += 8;
+    }
+
+    if (advert.dataFlags & ADVERTISEMENT_FLAG_RFU1) {
+        expectedLen += 2;
+    }
+    if (advert.dataFlags & ADVERTISEMENT_FLAG_RFU2) {
+        expectedLen += 2;
+    }
+
+    if (frame->payloadLen < expectedLen) {
+        MESH_LOGW (TAG, "Advertisement frame with data too short (%d < %d)", frame->payloadLen, expectedLen);
+        return;
+    }
+
+    if (advert.dataFlags & ADVERTISEMENT_FLAG_HAS_LOCATION) {
+        memcpy (&advert.latitude, frame->payload + index, 4);
+        index += 4;
+        memcpy (&advert.longitude, frame->payload + index, 4);
+        index += 4;
+    }
+
+    if (advert.dataFlags & ADVERTISEMENT_FLAG_RFU1) {
+        memcpy (&advert.rfu1, frame->payload + index, 2);
+        index += 2;
+    }
+    if (advert.dataFlags & ADVERTISEMENT_FLAG_RFU2) {
+        memcpy (&advert.rfu2, frame->payload + index, 2);
+        index += 2;
+    }
+    unsigned char nameLen = frame->payloadLen - index;
+
+    if (nameLen > 31) {
+        nameLen = 31;  // leave space for null
+    }
+    memcpy (advert.nodeName, frame->payload + index, nameLen);
+    advert.nodeName[nameLen] = 0;
+
+
+    printAdvertisement (&advert);
+    saveAdvert (&advert);
+}
+
+void saveAdvert (const AdvertisementPayload *advert) {
+    NodeEntry *node = getNode (advert->pubKey[0]);
+
+    if (node == NULL) {
+        node = getNextNode();
+        memset (node, 0, sizeof (NodeEntry));
+    }
+
+    memcpy (node->name, advert->nodeName, sizeof (node->name));
+    memcpy (node->pubKey, advert->pubKey, sizeof (node->pubKey));
+
+    // -------------------------------
+    // Ed25519 ¡ú X25519 conversion
+    // -------------------------------
+    uint8_t peer_x[32];
+    crypto_eddsa_to_x25519 (peer_x, advert->pubKey);
+
+    // -------------------------------
+    // IMPORTANT: derive correct scalar
+    // (THIS fixes your HMAC mismatch)
+    // -------------------------------
+    uint8_t scalar[64];
+    uint8_t scalarOut[32];
+
+    crypto_sha512 (scalar, persistent.privkey, 32);
+    crypto_eddsa_trim_scalar (scalarOut, scalar);
+
+    // -------------------------------
+    // X25519 Diffie-Hellman
+    // -------------------------------
+    crypto_x25519 (node->secret,
+                   scalarOut,
+                   peer_x);
+
+
+    node->gps_latitude = advert->latitude;
+    node->gps_longitude = advert->longitude;
+
+    // add path
+
+    node->type = advert->dataFlags & 0x0F;
+    node->last_seen_lt = RTC_GetCounter();
+    node->last_seen_rt = advert->timestamp;
+}
+
+void printAdvertisement (const AdvertisementPayload *advert) {
+    iprintf (
+        "%s on %ld with type %s on %s location %ld %ld\n",
+        advert->dataFlags & ADVERTISEMENT_FLAG_HAS_NAME
+            ? advert->nodeName
+            : "nameless node",
+        advert->timestamp,
+        (advert->dataFlags & 0x07) == 0x04   ? "sensor"
+        : (advert->dataFlags & 0x07) == 0x03 ? "room server"
+        : (advert->dataFlags & 0x07) == 0x02 ? "repeater"
+                                             : "chat node",
+        advert->dataFlags & 0x80 ? "known" : "unknown",
+        advert->latitude,
+        advert->longitude);
+
+    hexdump ("Public key", advert->pubKey, 32);
+    hexdump ("Signature", advert->signature, 64);
+}

User/meshcore/packets/advert.h

@@ -0,0 +1,14 @@
+#ifndef ADVERT_HEADER
+#define ADVERT_HEADER
+
+#include "meshcore/packetstructs.h"
+
+void sendAdvert (uint8_t shouldFlood);
+
+void decodeAdvertisement (const FrameStruct *frame);
+
+void printAdvertisement (const AdvertisementPayload *advert);
+
+void saveAdvert (const AdvertisementPayload *advert);
+
+#endif

User/meshcore/packets/anonymous.c

@@ -0,0 +1,234 @@
+#include "ch32v20x_rtc.h"
+#include "lib/config.h"
+#include "lib/monocypher/monocypher-ed25519.h"
+#include "meshcore/meshframing.h"
+#include "meshcore/packets/encrypted.h"
+#include "meshcore/packetstructs.h"
+#include "anonymous.h"
+#include "util/hexdump.h"
+#include <stdlib.h>
+
+#define TAG "Anonymous"
+
+void sendAnonymousRequest (const NodeEntry *targetNode, const uint8_t *password, uint32_t sync) {
+    uint8_t passwordLen = strlen ((const char *)password);
+    FrameStruct frame;
+    frame.path.pathLen = 0;
+    uint8_t offset = 0;
+
+    // 1. Frame header
+    frame.header = ((targetNode->path.pathLen > 0) ? ROUTE_TYPE_DIRECT : ROUTE_TYPE_FLOOD) | PAYLOAD_TYPE_ANON_REQ | PAYLOAD_VERSION_0;
+
+    // 2. Payload header (unencrypted)
+    frame.payload[offset++] = targetNode->pubKey[0];
+
+    memcpy (frame.payload + offset, persistent.pubkey, 32);
+    offset += 32;
+
+    // 3. Build plaintext payload
+    uint8_t plaintext[32];
+    uint8_t p = 0;
+
+    uint32_t last_seen_rt = RTC_GetCounter();
+    plaintext[p++] = (last_seen_rt >> 0) & 0xFF;
+    plaintext[p++] = (last_seen_rt >> 8) & 0xFF;
+    plaintext[p++] = (last_seen_rt >> 16) & 0xFF;
+    plaintext[p++] = (last_seen_rt >> 24) & 0xFF;
+
+    if (targetNode->type == NODE_TYPE_ROOM_SERVER) {
+        plaintext[p++] = (sync >> 0) & 0xFF;
+        plaintext[p++] = (sync >> 8) & 0xFF;
+        plaintext[p++] = (sync >> 16) & 0xFF;
+        plaintext[p++] = (sync >> 24) & 0xFF;
+    }
+
+    if (passwordLen > 16) {
+        passwordLen = 16;
+    }
+    memcpy (plaintext + p, password, passwordLen);
+    p += passwordLen;
+
+    size_t outputLen;
+    // 4. Encrypt + MAC
+    encrypt_then_mac (
+        targetNode->secret,
+        32,
+        plaintext,
+        p,
+        frame.payload + offset,
+        &outputLen);
+
+    offset += outputLen;
+
+    // 5. Finalize and send
+    frame.payloadLen = offset;
+    memcpy (&(frame.path), &(targetNode->path), sizeof (frame.path));
+
+    hexdump ("Anon payload", frame.payload, frame.payloadLen);
+
+    LoRaTransmit (&frame);
+}
+
+void printAnonRequest (const AnonymousRequestPayload *req, int isRoomServer) {
+    if (!req)
+        return;
+
+    iprintf ("AnonymousRequestPayload at %p\n", (void *)req);
+    iprintf ("  destination hash: 0x%02X\n", req->destinationHash);
+
+    iprintf ("  sender pubKey: ");
+    for (int i = 0; i < sizeof (req->pubKey); i++) {
+        iprintf ("%02X", req->pubKey[i]);
+    }
+    iprintf ("\n");
+
+    iprintf ("  cipher MAC: 0x%04X\n", req->cipherMAC);
+
+    iprintf ("  decrypted payload (%u bytes):\n", req->payloadLen);
+    uint8_t index = 0;
+
+    // timestamp (first 4 bytes)
+    if (req->payloadLen >= 4) {
+        uint32_t timestamp = req->payload[index++];
+        timestamp |= req->payload[index++] << 8;
+        timestamp |= req->payload[index++] << 16;
+        timestamp |= req->payload[index++] << 24;
+        iprintf ("    timestamp: %u\n", timestamp);
+    }
+
+    // room server sync timestamp
+    if (isRoomServer && req->payloadLen >= index + 4) {
+        uint32_t syncTimestamp = req->payload[index++];
+        syncTimestamp |= req->payload[index++] << 8;
+        syncTimestamp |= req->payload[index++] << 16;
+        syncTimestamp |= req->payload[index++] << 24;
+        iprintf ("    sync timestamp: %u\n", syncTimestamp);
+    }
+
+
+    // remaining bytes = password
+    if (index < req->payloadLen) {
+        uint8_t passwordLen = req->payloadLen - index;
+        if (passwordLen > 16)
+            passwordLen = 16;
+        passwordLen = strnlen (&(req->payload[index]), passwordLen);
+        iprintf ("    password: ");
+        for (uint8_t i = 0; i < passwordLen; i++) {
+            iprintf ("%c", req->payload[i + index]);
+        }
+        iprintf ("\n");
+    }
+}
+
+size_t strnlen (const char *s, size_t maxLen) {
+    size_t len = 0;
+    while (len < maxLen && s[len] != '\0') {
+        len++;
+    }
+    return len;
+}
+
+void decodeAnonReq (const FrameStruct *frame) {
+    uint8_t index = 0;
+    AnonymousRequestPayload anonReq;
+
+    anonReq.destinationHash = frame->payload[index++];
+    memcpy (anonReq.pubKey, &(frame->payload[index]), sizeof (anonReq.pubKey));
+    index += sizeof (anonReq.pubKey);
+    anonReq.cipherMAC = frame->payload[index];
+    anonReq.cipherMAC |= frame->payload[index + 1] << 8;
+
+    NodeEntry *foundNode = getNode (anonReq.pubKey[0]);
+
+    if (foundNode == NULL) {
+        foundNode = getNextNode();
+        memset (foundNode, 0, sizeof (NodeEntry));
+
+        strcpy (foundNode->name, "Anonymous node");
+        foundNode->path.pathLen = 0;
+
+        memcpy (foundNode->pubKey,
+                anonReq.pubKey,
+                sizeof (foundNode->pubKey));
+
+        // --- X25519 conversion + DH ---
+        uint8_t peer_x[32];
+
+        crypto_eddsa_to_x25519 (peer_x, anonReq.pubKey);
+
+        crypto_x25519 (foundNode->secret,
+                       persistent.privkey,
+                       peer_x);
+
+        foundNode->gps_latitude = 0;
+        foundNode->gps_longitude = 0;
+        foundNode->type = 0;
+        foundNode->last_seen_lt = RTC_GetCounter();
+
+        MESH_LOGI (TAG, "New anonymous node created: %s", foundNode->name);
+    } else {
+        MESH_LOGD (TAG,
+                   "Existing node found for pubKey[0]=0x%02X",
+                   anonReq.pubKey[0]);
+    }
+
+    mac_then_decrypt (foundNode->secret, 32, &(frame->payload[index]), frame->payloadLen - index, anonReq.payload);
+    anonReq.payloadLen = frame->payloadLen - index - 2;
+
+    hexdump ("AnonReq payload", anonReq.payload, anonReq.payloadLen);
+
+    uint8_t index2 = 0;
+    foundNode->last_seen_rt = anonReq.payload[index2++];
+    foundNode->last_seen_rt |= anonReq.payload[index2++] << 8;
+    foundNode->last_seen_rt |= anonReq.payload[index2++] << 16;
+    foundNode->last_seen_rt |= anonReq.payload[index2++] << 24;
+
+    if (persistent.nodeType == NODE_TYPE_ROOM_SERVER) {
+        foundNode->sync_timestamp = anonReq.payload[index2++];
+        foundNode->sync_timestamp |= anonReq.payload[index2++] << 8;
+        foundNode->sync_timestamp |= anonReq.payload[index2++] << 16;
+        foundNode->sync_timestamp |= anonReq.payload[index2++] << 24;
+    }
+
+    printAnonRequest (&anonReq, persistent.nodeType == NODE_TYPE_ROOM_SERVER);
+
+    uint8_t passwordLen = anonReq.payloadLen - index2;
+    if (passwordLen > 16) {
+        passwordLen = 16;
+    }
+
+    passwordLen = strnlen (&(anonReq.payload[index2]), passwordLen);
+
+    MESH_LOGI (TAG, "Password len is %d.", passwordLen);
+    uint8_t passwordBuf[16];
+    memcpy (passwordBuf, &(anonReq.payload[index2]), passwordLen);
+
+    if (memcmp (passwordBuf, persistent.password, passwordLen) == 0) {
+        foundNode->authenticated = 1;
+        MESH_LOGI (TAG, "Password correct, node %s authenticated.", foundNode->name);
+
+        MESH_LOGI (TAG, "Login response sent to node %s.", foundNode->name);
+    } else {
+        MESH_LOGW (TAG, "Password incorrect for node %s.", foundNode->name);
+    }
+
+    Response resp;
+    resp.tag = RTC_GetCounter();
+    uint8_t index3 = 0;
+    uint32_t randOut = rand();
+    resp.data[index3++] = RESP_SERVER_LOGIN_OK;
+    resp.data[index3++] = 0;                                                           // legacy
+    resp.data[index3++] = foundNode->authenticated;                                    // isadmin
+    resp.data[index3++] = foundNode->authenticated ? PERM_ACL_ADMIN : PERM_ACL_GUEST;  // permissions
+    resp.data[index3++] = randOut & 0xFF;
+    resp.data[index3++] = (randOut >> 8) & 0xFF;
+    resp.data[index3++] = (randOut >> 16) & 0xFF;
+    resp.data[index3++] = (randOut >> 24) & 0xFF;
+    resp.data[index3++] = FIRMWARE_VER_LEVEL;
+    resp.dataLen = index3;
+    if ((frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_FLOOD ||
+        (frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_FLOOD) {
+        sendPathBack (foundNode, &(frame->path));
+    }
+    sendEncryptedResponse (foundNode, &resp);
+}

User/meshcore/packets/anonymous.h

@@ -0,0 +1,11 @@
+#ifndef ANONYMOUS_HEADER
+#define ANONYMOUS_HEADER
+
+#include "meshcore/packetstructs.h"
+#include "lib/config.h"
+
+void decodeAnonReq (const FrameStruct *frame);
+
+void sendAnonymousRequest (const NodeEntry *targetNode, const uint8_t *password, uint32_t sync);
+
+#endif

User/meshcore/packets/control.c

@@ -0,0 +1,148 @@
+#include "lib/config.h"
+#include "meshcore/meshframing.h"
+#include "meshcore/packetstructs.h"
+#include "control.h"
+#include "meshcore/stats.h"
+#include "string.h"
+#include "util/hexdump.h"
+#include "util/log.h"
+#include <stdio.h>
+
+#define TAG "Control"
+
+void sendDiscoverRequest (const DiscoverRequestPayload *discReq) {
+
+    FrameStruct frame;
+    frame.path.pathLen = 0;
+    uint8_t offset = 0;
+
+    // Build payload
+    frame.payload[offset++] = (discReq->prefixOnly & 0x01) | CONTROL_DATA_FLAG_TYPE_NODE_DISCOVER_REQ;
+
+    frame.payload[offset++] = discReq->typeFilter;
+
+    frame.payload[offset++] = (discReq->tag >> 0) & 0xFF;
+    frame.payload[offset++] = (discReq->tag >> 8) & 0xFF;
+    frame.payload[offset++] = (discReq->tag >> 16) & 0xFF;
+    frame.payload[offset++] = (discReq->tag >> 24) & 0xFF;
+
+    // optional `since`
+    if (discReq->since != 0) {  // or another condition if you want to always include
+        frame.payload[offset++] = (discReq->since >> 0) & 0xFF;
+        frame.payload[offset++] = (discReq->since >> 8) & 0xFF;
+        frame.payload[offset++] = (discReq->since >> 16) & 0xFF;
+        frame.payload[offset++] = (discReq->since >> 24) & 0xFF;
+    }
+
+    frame.payloadLen = offset;
+
+    LoRaTransmit (&frame);
+}
+
+void sendDiscoverResponse (const DiscoverResponsePayload *discResp) {
+    FrameStruct frame;
+    frame.header = ROUTE_TYPE_DIRECT | PAYLOAD_TYPE_CONTROL | PAYLOAD_VERSION_0;
+    frame.path.pathLen = 0;
+    uint8_t offset = 0;
+
+    /* Control type + node type (lower nibble) */
+    frame.payload[offset++] =
+        (discResp->nodeType & 0x0F) |
+        CONTROL_DATA_FLAG_DISCOVER_RESP;
+
+    /* SNR */
+    frame.payload[offset++] = (uint8_t)discResp->snr;
+
+    /* Tag (LE) */
+    frame.payload[offset++] = (discResp->tag >> 0) & 0xFF;
+    frame.payload[offset++] = (discResp->tag >> 8) & 0xFF;
+    frame.payload[offset++] = (discResp->tag >> 16) & 0xFF;
+    frame.payload[offset++] = (discResp->tag >> 24) & 0xFF;
+
+    /* Pubkey */
+    if (discResp->pubkeyLen > 0) {
+        memcpy (&frame.payload[offset],
+                discResp->pubkey,
+                discResp->pubkeyLen);
+        offset += discResp->pubkeyLen;
+    }
+
+    frame.payloadLen = offset;
+
+    LoRaTransmit (&frame);
+}
+
+void printDiscoverRequest (const DiscoverRequestPayload *p) {
+    iprintf ("=== Discover Request ===\n");
+    iprintf ("prefixOnly : %u\n", p->prefixOnly);
+    iprintf ("typeFilter : 0x%02X\n", p->typeFilter);
+    iprintf ("tag        : 0x%08lX\n", (unsigned long)p->tag);
+    iprintf ("since      : 0x%08lX\n", (unsigned long)p->since);
+}
+
+void printDiscoverResponse (const DiscoverResponsePayload *p) {
+    iprintf ("=== Discover Response ===\n");
+    iprintf ("nodeType : %u\n", p->nodeType);
+    iprintf ("snr      : %u\n", p->snr);
+    iprintf ("tag      : 0x%08lX\n", (unsigned long)p->tag);
+
+    hexdump ("pubkey   : ", p->pubkey, p->pubkeyLen);
+    iprintf ("\n");
+}
+
+void decodeControlFrame (const FrameStruct *frame) {
+    uint8_t index = 0;
+    uint8_t type = frame->payload[index] & 0xF0;
+    if (type == CONTROL_DATA_FLAG_TYPE_NODE_DISCOVER_REQ) {
+        DiscoverRequestPayload discReq;
+        discReq.prefixOnly = frame->payload[index++] & 0x01;
+
+        discReq.typeFilter = frame->payload[index++];
+
+        discReq.tag = frame->payload[index++];
+        discReq.tag |= frame->payload[index++] << 8;
+        discReq.tag |= frame->payload[index++] << 16;
+        discReq.tag |= frame->payload[index++] << 24;
+
+        if (index < frame->payloadLen) {
+            discReq.since = frame->payload[index++];
+            discReq.since |= frame->payload[index++] << 8;
+            discReq.since |= frame->payload[index++] << 16;
+            discReq.since |= frame->payload[index++] << 24;
+        }
+        printDiscoverRequest (&discReq);
+        if ((discReq.typeFilter >> 1) & persistent.nodeType) {
+            DiscoverResponsePayload discResp;
+
+            discResp.tag = discReq.tag;
+            discResp.nodeType = persistent.nodeType;
+            discResp.pubkeyLen = sizeof (persistent.pubkey);
+            memcpy (discResp.pubkey, persistent.pubkey, discResp.pubkeyLen);
+            discResp.snr = stats.lastSNR;  // hopefully the correct one
+
+
+            MESH_LOGD (TAG, "Replying to a discover request with tag %d", discResp.tag);
+            sendDiscoverResponse (&discResp);
+            printDiscoverResponse (&discResp);
+        }
+
+    } else if (type == CONTROL_DATA_FLAG_DISCOVER_RESP) {
+        DiscoverResponsePayload discResp;
+        discResp.nodeType = frame->payload[index++] & 0x0F;
+
+        discResp.snr = frame->payload[index++];
+
+        discResp.tag = frame->payload[index++];
+        discResp.tag |= frame->payload[index++] << 8;
+        discResp.tag |= frame->payload[index++] << 16;
+        discResp.tag |= frame->payload[index++] << 24;
+
+        uint8_t remainingLen = frame->payloadLen - index;
+
+        uint8_t pubKeyLen = (remainingLen > 8) ? sizeof (discResp.pubkey) : 8;
+        discResp.pubkeyLen = pubKeyLen;
+        memcpy (discResp.pubkey, &(frame->payload[index]), discResp.pubkeyLen);
+        index += pubKeyLen;
+        printDiscoverResponse (&discResp);
+    }
+}

User/meshcore/packets/control.h

@@ -0,0 +1,16 @@
+#ifndef CONTROL_HEADER
+#define CONTROL_HEADER
+
+#include "meshcore/packetstructs.h"
+
+void sendDiscoverRequest (const DiscoverRequestPayload *discReq);
+
+void sendDiscoverResponse (const DiscoverResponsePayload *discResp);
+
+void printDiscoverRequest (const DiscoverRequestPayload *p);
+
+void printDiscoverResponse (const DiscoverResponsePayload *p);
+
+void decodeControlFrame (const FrameStruct *frame);
+
+#endif

User/meshcore/packets/custom.c

@@ -0,0 +1,3 @@
+#include "meshcore/packetstructs.h"
+
+#define TAG "Custom"

User/meshcore/packets/custom.h

@@ -0,0 +1,5 @@
+#ifndef CUSTOM_HEADER
+#define CUSTOM_HEADER
+
+
+#endif

User/meshcore/packets/encrypted.c

@@ -0,0 +1,837 @@
+#include "lib/config.h"
+#include "ch32v20x.h"
+#include "lib/telemetry/telemetry.h"
+#include "meshcore/meshframing.h"
+#include "meshcore/packets/ack.h"
+#include "meshcore/packets/advert.h"
+#include "meshcore/packetstructs.h"
+#include "meshcore/stats.h"
+#include "util/hexdump.h"
+#include "util/log.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "encrypted.h"
+#include "lib/adc/temperature.h"
+#include "lib/rtc/rtc.h"
+#include "sx1262.h"
+
+#define TAG "EncryptedMessage"
+
+void sendEncryptedFrame (const NodeEntry *targetNode, uint8_t payloadType, const uint8_t *plain, size_t plainLen) {
+    FrameStruct frame;
+    memset(&frame, 0, sizeof(frame));
+    size_t offset = 0;
+
+    // 1. Header
+    frame.header =
+        (targetNode->path.pathLen > 0 ? ROUTE_TYPE_DIRECT : ROUTE_TYPE_FLOOD) |  // currently flood
+        payloadType |
+        PAYLOAD_VERSION_0;
+
+    // 2. Destination + source
+    frame.payload[offset++] = targetNode->pubKey[0];
+    frame.payload[offset++] = persistent.pubkey[0];
+
+    // 4. Encrypt + MAC
+    size_t encLen;
+    encrypt_then_mac (
+        targetNode->secret,
+        32,
+        plain,
+        plainLen,
+        frame.payload + offset,
+        &encLen);
+
+    MESH_LOGD(TAG, "Plain len: %d, enc len: %d", plainLen, encLen);
+
+    offset += encLen;
+
+    // 5. Finalize
+    frame.payloadLen = offset;
+    memcpy (&frame.path, &targetNode->path, sizeof (frame.path));
+
+    hexdump ("Encrypted frame", frame.payload, frame.payloadLen);
+    LoRaTransmit (&frame);
+    MESH_LOGD (TAG, "Encrypted frame tx finish\n");
+}
+
+void sendEncryptedTextMessage (const NodeEntry *targetNode, const PlainTextMessagePayload *msg) {
+    if (targetNode == NULL) {
+        MESH_LOGW (TAG, "Node is null");
+        return;
+    }
+    if (targetNode->last_seen_lt == 0) {
+        MESH_LOGW (TAG, "Node is not populated");
+        return;
+    }
+    uint8_t buf[256];
+    uint8_t index = 0;
+
+    uint8_t msgLen = strlen (msg->message) + 1;
+    buf[index++] = msg->timestamp;
+    buf[index++] = msg->timestamp >> 8;
+    buf[index++] = msg->timestamp >> 16;
+    buf[index++] = msg->timestamp >> 24;
+
+    buf[index++] = (msg->textType << 2) | (msg->attempt & 0x03);
+    memcpy (&buf[index], msg->message, msgLen);
+    index += msgLen;
+
+    sendEncryptedFrame (
+        targetNode,
+        PAYLOAD_TYPE_TXT_MSG,
+        buf,
+        index);
+}
+
+void sendEncryptedResponse (const NodeEntry *targetNode, const Response *resp) {
+    uint8_t buf[256];
+    uint8_t index = 0;
+
+    buf[index++] = (resp->tag) & 0xFF;
+    buf[index++] = (resp->tag >> 8) & 0xFF;
+    buf[index++] = (resp->tag >> 16) & 0xFF;
+    buf[index++] = (resp->tag >> 24) & 0xFF;
+
+    memcpy (&(buf[index]), resp->data, resp->dataLen);
+    index += resp->dataLen;
+
+    sendEncryptedFrame (
+        targetNode,
+        PAYLOAD_TYPE_RESPONSE,
+        buf,
+        index);
+}
+
+void sendEncryptedRequest (const NodeEntry *targetNode, const Request *req) {
+    uint8_t buf[256];
+    uint8_t index = 0;
+
+    buf[index++] = req->timestamp;
+    buf[index++] = req->timestamp >> 8;
+    buf[index++] = req->timestamp >> 16;
+    buf[index++] = req->timestamp >> 24;
+
+    buf[index++] = req->requestType;
+    memcpy (&(buf[index]), req->data, req->dataLen);
+    index += req->dataLen;
+
+    sendEncryptedFrame (
+        targetNode,
+        PAYLOAD_TYPE_REQ,
+        buf,
+        index);
+}
+
+void sendEncryptedPathPayload (const NodeEntry *targetNode, const ReturnedPathPayload *path) {
+    uint8_t buf[256];
+    uint8_t index = 0;
+
+    buf[index++] = path->path.pathLen;
+    memcpy (&buf[index], path->path.path, path->path.pathLen);
+    index += path->path.pathLen;
+
+    if (path->extra.dataLen > 0) {
+        buf[index++] = path->extra.type;
+        memcpy (&buf[index], path->extra.data, path->extra.dataLen);
+    } else {
+        buf[index++] = 0xFF;
+        uint32_t timestamp = RTC_GetCounter();
+        buf[index++] = timestamp;
+        buf[index++] = timestamp >> 8;
+        buf[index++] = timestamp >> 16;
+        buf[index++] = timestamp >> 24;
+    }
+
+    sendEncryptedFrame (
+        targetNode,
+        PAYLOAD_TYPE_PATH,
+        buf,
+        index);
+}
+
+void printRequest (const Request *req) {
+    iprintf ("Request:\n");
+    iprintf ("  Timestamp: %u\n", req->timestamp);
+    iprintf ("  Type: 0x%02X\n", req->requestType);
+    iprintf ("  Data: ");
+    hexdump ("  Data", req->data, req->dataLen);
+}
+
+void printResponse (const Response *resp) {
+    iprintf ("Response:\n");
+    iprintf ("  Tag: %u\n", resp->tag);
+    iprintf ("  Data: ");
+    hexdump ("  Data", resp->data, resp->dataLen);
+}
+
+void printPlainTextMessage (const PlainTextMessagePayload *msg) {
+    iprintf ("PlainTextMessage:\n");
+    iprintf ("  Timestamp: %u\n", msg->timestamp);
+    iprintf ("  Attempt: %u\n", msg->attempt);
+    iprintf ("  TextType: %u\n", msg->textType);
+    iprintf ("  Message: %.*s\n", (int)strlen (msg->message), msg->message);
+}
+
+void printReturnedPathPayload (const ReturnedPathPayload *path) {
+    iprintf ("ReturnedPathPayload:\n");
+    iprintf ("  Path Length: %u\n", path->path.pathLen);
+    iprintf ("  Path: ");
+    hexdump ("  Path:", path->path.path, path->path.pathLen);
+    iprintf ("  Extra Type: %u\n", path->extra.type);
+    iprintf ("  Extra Data: ");
+    hexdump ("  Extra data:", path->extra.data, path->extra.dataLen);
+}
+
+void printEncryptedPayload (const EncryptedPayloadStruct *enc) {
+    iprintf ("EncryptedPayload:\n");
+    iprintf ("  Type: 0x%02X\n", enc->type);
+    iprintf ("  DestinationHash: 0x%02X\n", enc->destinationHash);
+    iprintf ("  SourceHash: 0x%02X\n", enc->sourceHash);
+    iprintf ("  CipherMAC: 0x%04X\n", enc->cipherMAC);
+    iprintf ("  PayloadLen: %zu\n", enc->payloadLen);
+    iprintf ("  Payload: ");
+    for (size_t i = 0; i < enc->payloadLen; i++) {
+        iprintf ("%02X ", enc->payload[i]);
+    }
+    iprintf ("\n");
+}
+
+void decodeEncryptedPayload (const FrameStruct *frame) {
+    EncryptedPayloadStruct enc;
+    memset (&enc, 0, sizeof (enc));
+    enc.path = &(frame->path);
+    enc.origFrame = frame;
+    enc.type = frame->header & PAYLOAD_TYPE_MASK;
+    unsigned char index = 0;
+
+    enc.destinationHash = frame->payload[index++];
+    enc.sourceHash = frame->payload[index++];
+    enc.cipherMAC = frame->payload[index];
+    enc.cipherMAC |= frame->payload[index + 1] << 8;
+
+    if (enc.destinationHash != persistent.pubkey[0]) {
+        return;
+    }
+
+    MESH_LOGI (TAG, "Finding remote node, sourceHash is %d", enc.sourceHash);
+
+    NodeEntry *remNode = getNode (enc.sourceHash);
+
+    enc.remNode = remNode;
+
+    if (remNode == NULL) {
+        MESH_LOGW (TAG, "Node not in DB");
+        return;
+    }
+    remNode->last_seen_lt = RTC_GetCounter();
+    MESH_LOGI (TAG, "Found node with index %d", remNode - persistent.contacts);
+
+
+    if (mac_then_decrypt (remNode->secret, 32, &(frame->payload[index]), frame->payloadLen - index, enc.payload) != 0) {
+        MESH_LOGW (TAG, "HMAC failed on encrypted message %s", remNode->name);
+    } else {
+        enc.payloadLen = frame->payloadLen - HMAC_SIZE;
+        MESH_LOGI (TAG, "HMAC success from %s, %u bytes long", remNode->name, enc.payloadLen);
+        sendDiscreteAck (enc.payload, 5 + strlen ((char *)&enc.payload[5]), remNode->pubKey);
+    }
+
+    iprintf ("  Typexdd: 0x%02X\n", enc.type);
+    if (enc.payloadLen > 0) {
+        parseEncryptedPayload (&enc);
+    }
+}
+
+void sendPathBack (const NodeEntry *node, const Path *path) {
+    ReturnedPathPayload retPath;
+    retPath.extra.dataLen = 0;  // redo to send the resp in path
+    retPath.extra.type = 0xFF;
+
+    retPath.path.pathLen = path->pathLen;
+    memcpy (retPath.path.path, path->path, path->pathLen);
+
+    sendEncryptedPathPayload (node, &retPath);
+}
+
+void parseEncryptedPayload (const EncryptedPayloadStruct *enc) {
+    // printEncryptedPayload(&enc);
+
+    iprintf ("EncryptedPayload:\n");
+    iprintf ("  Type: 0x%02X\n", enc->type);
+    iprintf ("  DestinationHash: 0x%02X\n", enc->destinationHash);
+    iprintf ("  SourceHash: 0x%02X\n", enc->sourceHash);
+    iprintf ("  CipherMAC: 0x%04X\n", enc->cipherMAC);
+    iprintf ("  PayloadLen: %u\n", enc->payloadLen);
+    hexdump ("  Payload: ", enc->payload, enc->payloadLen);
+    iprintf ("\n");
+
+    uint8_t index = 0;
+    if (enc->type == PAYLOAD_TYPE_PATH) {
+        ReturnedPathPayload retPath;
+        retPath.path.pathLen = enc->payload[index++];
+        if (retPath.path.pathLen > 64) {
+            MESH_LOGW (TAG, "Path too long\n");
+            return;
+        }
+        memcpy (retPath.path.path, &(enc->payload[index]), retPath.path.pathLen);
+        index += retPath.path.pathLen;
+        retPath.extra.type = enc->payload[index++];
+        retPath.extra.dataLen = enc->payloadLen - index;
+        memcpy (retPath.extra.data, &(enc->payload[index]), retPath.extra.dataLen);
+
+        if ((enc->origFrame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_FLOOD ||
+            (enc->origFrame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_FLOOD) {
+            sendPathBack (enc->remNode, enc->path);
+        }
+
+    } else if (enc->type == PAYLOAD_TYPE_REQ) {
+        Request req;
+        req.timestamp = enc->payload[index++];
+        req.timestamp |= enc->payload[index++] << 8;
+        req.timestamp |= enc->payload[index++] << 16;
+        req.timestamp |= enc->payload[index++] << 24;
+        enc->remNode->last_seen_rt = req.timestamp;
+        req.requestType = enc->payload[index++];
+        req.dataLen = enc->payloadLen - index;
+        memcpy (req.data, &(enc->payload[index]), req.dataLen);
+        printRequest (&req);
+        if ((enc->origFrame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_FLOOD ||
+            (enc->origFrame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_FLOOD) {
+            sendPathBack (enc->remNode, enc->path);
+        }
+        switch (req.requestType) {
+        case REQUEST_GET_STATS: {
+            Response resp;
+            resp.tag = RTC_GetCounter();
+            stats.totalUpTimeSeconds = RTC_GetCounter() - startupTime;
+            //stats.totalAirTimeSeconds = TICKS_TO_MS (tickAirtime / 1000);
+            stats.totalAirTimeSeconds = 0;
+            memcpy (resp.data, &stats, sizeof (stats));
+            resp.dataLen = sizeof (stats);
+            sendEncryptedResponse (enc->remNode, &resp);
+            break;
+        }
+        case REQUEST_KEEPALIVE:
+            break;
+        case REQUEST_GET_TELEMETRY_DATA: {
+            Response resp;
+            resp.tag = req.timestamp;
+            enc->remNode->last_seen_rt = req.timestamp;
+            resp.dataLen = 0;
+            resp.data[resp.dataLen++] = TELEM_CHANNEL_SELF;
+            resp.data[resp.dataLen++] = LPP_TEMPERATURE;
+
+            int16_t dataTemp = getDeciTemperature();
+
+            resp.data[resp.dataLen++] = (dataTemp >> 8) & 0xFF;
+
+            resp.data[resp.dataLen++] = dataTemp & 0xFF;
+
+            resp.data[resp.dataLen++] = TELEM_CHANNEL_SELF;
+            resp.data[resp.dataLen++] = LPP_VOLTAGE;
+
+            int16_t dataVolt = stats.millivolts / 10;
+
+            resp.data[resp.dataLen++] = (dataVolt >> 8) & 0xFF;
+
+            resp.data[resp.dataLen++] = dataVolt & 0xFF;
+
+            resp.data[resp.dataLen++] = 2;
+            resp.data[resp.dataLen++] = LPP_VOLTAGE;
+
+            dataVolt = 1973;
+
+            resp.data[resp.dataLen++] = (dataVolt >> 8) & 0xFF;
+
+            resp.data[resp.dataLen++] = dataVolt & 0xFF;
+
+            if (enc->remNode->authenticated) {
+
+                resp.data[resp.dataLen++] = 2;  // channel 2
+                resp.data[resp.dataLen++] = LPP_TEMPERATURE;
+
+                int16_t jokeTemp = 6942;
+
+                resp.data[resp.dataLen++] = (jokeTemp >> 8) & 0xFF;
+
+                resp.data[resp.dataLen++] = jokeTemp & 0xFF;
+
+
+                encode_gps (TELEM_CHANNEL_SELF, persistent.latitude, persistent.longitude, persistent.altitude, &(resp.data[resp.dataLen]));
+                // encode_gps(TELEM_CHANNEL_SELF, 48.1909f, 17.0303f, 234.0f, &(resp.data[resp.dataLen]));
+
+                resp.dataLen += LPP_GPS_SIZE + 2;
+            }
+
+
+            sendEncryptedResponse (enc->remNode, &resp);
+
+            iprintf ("Sent response, the temperature is %d decicelsius\n", dataTemp);
+
+            break;
+        }
+        case REQUEST_GET_MIN_MAX_AVG:
+            break;
+        case REQUEST_GET_ACCESS_LIST:
+            break;
+        }
+
+    } else if (enc->type == PAYLOAD_TYPE_RESPONSE) {
+        Response resp;
+        resp.tag = enc->payload[index++];
+        resp.tag |= enc->payload[index++] << 8;
+        resp.tag |= enc->payload[index++] << 16;
+        resp.tag |= enc->payload[index++] << 24;
+        resp.dataLen = enc->payloadLen - index;
+        memcpy (resp.data, &(enc->payload[index]), resp.dataLen);
+        printResponse (&resp);
+
+    } else if (enc->type == PAYLOAD_TYPE_TXT_MSG) {
+        PlainTextMessagePayload plaintext;
+        plaintext.timestamp = enc->payload[index++];
+        plaintext.timestamp |= enc->payload[index++] << 8;
+        plaintext.timestamp |= enc->payload[index++] << 16;
+        plaintext.timestamp |= enc->payload[index++] << 24;
+        enc->remNode->last_seen_rt = plaintext.timestamp;
+        plaintext.attempt = enc->payload[index] & 0x03;
+        plaintext.textType = enc->payload[index++] >> 2;
+        memcpy (plaintext.message, &(enc->payload[index]), enc->payloadLen - index);
+        if ((enc->origFrame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_FLOOD ||
+            (enc->origFrame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_FLOOD) {
+            sendPathBack (enc->remNode, enc->path);
+        }
+        switch (plaintext.textType) {
+        case TXT_TYPE_PLAIN:
+            iprintf ("Plaintext message from %s, attempt %d, timestamp %d: %s", enc->remNode->name, plaintext.attempt, plaintext.timestamp, plaintext.message);
+            break;
+
+        case TXT_TYPE_CLI_DATA:
+            if (enc->remNode->authenticated) {
+                processCommand (plaintext.message, enc->remNode);
+            }
+            break;
+
+        case TXT_TYPE_SIGNED_PLAIN: {
+            uint8_t senderPubKeyPrefix[4];
+            memcpy (senderPubKeyPrefix, plaintext.message, sizeof (senderPubKeyPrefix));
+            NodeEntry *senderNode = getNodePrefix (senderPubKeyPrefix);
+            iprintf ("Plaintext message from server %s, sender is %s, attempt %d, timestamp %d: %s", enc->remNode->name, senderNode->name, plaintext.attempt, plaintext.timestamp, &(plaintext.message[4]));
+            break;
+        }
+
+        default:
+            MESH_LOGW (TAG, "Unknown text type: %d", plaintext.textType);
+            break;
+        }
+    }
+}
+
+// #define STR_EQ_LIT(s, lit) (memcmp ((s), (lit), sizeof (lit) - 1) == 0)
+#define STR_EQ_LIT(s, lit) (strcmp (s, lit) == 0)
+
+static int32_t parse_coord_micro(const char *s)
+{
+    int32_t sign = 1;
+    int32_t int_part = 0;
+    int32_t frac_part = 0;
+    int32_t frac_div = 1;
+
+    if (*s == '-') {
+        sign = -1;
+        s++;
+    }
+
+    // integer part
+    while (*s >= '0' && *s <= '9') {
+        int_part = int_part * 10 + (*s - '0');
+        s++;
+    }
+
+    if (*s == '.') {
+        s++;
+        while (*s >= '0' && *s <= '9' && frac_div < 1000000) {
+            frac_part = frac_part * 10 + (*s - '0');
+            frac_div *= 10;
+            s++;
+        }
+    }
+
+    // scale to microdegrees
+    while (frac_div < 1000000) {
+        frac_part *= 10;
+        frac_div *= 10;
+    }
+
+    return sign * (int_part * 1000000 + frac_part);
+}
+
+void processCommand (char *cmd, NodeEntry *remNode) {
+    PlainTextMessagePayload replyPayload;
+    replyPayload.timestamp = RTC_GetCounter();
+    replyPayload.attempt = 0;
+    replyPayload.textType = TXT_TYPE_CLI_DATA;
+
+    uint8_t *reply = replyPayload.message;
+    reply[0] = 0;
+
+    while (*cmd == ' ') cmd++;  // skip leading spaces
+
+    // Optional CLI prefix (xx|) for companion radio
+    if (strlen (cmd) > 4 && cmd[2] == '|') {
+        memcpy (reply, cmd, 3);
+        reply += 3;
+        cmd += 3;
+    }
+
+    /* ---------------- System ---------------- */
+    if (STR_EQ_LIT (cmd, "reboot")) {
+        NVIC_SystemReset();
+    } else if (STR_EQ_LIT (cmd, "advert")) {
+        sendAdvert (1);  // 1500ms delay in reference
+        strcpy ((char *)reply, "OK - Advert sent");
+    } else if (STR_EQ_LIT (cmd, "clear stats")) {
+        memset (&stats, 0, sizeof (stats));
+        strcpy ((char *)reply, "(OK - stats reset)");
+    }
+
+    else if (STR_EQ_LIT (cmd, "ver")) {
+        sprintf ((char *)reply, "%s (Build: %s)", VERSION, __DATE__);
+    }
+
+    else if (STR_EQ_LIT (cmd, "board")) {
+        sprintf ((char *)reply, "%s", BOARD);
+    }
+
+    /* ---------------- Clock ---------------- */
+    else if (STR_EQ_LIT (cmd, "clock")) {
+        RTC_Get();
+        sprintf ((char *)reply, "%02d:%02d:%02d - %d/%d/%d UTC",
+                 calendar.hour, calendar.min, calendar.sec,
+                 calendar.w_date, calendar.w_month, calendar.w_year);
+    } else if (STR_EQ_LIT (cmd, "time ")) {
+        uint32_t secs = atoi (&cmd[5]);
+        uint32_t curr = RTC_GetCounter();
+        if (secs > curr) {
+            RTC_SetCounter (secs);
+            RTC_Get();
+            sprintf ((char *)reply, "OK - clock set: %02d:%02d:%02d - %d/%d/%d UTC",
+                     calendar.hour, calendar.min, calendar.sec,
+                     calendar.w_date, calendar.w_month, calendar.w_year);
+        } else {
+            strcpy ((char *)reply, "(ERR: clock cannot go backwards)");
+        }
+    } else if (STR_EQ_LIT (cmd, "clock sync")) {
+        uint32_t curr = RTC_GetCounter();
+        uint32_t sender = replyPayload.timestamp;
+        if (sender > curr) {
+            RTC_SetCounter (sender + 1);
+            RTC_Get();
+            sprintf ((char *)reply, "OK - clock set: %02d:%02d:%02d - %d/%d/%d UTC",
+                     calendar.hour, calendar.min, calendar.sec,
+                     calendar.w_date, calendar.w_month, calendar.w_year);
+        } else {
+            strcpy ((char *)reply, "ERR: clock cannot go backwards");
+        }
+    }
+
+    /*
+    else if (STR_EQ_LIT (cmd, "tempradio ")) {
+        char tmp[64];
+        strcpy (tmp, &cmd[10]);
+
+        const char *parts[5];
+        int num = mesh_ParseTextParts (tmp, parts, 5);  // assume helper
+        float freq = num > 0 ? strtof (parts[0], NULL) : 0.0f;
+        float bw = num > 1 ? strtof (parts[1], NULL) : 0.0f;
+        uint8_t sf = num > 2 ? atoi (parts[2]) : 0;
+        uint8_t cr = num > 3 ? atoi (parts[3]) : 0;
+        int timeout_mins = num > 4 ? atoi (parts[4]) : 0;
+        if (freq >= 300.0f && freq <= 2500.0f && bw >= 7.0f && bw <= 500.0f &&
+            sf >= 5 && sf <= 12 && cr >= 5 && cr <= 8 && timeout_mins > 0) {
+            Callbacks_ApplyTempRadioParams (freq, bw, sf, cr, timeout_mins);
+
+            LoraApply();
+            sprintf ((char *)reply, "OK - temp params for %d mins", timeout_mins);
+        } else {
+            strcpy ((char *)reply, "Error, invalid params");
+        }
+    }
+    */
+
+    /* ---------------- Password ---------------- */
+    else if (STR_EQ_LIT (cmd, "password ")) {
+        strncpy (persistent.password, &cmd[9], sizeof (persistent.password));
+        // savePrefs(); TODO add this
+        sprintf ((char *)reply, "password now: %s", persistent.password);
+    }
+
+    /* ---------------- GET / SET Config ---------------- */
+    else if (STR_EQ_LIT (cmd, "get ")) {
+        const char *config = &cmd[4];
+
+/*
+        if (memcmp (config, "af", 2) == 0) {
+            sprintf (reply, "> %f", persistent.airtimeFactor);
+            */
+            /*
+        } else if (memcmp (config, "int.thresh", 10) == 0) {
+            sprintf (reply, "> %d", (uint32_t)_prefs->interference_threshold);
+        } else if (memcmp (config, "agc.reset.interval", 18) == 0) {
+            sprintf (reply, "> %d", ((uint32_t)_prefs->agc_reset_interval) * 4);
+        } else if (memcmp (config, "multi.acks", 10) == 0) {
+            sprintf (reply, "> %d", (uint32_t)_prefs->multi_acks);
+        } else if (memcmp (config, "allow.read.only", 15) == 0) {
+            sprintf (reply, "> %s", _prefs->allow_read_only ? "on" : "off");
+        } else if (memcmp (config, "flood.advert.interval", 21) == 0) {
+            sprintf (reply, "> %d", (uint32_t)_prefs->flood_advert_interval);
+        } else if (memcmp (config, "advert.interval", 15) == 0) {
+            sprintf (reply, "> %d", ((uint32_t)_prefs->advert_interval) * 2);1
+            */
+        //} else 
+        if (memcmp (config, "guest.password", 14) == 0) {
+            sprintf (reply, "> %s", persistent.guestPassword);
+        } else if (memcmp (config, "name", 4) == 0) {
+            sprintf (reply, "> %s", persistent.nodeName);
+        } else if (memcmp (config, "repeat", 6) == 0) {
+            sprintf (reply, "> %s", persistent.doRepeat ? "on" : "off");
+        } else if (memcmp (config, "lat", 3) == 0) {
+            sprintf (reply, "> %d", persistent.latitude);
+        } else if (memcmp (config, "lon", 3) == 0) {
+            sprintf (reply, "> %d", persistent.longitude);
+            /*
+        } else if (memcmp (config, "radio", 5) == 0) {
+            char freq[16], bw[16];
+            snprintf(freq, sizeof(freq), "%lf", persistent.frequencyInHz / 1000000.0);
+            snprintf(bw, sizeof(bw), "%lf", loraBwToFloat(persistent.bandwidth));
+            sprintf (reply, "> %s,%s,%d,%d", freq, bw, persistent.spreadingFactor, persistent.codingRate + 4);
+        } else if (memcmp (config, "rxdelay", 7) == 0) {
+            sprintf (reply, "> %s", StrHelper::ftoa (_prefs->rx_delay_base));
+        } else if (memcmp (config, "txdelay", 7) == 0) {
+            sprintf (reply, "> %s", StrHelper::ftoa (_prefs->tx_delay_factor));
+        } else if (memcmp (config, "flood.max", 9) == 0) {
+            sprintf (reply, "> %d", (uint32_t)_prefs->flood_max);
+        } else if (memcmp (config, "direct.txdelay", 14) == 0) {
+            sprintf (reply, "> %s", StrHelper::ftoa (_prefs->direct_tx_delay_factor));
+        } else if (memcmp (config, "tx", 2) == 0 && (config[2] == 0 || config[2] == ' ')) {
+            sprintf (reply, "> %d", (uint32_t)_prefs->tx_power_dbm);
+        } else if (memcmp (config, "freq", 4) == 0) {
+            sprintf (reply, "> %s", StrHelper::ftoa (_prefs->freq));
+            */
+        } else if (memcmp (config, "public.key", 10) == 0) {
+            strcpy (reply, "> ");
+            hexdump_compact (persistent.pubkey, sizeof (persistent.pubkey), &(reply[2]), 70);
+        } else if (memcmp (config, "role", 4) == 0) {
+            sprintf (reply, "> %s", getStringRole (persistent.nodeType));
+        } else if (memcmp (config, "adc.multiplier", 14) == 0) {
+            sprintf (reply, "> %d.%d", persistent.adcMultiplier / 1000, persistent.adcMultiplier % 1000);
+        } else {
+            sprintf (reply, "??: %s", config);
+        }
+
+    } else if (STR_EQ_LIT (cmd, "set ")) {
+        const char *config = &cmd[4];
+
+
+/*
+        if (memcmp (config, "af ", 3) == 0) {
+            persistent.airtimeFactor = atof (&config[3]);
+            // savePrefs();
+            strcpy (reply, "OK");
+            */
+            /*
+        } else if (memcmp (config, "int.thresh ", 11) == 0) {
+            _prefs->interference_threshold = atoi (&config[11]);
+            // savePrefs();
+            strcpy (reply, "OK");
+        } else if (memcmp (config, "agc.reset.interval ", 19) == 0) {
+            _prefs->agc_reset_interval = atoi (&config[19]) / 4;
+            // savePrefs();
+            sprintf (reply, "OK - interval rounded to %d", ((uint32_t)_prefs->agc_reset_interval) * 4);
+        } else if (memcmp (config, "multi.acks ", 11) == 0) {
+            _prefs->multi_acks = atoi (&config[11]);
+            // savePrefs();
+            strcpy (reply, "OK");
+            */
+        //} else 
+        if (memcmp (config, "allow.read.only ", 16) == 0) {
+            if (memcmp (&config[16], "on", 2) == 0) {
+                persistent.allowReadOnly = 1;
+                strcpy (reply, "OK");
+            } else if (memcmp (&config[16], "off", 3) == 0) {
+                persistent.allowReadOnly = 0;
+                strcpy (reply, "OK");
+            }
+            // savePrefs();
+            /*
+        } else if (memcmp (config, "flood.advert.interval ", 22) == 0) {
+            int hours = _atoi (&config[22]);
+            if ((hours > 0 && hours < 3) || (hours > 48)) {
+                strcpy (reply, "Error: interval range is 3-48 hours");
+            } else {
+                _prefs->flood_advert_interval = (uint8_t)hours;
+                _callbacks->updateFloodAdvertTimer();
+                savePrefs();
+                strcpy (reply, "OK");
+            }
+        } else if (memcmp (config, "advert.interval ", 16) == 0) {
+            int mins = _atoi (&config[16]);
+            if ((mins > 0 && mins < MIN_LOCAL_ADVERT_INTERVAL) || (mins > 240)) {
+                sprintf (reply, "Error: interval range is %d-240 minutes", MIN_LOCAL_ADVERT_INTERVAL);
+            } else {
+                _prefs->advert_interval = (uint8_t)(mins / 2);
+                _callbacks->updateAdvertTimer();
+                savePrefs();
+                strcpy (reply, "OK");
+            }
+            */
+        } else if (memcmp (config, "guest.password ", 15) == 0) {
+            strncpy (persistent.guestPassword, &config[15], sizeof (persistent.guestPassword));
+            // savePrefs();
+            strcpy (reply, "OK");
+        } else if (memcmp (config, "name ", 5) == 0) {
+            strncpy (persistent.nodeName, &config[5], sizeof (persistent.nodeName));
+            // savePrefs();
+            strcpy (reply, "OK");
+        } else if (memcmp (config, "repeat ", 7) == 0) {
+            if (memcmp (&config[7], "off", 3) == 0) {
+                persistent.doRepeat = 0;
+            } else if (memcmp (&config[7], "on", 2) == 0) {
+                persistent.doRepeat = 1;
+            }
+            // savePrefs();
+            strcpy (reply, persistent.doRepeat ? "OK - repeat is now ON" : "OK - repeat is now OFF");
+            /*
+        } else if (memcmp (config, "radio ", 6) == 0) {
+            strcpy (tmp, &config[6]);
+            const char *parts[4];
+            int num = mesh::Utils::parseTextParts (tmp, parts, 4);
+            float freq = num > 0 ? strtof (parts[0], 0) : 0.0f;
+            float bw = num > 1 ? strtof (parts[1], 0) : 0.0f;
+            uint8_t sf = num > 2 ? atoi (parts[2]) : 0;
+            uint8_t cr = num > 3 ? atoi (parts[3]) : 0;
+            if (freq >= 300.0f && freq <= 2500.0f && sf >= 5 && sf <= 12 && cr >= 5 && cr <= 8 && bw >= 7.0f && bw <= 500.0f) {
+                _prefs->sf = sf;
+                _prefs->cr = cr;
+                _prefs->freq = freq;
+                _prefs->bw = bw;
+                _callbacks->savePrefs();
+                strcpy (reply, "OK - reboot to apply");
+            } else {
+                strcpy (reply, "Error, invalid radio params");
+            }
+            */
+        } else if (memcmp (config, "lat ", 4) == 0) {
+            persistent.latitude = parse_coord_micro(&config[4]);
+            // savePrefs();
+            strcpy (reply, "OK");
+        } else if (memcmp (config, "lon ", 4) == 0) {
+            persistent.longitude = parse_coord_micro(&config[4]);
+            // savePrefs();
+            strcpy (reply, "OK");
+            /*
+        } else if (memcmp (config, "rxdelay ", 8) == 0) {
+            float db = atof (&config[8]);
+            if (db >= 0) {
+                _prefs->rx_delay_base = db;
+                savePrefs();
+                strcpy (reply, "OK");
+            } else {
+                strcpy (reply, "Error, cannot be negative");
+            }
+        } else if (memcmp (config, "txdelay ", 8) == 0) {
+            float f = atof (&config[8]);
+            if (f >= 0) {
+                _prefs->tx_delay_factor = f;
+                savePrefs();
+                strcpy (reply, "OK");
+            } else {
+                strcpy (reply, "Error, cannot be negative");
+            }
+        } else if (memcmp (config, "flood.max ", 10) == 0) {
+            uint8_t m = atoi (&config[10]);
+            if (m <= 64) {
+                _prefs->flood_max = m;
+                savePrefs();
+                strcpy (reply, "OK");
+            } else {
+                strcpy (reply, "Error, max 64");
+            }
+        } else if (memcmp (config, "direct.txdelay ", 15) == 0) {
+            float f = atof (&config[15]);
+            if (f >= 0) {
+                _prefs->direct_tx_delay_factor = f;
+                savePrefs();
+                strcpy (reply, "OK");
+            } else {
+                strcpy (reply, "Error, cannot be negative");
+            }
+        } else if (memcmp (config, "tx ", 3) == 0) {
+            _prefs->tx_power_dbm = atoi (&config[3]);
+            savePrefs();
+            _callbacks->setTxPower (_prefs->tx_power_dbm);
+            strcpy (reply, "OK");
+        */
+        /*
+        } else if (memcmp (config, "freq ", 5) == 0) {
+            double freq = atof (&config[5]);
+            uint32_t newFreq = mhzToHzLimited (persistent.loraSettings.frequencyInHz);
+            if (newFreq != 0) {
+                persistent.loraSettings.frequencyInHz = newFreq;
+            }
+            // savePrefs();
+            strcpy (reply, "OK - reboot to apply");
+        */
+        /*
+        } else if (memcmp (config, "adc.multiplier ", 15) == 0) {
+            persistent.adcMultiplier = atof (&config[15]);
+            if (persistent.adcMultiplier == 0.0f) {
+                strcpy (reply, "OK - using default board multiplier");
+            } else {
+                sprintf (reply, "OK - multiplier set to %.3f", persistent.adcMultiplier);
+            }
+            // savePrefs();
+            */
+        } else {
+            sprintf (reply, "unknown config: %s", config);
+        }
+    }
+
+
+    /* ---------------- Stats ---------------- */
+    else if (STR_EQ_LIT (cmd, "stats-packets")) {
+        sprintf (reply,
+                 "{\"recv\":%u,\"sent\":%u,\"flood_tx\":%u,\"direct_tx\":%u,\"flood_rx\":%u,\"direct_rx\":%u}",
+                 stats.packetsReceivedCount,
+                 stats.packetsSentCount,
+                 stats.sentFloodCount,
+                 stats.sentDirectCount,
+                 stats.receivedFloodCount,
+                 stats.receivedDirectCount);
+    } else if (STR_EQ_LIT (cmd, "stats-radio")) {
+        sprintf (reply,
+                 "{\"noise_floor\":%d,\"last_rssi\":%d,\"last_snr\":%d.00,\"tx_air_secs\":%u,\"rx_air_secs\":%u}",
+                 stats.noiseFloor,
+                 stats.lastRSSI,
+                 stats.lastSNR / 4,
+                 stats.totalAirTimeSeconds,
+                 stats.total_rx_air_time_secs);
+    } else if (STR_EQ_LIT (cmd, "stats-core")) {
+        stats.totalUpTimeSeconds = RTC_GetCounter() - startupTime;
+        //stats.totalAirTimeSeconds = TICKS_TO_MS (tickAirtime / 1000);
+        stats.totalAirTimeSeconds = 0;
+        sprintf (reply,
+                 "{\"battery_mv\":%u,\"uptime_secs\":%u,\"errors\":%u,\"queue_len\":%u}",
+                 getVoltage(),
+                 stats.totalUpTimeSeconds,
+                 stats.err_events,
+                 stats.txQueueLength);
+    }
+
+    /* ---------------- Unknown ---------------- */
+    else {
+        strcpy ((char *)reply, "Unknown command");
+    }
+
+    sendEncryptedTextMessage (remNode, &replyPayload);
+}

User/meshcore/packets/encrypted.h

@@ -0,0 +1,30 @@
+#ifndef ENCRYPTED_HEADER
+#define ENCRYPTED_HEADER
+
+#include "lib/config.h"
+#include "meshcore/meshframing.h"
+#include "meshcore/packetstructs.h"
+#include "util/log.h"
+#include <string.h>
+
+#define MIN_LOCAL_ADVERT_INTERVAL   60
+
+void sendEncryptedFrame (const NodeEntry *targetNode, uint8_t payloadType, const uint8_t *plain, size_t plainLen);
+
+void sendEncryptedTextMessage (const NodeEntry *targetNode, const PlainTextMessagePayload *msg);
+
+void sendEncryptedResponse (const NodeEntry *targetNode, const Response *resp);
+
+void sendEncryptedRequest (const NodeEntry *targetNode, const Request *req);
+
+void sendEncryptedPathPayload (const NodeEntry *targetNode, const ReturnedPathPayload *path);
+
+void decodeEncryptedPayload (const FrameStruct *frame);
+
+void parseEncryptedPayload (const EncryptedPayloadStruct *enc);
+
+void processCommand(char * cmd, NodeEntry * remNode);
+
+void sendPathBack (const NodeEntry *node, const Path *path);
+
+#endif

User/meshcore/packets/group.c

@@ -0,0 +1,107 @@
+#include "ch32v20x_rtc.h"
+#include "meshcore/meshframing.h"
+#include "meshcore/packetstructs.h"
+#include "group.h"
+#include "lib/config.h"
+#include "util/hexdump.h"
+#include "util/log.h"
+#include "string.h"
+
+#define TAG "GroupMessage"
+
+void sendGroupMessage (const GroupTextMessage *msg) {
+    // Prepare values locally instead of modifying msg
+    Channel *channel = &(persistent.channels[msg->keyIndex]);
+    uint8_t flags = 0;
+    int32_t timestamp = RTC_GetCounter();
+
+    FrameStruct frame;
+    frame.header = ROUTE_TYPE_FLOOD | PAYLOAD_TYPE_GRP_TXT | PAYLOAD_VERSION_0;
+    frame.path.pathLen = 0;
+    size_t offset = 0;
+    frame.payload[offset++] = channel->hash;
+
+    // Build encryption buffer directly on stack (no extra large buffer)
+    uint8_t buf[180];  // enough for timestamp + flags + text
+    size_t buf_offset = 0;
+
+    memcpy (buf + buf_offset, &timestamp, sizeof (timestamp));
+    buf_offset += sizeof (timestamp);
+
+    buf[buf_offset++] = flags;
+
+    size_t textLen = strlen ((const char *)msg->text);
+    if (buf_offset + textLen > sizeof (buf)) {
+        textLen = sizeof (buf) - buf_offset;
+    }
+    memcpy (buf + buf_offset, msg->text, textLen);
+    buf_offset += textLen;
+
+    hexdump ("TxDumpDec", buf, buf_offset);
+
+    // Encrypt and MAC directly into frame payload after channelHash
+    size_t olen = 0;
+
+    encrypt_then_mac (channel->key, 16, buf, buf_offset, &frame.payload[offset], &olen);
+
+    frame.payloadLen = olen + 1;  // +1 for channelHash
+
+    channel->timestamp = timestamp;
+
+    LoRaTransmit (&frame);
+}
+
+void makeSendGroupMessage (char *txt, uint8_t keyIndex) {
+    GroupTextMessage msg;
+    strcpy ((char *)msg.text, persistent.nodeName);
+    strcat ((char *)msg.text, ": ");
+    strcat ((char *)msg.text, txt);
+    msg.keyIndex = keyIndex;
+    sendGroupMessage (&msg);
+    return;
+}
+
+void decodeGroupMessage (const FrameStruct *frame) {
+    GroupTextMessage msg;
+    memset (&msg, 0, sizeof (msg));
+    if ((frame->header & PAYLOAD_TYPE_MASK) != PAYLOAD_TYPE_GRP_TXT) {
+        MESH_LOGW (TAG, "Not a group text");
+        return;
+    }
+    unsigned char index = 0;
+    msg.channelHash = frame->payload[index++];
+    unsigned char tmp[184];
+
+
+    Channel *channel = NULL;
+
+    uint8_t i = 0;
+    do {
+        channel = getChannel (msg.channelHash, i);
+        if (channel == NULL) {
+            MESH_LOGW (TAG, "Channel hash %d not found", msg.channelHash);
+            return;
+        }
+        if (mac_then_decrypt (channel->key, 16, frame->payload + index, frame->payloadLen - index, tmp) != 0) {
+            MESH_LOGW (TAG, "HMAC failed on grouphash key %d", msg.channelHash);
+            i++;
+            continue;
+        } else {
+            unsigned char plaintextLen = frame->payloadLen - index;
+            index = 0;
+
+            memcpy (&msg.timestamp, tmp + index, 4);
+            index += 4;
+            msg.flags = tmp[index++];
+
+            memcpy (msg.text, tmp + index, plaintextLen - index);
+
+            channel->timestamp = RTC_GetCounter();
+
+            iprintf ("Message from channel %s: %s\n", channel->name, msg.text);
+            break;
+        }
+
+
+    } while (channel != NULL);
+}

User/meshcore/packets/group.h

@@ -0,0 +1,13 @@
+#ifndef GROUP_HEADER
+#define GROUP_HEADER
+
+#include "stdint.h"
+#include "meshcore/packetstructs.h"
+
+void sendGroupMessage (const GroupTextMessage *msg);
+
+void makeSendGroupMessage (char *txt, uint8_t keyIndex);
+
+void decodeGroupMessage (const FrameStruct *frame);
+
+#endif

User/meshcore/packets/multipart.c

@@ -0,0 +1,4 @@
+#include "meshcore/packetstructs.h"
+#include "multipart.h"
+
+#define TAG "Multipart"

User/meshcore/packets/multipart.h

@@ -0,0 +1,4 @@
+#ifndef MULTIPART_HEADER
+#define MULTIPART_HEADER
+
+#endif

User/meshcore/packets/trace.c

@@ -0,0 +1,4 @@
+#include "meshcore/packetstructs.h"
+#include "trace.h"
+
+#define TAG "Trace"

User/meshcore/packets/trace.h

@@ -0,0 +1,5 @@
+#ifndef TRACE_HEADER
+#define TRACE_HEADER
+
+
+#endif