still wip

User/meshcore/packets/ack.c

@@ -0,0 +1,42 @@
+#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;
+    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;
+
+
+    sendFrame (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,192 @@
+#include "ch32v30x_rtc.h"
+#include "lib/config.h"
+#include "lib/ed25519/ed_25519.h"
+#include "meshcore/meshframing.h"
+#include "meshcore/packetstructs.h"
+#include <string.h>
+#include "advert.h"
+#include "util/hexdump.h"
+#include "lib/base64.h"
+
+#define TAG "Advert"
+
+void sendAdvert() {
+    AdvertisementPayload ad;
+    memcpy (ad.pubKey, persistent.pubkey, sizeof (ad.pubKey));
+    ad.dataFlags = ADVERTISEMENT_FLAG_HAS_NAME;
+    if (persistent.nodeType == NODE_TYPE_CHAT_NODE) {
+        ad.dataFlags |= ADVERTISEMENT_FLAG_IS_CHAT_NODE;
+    } else if (persistent.nodeType == NODE_TYPE_REPEATER) {
+        ad.dataFlags |= ADVERTISEMENT_FLAG_IS_REAPEATER;
+    } else if (persistent.nodeType == NODE_TYPE_ROOM_SERVER) {
+        ad.dataFlags |= ADVERTISEMENT_FLAG_IS_ROOM_SERVER;
+    } else if (persistent.nodeType == NODE_TYPE_SENSOR) {
+        ad.dataFlags |= ADVERTISEMENT_FLAG_IS_SENSOR;
+    }
+    strcpy (ad.nodeName, persistent.nodeName);
+    ad.timestamp = RTC_GetCounter();
+
+    uint8_t app_data[40];
+    size_t app_len = 0;
+
+    app_data[app_len++] = ad.dataFlags;
+
+    if (ad.dataFlags & ADVERTISEMENT_FLAG_HAS_LOCATION) {
+        memcpy (app_data + app_len, &ad.latitude, sizeof (ad.latitude));
+        app_len += sizeof (ad.latitude);
+        memcpy (app_data + app_len, &ad.longitude, sizeof (ad.longitude));
+        app_len += sizeof (ad.longitude);
+    }
+
+    if (ad.dataFlags & ADVERTISEMENT_FLAG_RFU1) {
+        memcpy (app_data + app_len, &ad.rfu1, sizeof (ad.rfu1));
+        app_len += sizeof (ad.rfu1);
+    }
+
+    if (ad.dataFlags & ADVERTISEMENT_FLAG_RFU2) {
+        memcpy (app_data + app_len, &ad.rfu2, sizeof (ad.rfu2));
+        app_len += sizeof (ad.rfu2);
+    }
+
+    if (ad.dataFlags & ADVERTISEMENT_FLAG_HAS_NAME) {
+        size_t nodenameLen = strlen (ad.nodeName);
+        memcpy (app_data + app_len, ad.nodeName, nodenameLen);
+        app_len += nodenameLen;
+    }
+
+    // 2. Reserve frame and build payload header
+    FrameStruct frame;
+    frame.header = ROUTE_TYPE_FLOOD | PAYLOAD_TYPE_ADVERT | PAYLOAD_VERSION_0;
+
+    size_t offset = 0;
+    memcpy (frame.payload + offset, ad.pubKey, sizeof (ad.pubKey));
+    offset += sizeof (ad.pubKey);
+
+    memcpy (frame.payload + offset, &ad.timestamp, sizeof (ad.timestamp));
+    offset += sizeof (ad.timestamp);
+
+    // reserve signature space
+    uint8_t *signature_pos = frame.payload + offset;
+    offset += 64;
+
+    // append app_data after signature
+    memcpy (frame.payload + offset, app_data, app_len);
+    offset += app_len;
+
+    // 3. Sign pubKey + timestamp + app_data
+    uint8_t message[76];
+    size_t msg_len = 0;
+    memcpy (message + msg_len, ad.pubKey, sizeof (ad.pubKey));
+    msg_len += sizeof (ad.pubKey);
+    memcpy (message + msg_len, &ad.timestamp, sizeof (ad.timestamp));
+    msg_len += sizeof (ad.timestamp);
+    memcpy (message + msg_len, app_data, app_len);
+    msg_len += app_len;
+
+    ed25519_sign (signature_pos, message, msg_len, persistent.pubkey, persistent.privkey);
+
+    hexdump ("Complete advert", frame.payload, offset);
+
+    hexdump ("Public key", ad.pubKey, 32);
+
+    hexdump ("Signature", signature_pos, 64);
+
+    printf ("Timestamp is %d\n", ad.timestamp);
+
+    printf ("NodeName %s\n", ad.nodeName);
+
+    hexdump ("Appdata", app_data, app_len);
+
+    // 5. Set payload length and send
+    frame.payloadLen = offset;
+    frame.path.pathLen = 0;
+    sendFrame (frame);
+}
+
+AdvertisementPayload decodeAdvertisement (FrameStruct frame) {
+    AdvertisementPayload advert;
+    memset (&advert, 0, sizeof (advert));
+
+    if ((frame.header & PAYLOAD_TYPE_MASK) != PAYLOAD_TYPE_ADVERT) {
+        return advert;
+    }
+
+    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++];
+
+    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 > 32) {
+        nameLen = 32;
+    }
+    memcpy (advert.nodeName, frame.payload + index, nameLen);
+    advert.nodeName[frame.payloadLen - index] = 0;
+
+
+    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_key_exchange ((unsigned char *)node->secret, advert.pubKey, persistent.privkey);
+    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;
+
+
+    return advert;
+}
+
+void printAdvertisement (AdvertisementPayload advert) {
+    unsigned char keyBuf[50];
+    unsigned char sigBuf[90];
+    memset (keyBuf, 0, sizeof (keyBuf));
+    memset (sigBuf, 0, sizeof (sigBuf));
+    base64_encode (advert.pubKey, 32, keyBuf);
+    base64_encode (advert.signature, 64, sigBuf);
+
+    printf ("%s on %ld with type %s on %s location %ld %ld, public key %s and "
+            "signature %s\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, keyBuf, sigBuf);
+}

User/meshcore/packets/advert.h

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

User/meshcore/packets/anonymous.c

@@ -0,0 +1,133 @@
+#include "ch32v30x_rtc.h"
+#include "lib/config.h"
+#include "lib/ed25519/ed_25519.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 (NodeEntry *targetNode, const uint8_t *password, uint32_t sync) {
+    uint8_t passwordLen = strlen ((const char *)password);
+    FrameStruct frame;
+    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);
+
+    sendFrame (frame);
+}
+
+AnonymousRequestPayload decodeAnonReq (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();
+
+        strcpy (foundNode->name, "Anonymous node");
+        memcpy (foundNode->pubKey, anonReq.pubKey, sizeof (foundNode->pubKey));
+        ed25519_key_exchange ((unsigned char *)foundNode->secret, anonReq.pubKey, persistent.privkey);
+        foundNode->gps_latitude = 0;
+        foundNode->gps_longitude = 0;
+        // ADD PATH
+        foundNode->type = 0;
+        foundNode->last_seen_lt = RTC_GetCounter();
+    }
+    mac_then_decrypt (foundNode->secret, 32, &(frame.payload[index]), frame.payloadLen - index, anonReq.payload);
+    anonReq.payloadLen = frame.payloadLen - index - 2;
+    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;
+    }
+
+    uint8_t passwordLen = anonReq.payloadLen - index2;
+    if (passwordLen > 16) {
+        passwordLen = 16;
+    }
+    uint8_t passwordBuf[16];
+    memcpy (passwordBuf, &(anonReq.payload[index2]), passwordLen);
+    if (memcmp (passwordBuf, persistent.password, sizeof (persistent.password)) == 0) {
+        foundNode->authenticated = 1;
+        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++] = 1;//isadmin
+        resp.data[index3++] = PERM_ACL_ADMIN;//permissions
+        resp.data[index3++] = randOut & 0xFF;//rng
+        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;
+        sendEncryptedResponse(foundNode, &resp);
+    }
+
+    return anonReq;
+}

