meshcore-wch/User/meshcore/packets/advert.c

#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"
#include "util/log.h"
#include "FreeRTOS.h"
#include "task.h"

#define TAG "Advert"

void sendAdvert (uint8_t shouldFlood) {
    printf ("High-water mark before preparing advert: %u\n", uxTaskGetStackHighWaterMark (NULL));
    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 ---- */
    printf ("High-water mark before signing: %u\n", uxTaskGetStackHighWaterMark (NULL));
    ed25519_sign_ad (&frame);
    printf ("High-water mark after signing: %u\n", uxTaskGetStackHighWaterMark (NULL));

    /* ---- debug ---- */
    hexdump ("Public key", frame.payload, 32);
    hexdump ("Signature", signature_pos, 64);
    hexdump ("Appdata", frame.payload + app_start, app_len);
    printf ("Timestamp is %lu\n", timestamp);
    printf ("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) != 1) {
        MESH_LOGW (TAG, "Incorrect signature");
        return;
    }
    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_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;
}

void printAdvertisement (const AdvertisementPayload *advert) {
    printf (
        "%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);
}