76 lines
2.6 KiB
C
76 lines
2.6 KiB
C
#include "meshcore/meshframing.h"
|
|
#include "meshcore/packetstructs.h"
|
|
#include "control.h"
|
|
#include "string.h"
|
|
|
|
#define TAG "Control"
|
|
|
|
void sendDiscoverRequest(const DiscoverRequestPayload *discReq) {
|
|
|
|
FrameStruct frame;
|
|
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;
|
|
|
|
sendFrame(frame);
|
|
}
|
|
|
|
|
|
void decodeControlFrame(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;
|
|
}
|
|
|
|
} 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;
|
|
memcpy(discResp.pubkey, &(frame.payload[index]), pubKeyLen);
|
|
index += pubKeyLen;
|
|
|
|
}
|
|
} |