User/meshcore/packets/anonymous.h

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

User/meshcore/packets/control.c

@@ -0,0 +1,76 @@
+#include "meshcore/meshframing.h"
+#include "meshcore/packetstructs.h"
+#include "control.h"
+#include "string.h"
+
+#define TAG "Control"
+
+void sendDiscoverRequest(const DiscoverRequestPayload *discReq) {
+
+    FrameStruct frame;
+    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;
+
+    sendFrame(frame);
+}
+
+
+void decodeControlFrame(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;
+            }
+
+        } 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;
+            memcpy(discResp.pubkey, &(frame.payload[index]), pubKeyLen);
+            index += pubKeyLen;
+
+        }
+}

User/meshcore/packets/control.h

@@ -0,0 +1,8 @@
+#ifndef CONTROL_HEADER
+#define CONTROL_HEADER
+
+#include "meshcore/packetstructs.h"
+
+void decodeControlFrame(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,6 @@
+#ifndef CUSTOM_HEADER
+#define CUSTOM_HEADER
+
+
+
+#endif

User/meshcore/packets/encrypted.c

@@ -0,0 +1,309 @@
+#include "lib/config.h"
+#include "meshcore/meshframing.h"
+#include "meshcore/packets/ack.h"
+#include "meshcore/packetstructs.h"
+#include "meshcore/stats.h"
+#include "util/hexdump.h"
+#include "util/log.h"
+#include <stdio.h>
+#include <string.h>
+#include "encrypted.h"
+#include "FreeRTOS.h"
+#include "task.h"
+
+
+#define TAG "EncryptedMessage"
+
+void sendEncryptedFrame (NodeEntry *targetNode, uint8_t payloadType, const uint8_t *plain, size_t plainLen) {
+    FrameStruct frame;
+    uint8_t offset = 0;
+
+    // 1. Header
+    frame.header =
+        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);
+
+    offset += encLen;
+
+    // 5. Finalize
+    frame.payloadLen = offset;
+    memcpy (&frame.path, &targetNode->path, sizeof (frame.path));
+
+    hexdump ("Encrypted frame", frame.payload, frame.payloadLen);
+    sendFrame (frame);
+}
+
+void sendEncryptedTextMessage (NodeEntry *targetNode, const PlainTextMessagePayload *msg) {
+    if (targetNode == NULL) {
+        ESP_LOGW(TAG, "Node is null");
+        return;
+    }
+    if (targetNode->last_seen_lt == 0) {
+        ESP_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);
+
+    sendEncryptedFrame (
+        targetNode,
+        PAYLOAD_TYPE_TXT_MSG,
+        buf,
+        index + msgLen);
+}
+
+void sendEncryptedResponse (NodeEntry *targetNode, const Response *resp) {
+    uint8_t buf[256];
+    uint8_t index = 0;
+
+    buf[index++] = resp->tag;
+    buf[index++] = resp->tag >> 8;
+    buf[index++] = resp->tag >> 16;
+    buf[index++] = resp->tag >> 24;
+
+    memcpy (&buf[index], resp->data, resp->dataLen);
+
+    sendEncryptedFrame (
+        targetNode,
+        PAYLOAD_TYPE_RESPONSE,
+        buf,
+        index);
+}
+
+void sendEncryptedRequest (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);
+
+    sendEncryptedFrame (
+        targetNode,
+        PAYLOAD_TYPE_REQ,
+        buf,
+        index);
+}
+
+void sendEncryptedPathPayload (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;
+
+    buf[index++] = path->extra.type;
+    memcpy (&buf[index], path->extra.data,
+            sizeof (path->extra.data));
+
+    sendEncryptedFrame (
+        targetNode,
+        PAYLOAD_TYPE_PATH,
+        buf,
+        index);
+}
+
+void printRequest (const Request *req) {
+    printf ("Request:\n");
+    printf ("  Timestamp: %u\n", req->timestamp);
+    printf ("  Type: 0x%02X\n", req->requestType);
+    printf ("  Data: ");
+    for (int i = 0; i < req->dataLen; i++) {
+        printf ("%02X ", req->data[i]);
+    }
+    printf ("\n");
+}
+
+void printResponse (const Response *resp) {
+    printf ("Response:\n");
+    printf ("  Tag: %u\n", resp->tag);
+    printf ("  Data: ");
+    for (int i = 0; i < resp->dataLen; i++) {
+        printf ("%02X ", resp->data[i]);
+    }
+    printf ("\n");
+}
+
+void printPlainTextMessage (const PlainTextMessagePayload *msg) {
+    printf ("PlainTextMessage:\n");
+    printf ("  Timestamp: %u\n", msg->timestamp);
+    printf ("  Attempt: %u\n", msg->attempt);
+    printf ("  TextType: %u\n", msg->textType);
+    printf ("  Message: %.*s\n", (int)strlen (msg->message), msg->message);
+}
+
+void printReturnedPathPayload (const ReturnedPathPayload *path) {
+    printf ("ReturnedPathPayload:\n");
+    printf ("  Path Length: %u\n", path->path.pathLen);
+    printf ("  Path: ");
+    for (int i = 0; i < path->path.pathLen; i++) {
+        printf ("%02X ", path->path.path[i]);
+    }
+    printf ("\n");
+    printf ("  Extra Type: %u\n", path->extra.type);
+    printf ("  Extra Data: ");
+    for (int i = 0; i < sizeof (path->extra.data); i++) {
+        printf ("%02X ", path->extra.data[i]);
+    }
+    printf ("\n");
+}
+
+void printEncryptedPayload (const EncryptedPayloadStruct *enc) {
+    printf ("EncryptedPayload:\n");
+    printf ("  Type: 0x%02X\n", enc->type);
+    printf ("  DestinationHash: 0x%02X\n", enc->destinationHash);
+    printf ("  SourceHash: 0x%02X\n", enc->sourceHash);
+    printf ("  CipherMAC: 0x%04X\n", enc->cipherMAC);
+    printf ("  PayloadLen: %zu\n", enc->payloadLen);
+    printf ("  Payload: ");
+    for (size_t i = 0; i < enc->payloadLen; i++) {
+        printf ("%02X ", enc->payload[i]);
+    }
+    printf ("\n");
+}
+
+EncryptedPayloadStruct decodeEncryptedPayload (FrameStruct frame) {
+    EncryptedPayloadStruct enc;
+    memset (&enc, 0, sizeof (enc));
+    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 enc;
+    }
+
+    ESP_LOGI(TAG, "Finding remote node, sourceHash is %d", enc.sourceHash);
+
+    NodeEntry *remNode = getNode (enc.sourceHash);
+
+    enc.remNode = remNode;
+
+
+    if (remNode == NULL) {
+        ESP_LOGW(TAG, "Node not in DB");
+        return enc;
+    }
+    ESP_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) {
+        ESP_LOGW (TAG, "HMAC failed on encrypted message %s", remNode->name);
+    } else {
+        enc.payloadLen = frame.payloadLen - HMAC_SIZE;
+        ESP_LOGI(TAG, "HMAC success from %s, %u bytes long", remNode->name, enc.payloadLen);
+        sendDiscreteAck(enc.payload, 5 + strlen((char *)&enc.payload[5]), remNode->pubKey);
+    }
+
+    return enc;
+}
+
+void parseEncryptedPayload (EncryptedPayloadStruct enc) {
+    // printEncryptedPayload(&enc);
+
+    printf ("EncryptedPayload:\n");
+    printf ("  Type: 0x%02X\n", enc.type);
+    printf ("  DestinationHash: 0x%02X\n", enc.destinationHash);
+    printf ("  SourceHash: 0x%02X\n", enc.sourceHash);
+    printf ("  CipherMAC: 0x%04X\n", enc.cipherMAC);
+    printf ("  PayloadLen: %u\n", enc.payloadLen);
+    printf ("  Payload: ");
+    hexdump("Full payload buffer", enc.payload, sizeof(enc.payload));
+    printf ("\n");
+
+    uint8_t index = 0;
+    if (enc.type == PAYLOAD_TYPE_PATH) {
+
+        ReturnedPathPayload retPath;
+        retPath.path.pathLen = enc.payload[index++];
+        if (retPath.path.pathLen > 64) {
+            ESP_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++];
+        memcpy (retPath.extra.data, &(enc.payload[index]), enc.payloadLen - index);
+
+    } 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;
+        req.requestType = enc.payload[index++];
+        req.dataLen = enc.payloadLen - index;
+        memcpy (req.data, &(enc.payload[index]), req.dataLen);
+        printRequest (&req);
+        switch (req.requestType) {
+            Response resp;
+            resp.tag = RTC_GetCounter();
+            memcpy(resp.data, &stats, sizeof(stats));
+            resp.dataLen = sizeof(stats);
+            sendEncryptedResponse(enc.remNode, &resp);
+            break;
+        case REQUEST_KEEPALIVE:
+            break;
+        case REQUEST_GET_TELEMETRY_DATA:
+            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;
+        plaintext.attempt = enc.payload[index] & 0x03;
+        plaintext.textType = enc.payload[index++] >> 2;
+        memcpy (plaintext.message, &(enc.payload[index]), enc.payloadLen - index);
+        printPlainTextMessage (&plaintext);
+    }
+}

