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"

#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);
}