really still wip
This commit is contained in:
@@ -7,186 +7,202 @@
|
||||
#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() {
|
||||
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
|
||||
void sendAdvert (uint8_t shouldFlood) {
|
||||
printf ("High-water mark before preparing advert: %u\n", uxTaskGetStackHighWaterMark (NULL));
|
||||
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);
|
||||
frame.header = (shouldFlood ? ROUTE_TYPE_FLOOD : ROUTE_TYPE_DIRECT) | PAYLOAD_TYPE_ADVERT | PAYLOAD_VERSION_0;
|
||||
|
||||
// reserve signature space
|
||||
/* ---- 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;
|
||||
|
||||
// append app_data after signature
|
||||
memcpy (frame.payload + offset, app_data, app_len);
|
||||
offset += app_len;
|
||||
/* ---- build app data directly into payload ---- */
|
||||
size_t app_start = offset;
|
||||
|
||||
// 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;
|
||||
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;
|
||||
|
||||
ed25519_sign (signature_pos, message, msg_len, persistent.pubkey, persistent.privkey);
|
||||
frame.payload[offset++] = dataFlags;
|
||||
|
||||
hexdump ("Complete advert", frame.payload, offset);
|
||||
if (dataFlags & ADVERTISEMENT_FLAG_HAS_LOCATION) {
|
||||
memcpy (frame.payload + offset, &persistent.latitude, sizeof (persistent.latitude));
|
||||
offset += sizeof (persistent.latitude);
|
||||
|
||||
hexdump ("Public key", ad.pubKey, 32);
|
||||
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);
|
||||
|
||||
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
|
||||
/* ---- send ---- */
|
||||
frame.payloadLen = offset;
|
||||
frame.path.pathLen = 0;
|
||||
sendFrame (frame);
|
||||
LoRaTransmit (&frame);
|
||||
}
|
||||
|
||||
AdvertisementPayload decodeAdvertisement (FrameStruct frame) {
|
||||
void decodeAdvertisement (const FrameStruct *frame) {
|
||||
AdvertisementPayload advert;
|
||||
memset (&advert, 0, sizeof (advert));
|
||||
|
||||
if ((frame.header & PAYLOAD_TYPE_MASK) != PAYLOAD_TYPE_ADVERT) {
|
||||
return 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);
|
||||
memcpy (advert.pubKey, frame->payload + index, 32);
|
||||
index += 32;
|
||||
|
||||
memcpy (&advert.timestamp, frame.payload + index, 4);
|
||||
memcpy (&advert.timestamp, frame->payload + index, 4);
|
||||
index += 4;
|
||||
|
||||
memcpy (advert.signature, frame.payload + index, 64);
|
||||
memcpy (advert.signature, frame->payload + index, 64);
|
||||
index += 64;
|
||||
|
||||
advert.dataFlags = frame.payload[index++];
|
||||
advert.dataFlags = frame->payload[index++];
|
||||
|
||||
uint8_t expectedLen = 101;
|
||||
|
||||
if (advert.dataFlags & ADVERTISEMENT_FLAG_HAS_LOCATION) {
|
||||
memcpy (&advert.latitude, frame.payload + index, 4);
|
||||
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);
|
||||
memcpy (&advert.longitude, frame->payload + index, 4);
|
||||
index += 4;
|
||||
}
|
||||
|
||||
if (advert.dataFlags & ADVERTISEMENT_FLAG_RFU1) {
|
||||
memcpy (&advert.rfu1, frame.payload + index, 2);
|
||||
memcpy (&advert.rfu1, frame->payload + index, 2);
|
||||
index += 2;
|
||||
}
|
||||
if (advert.dataFlags & ADVERTISEMENT_FLAG_RFU2) {
|
||||
memcpy (&advert.rfu2, frame.payload + index, 2);
|
||||
memcpy (&advert.rfu2, frame->payload + index, 2);
|
||||
index += 2;
|
||||
}
|
||||
unsigned char nameLen = frame.payloadLen - index;
|
||||
if (nameLen > 32) {
|
||||
nameLen = 32;
|
||||
unsigned char nameLen = frame->payloadLen - index;
|
||||
|
||||
if (nameLen > 31) {
|
||||
nameLen = 31; // leave space for null
|
||||
}
|
||||
memcpy (advert.nodeName, frame.payload + index, nameLen);
|
||||
advert.nodeName[frame.payloadLen - index] = 0;
|
||||
memcpy (advert.nodeName, frame->payload + index, nameLen);
|
||||
advert.nodeName[nameLen] = 0;
|
||||
|
||||
|
||||
NodeEntry *node = getNode (advert.pubKey[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;
|
||||
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->type = advert->dataFlags & 0x0F;
|
||||
node->last_seen_lt = RTC_GetCounter();
|
||||
node->last_seen_rt = advert.timestamp;
|
||||
|
||||
|
||||
return advert;
|
||||
node->last_seen_rt = advert->timestamp;
|
||||
}
|
||||
|
||||
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);
|
||||
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);
|
||||
|
||||
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);
|
||||
}
|
||||
hexdump ("Public key", advert->pubKey, 32);
|
||||
hexdump ("Signature", advert->signature, 64);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user