303 lines
8.6 KiB
C
303 lines
8.6 KiB
C
#include "ch32v30x_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, ×tamp, 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);
|
|
}
|