150 lines
5.1 KiB
C
150 lines
5.1 KiB
C
#include "lib/config.h"
|
|
#include "meshcore/meshframing.h"
|
|
#include "meshcore/packetstructs.h"
|
|
#include "control.h"
|
|
#include "meshcore/stats.h"
|
|
#include "string.h"
|
|
#include "util/hexdump.h"
|
|
#include "util/log.h"
|
|
#include <stdio.h>
|
|
#include "FreeRTOS.h"
|
|
#include "task.h"
|
|
|
|
#define TAG "Control"
|
|
|
|
void sendDiscoverRequest (const DiscoverRequestPayload *discReq) {
|
|
|
|
FrameStruct frame;
|
|
frame.path.pathLen = 0;
|
|
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;
|
|
|
|
LoRaTransmit (&frame);
|
|
}
|
|
|
|
void sendDiscoverResponse (const DiscoverResponsePayload *discResp) {
|
|
FrameStruct frame;
|
|
frame.header = ROUTE_TYPE_DIRECT | PAYLOAD_TYPE_CONTROL | PAYLOAD_VERSION_0;
|
|
frame.path.pathLen = 0;
|
|
uint8_t offset = 0;
|
|
|
|
/* Control type + node type (lower nibble) */
|
|
frame.payload[offset++] =
|
|
(discResp->nodeType & 0x0F) |
|
|
CONTROL_DATA_FLAG_DISCOVER_RESP;
|
|
|
|
/* SNR */
|
|
frame.payload[offset++] = (uint8_t)discResp->snr;
|
|
|
|
/* Tag (LE) */
|
|
frame.payload[offset++] = (discResp->tag >> 0) & 0xFF;
|
|
frame.payload[offset++] = (discResp->tag >> 8) & 0xFF;
|
|
frame.payload[offset++] = (discResp->tag >> 16) & 0xFF;
|
|
frame.payload[offset++] = (discResp->tag >> 24) & 0xFF;
|
|
|
|
/* Pubkey */
|
|
if (discResp->pubkeyLen > 0) {
|
|
memcpy (&frame.payload[offset],
|
|
discResp->pubkey,
|
|
discResp->pubkeyLen);
|
|
offset += discResp->pubkeyLen;
|
|
}
|
|
|
|
frame.payloadLen = offset;
|
|
|
|
LoRaTransmit (&frame);
|
|
}
|
|
|
|
void printDiscoverRequest (const DiscoverRequestPayload *p) {
|
|
printf ("=== Discover Request ===\n");
|
|
printf ("prefixOnly : %u\n", p->prefixOnly);
|
|
printf ("typeFilter : 0x%02X\n", p->typeFilter);
|
|
printf ("tag : 0x%08lX\n", (unsigned long)p->tag);
|
|
printf ("since : 0x%08lX\n", (unsigned long)p->since);
|
|
}
|
|
|
|
void printDiscoverResponse (const DiscoverResponsePayload *p) {
|
|
printf ("=== Discover Response ===\n");
|
|
printf ("nodeType : %u\n", p->nodeType);
|
|
printf ("snr : %u\n", p->snr);
|
|
printf ("tag : 0x%08lX\n", (unsigned long)p->tag);
|
|
|
|
hexdump ("pubkey : ", p->pubkey, p->pubkeyLen);
|
|
printf ("\n");
|
|
}
|
|
|
|
void decodeControlFrame (const 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;
|
|
}
|
|
printDiscoverRequest (&discReq);
|
|
if ((discReq.typeFilter >> 1) & persistent.nodeType) {
|
|
DiscoverResponsePayload discResp;
|
|
|
|
discResp.tag = discReq.tag;
|
|
discResp.nodeType = persistent.nodeType;
|
|
discResp.pubkeyLen = sizeof (persistent.pubkey);
|
|
memcpy (discResp.pubkey, persistent.pubkey, discResp.pubkeyLen);
|
|
discResp.snr = stats.lastSNR; // hopefully the correct one
|
|
|
|
|
|
MESH_LOGD (TAG, "Replying to a discover request with tag %d", discResp.tag);
|
|
sendDiscoverResponse (&discResp);
|
|
printDiscoverResponse (&discResp);
|
|
}
|
|
|
|
} 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;
|
|
discResp.pubkeyLen = pubKeyLen;
|
|
memcpy (discResp.pubkey, &(frame->payload[index]), discResp.pubkeyLen);
|
|
index += pubKeyLen;
|
|
printDiscoverResponse (&discResp);
|
|
}
|
|
} |