User/meshcore/packets/encrypted.h

@@ -0,0 +1,25 @@
+#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>
+
+
+void sendEncryptedFrame (NodeEntry *targetNode, uint8_t payloadType, const uint8_t *plain, size_t plainLen);
+
+void sendEncryptedTextMessage (NodeEntry *targetNode, const PlainTextMessagePayload *msg);
+
+void sendEncryptedResponse (NodeEntry *targetNode, const Response *resp);
+
+void sendEncryptedRequest (NodeEntry *targetNode, const Request *req);
+
+void sendEncryptedPathPayload (NodeEntry *targetNode, const ReturnedPathPayload *path);
+
+EncryptedPayloadStruct decodeEncryptedPayload (FrameStruct frame);
+
+void parseEncryptedPayload (EncryptedPayloadStruct enc);
+
+#endif

User/meshcore/packets/group.c

@@ -0,0 +1,108 @@
+#include "ch32v30x_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 (GroupTextMessage msg) {
+
+    msg.channelHash = persistent.aesKeys[msg.keyIndex][0];
+
+    msg.flags = 0;
+    msg.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;
+    memset (frame.payload, 0, sizeof (frame.payload));
+    frame.payload[offset++] = msg.channelHash;
+
+    uint8_t cipherBuf[176];
+    size_t offset2 = 0;
+    memcpy (cipherBuf, (const void *)&(msg.timestamp), 4);
+    offset2 += 4;
+    cipherBuf[offset2++] = msg.flags;
+    size_t textSize = offset2 + strlen ((const char *)msg.text);
+    if (textSize > 175) {
+        textSize = 175;
+    }
+    memcpy (cipherBuf + offset2, msg.text, textSize);
+    offset2 += textSize;
+
+    size_t olen = 0;
+    hexdump ("TxDumpDec", cipherBuf, offset2);
+    encrypt_then_mac (&(persistent.aesKeys[msg.keyIndex][1]), 16, cipherBuf, offset2, &(frame.payload[offset]), &olen);
+
+    frame.payloadLen = olen + 1;
+
+    sendFrame (frame);
+    return;
+}
+
+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;
+}
+
+GroupTextMessage decodeGroupMessage (FrameStruct frame) {
+    GroupTextMessage msg;
+    memset (&msg, 0, sizeof (msg));
+    if ((frame.header & PAYLOAD_TYPE_MASK) != PAYLOAD_TYPE_GRP_TXT) {
+        ESP_LOGW (TAG, "Not a group text");
+        return msg;
+    }
+    unsigned char index = 0;
+    msg.channelHash = frame.payload[index++];
+    unsigned char tmp[184];
+
+
+    unsigned char decrypted = 0;
+    for (unsigned char i = 0; i < AESKeyCount; i++) {
+
+        if (msg.channelHash != persistent.aesKeys[i][0]) {
+            ESP_LOGW (TAG, "Hash %d does not equal %d", persistent.aesKeys[i][0], msg.channelHash);
+            continue;
+        }
+
+        ESP_LOGW (TAG, "Hash does equal %d", msg.channelHash);
+
+        if (mac_then_decrypt (persistent.aesKeys[i] + 1, 16, frame.payload + index, frame.payloadLen - index, tmp) != 0) {
+            ESP_LOGW (TAG, "HMAC failed on grouphash key %d not matching %d", persistent.aesKeys[i][0], msg.channelHash);
+            continue;
+        }
+        hexdump ("RxDumpDec", tmp, frame.payloadLen - index);
+        decrypted = 1;
+        break;
+    }
+
+
+    if (!decrypted) {
+        return msg;
+    }
+
+    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);
+    return msg;
+}
+
+void printGroupMessage (GroupTextMessage msg) {
+    printf ("Message with channel hash %d, flags %d: %s\n", msg.channelHash,
+            msg.flags, msg.text);
+}

User/meshcore/packets/group.h

@@ -0,0 +1,15 @@
+#ifndef GROUP_HEADER
+#define GROUP_HEADER
+
+#include "stdint.h"
+#include "meshcore/packetstructs.h"
+
+void sendGroupMessage (GroupTextMessage msg);
+
+void makeSendGroupMessage (char *txt, uint8_t keyIndex);
+
+GroupTextMessage decodeGroupMessage (FrameStruct frame);
+
+void printGroupMessage (GroupTextMessage msg);
+
+#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,6 @@
+#ifndef TRACE_HEADER
+#define TRACE_HEADER
+
+
+
+#endif