BRNSystems/meshcore-wch
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
31dda624741ca0cc0b819a953ab1eb59dc64b3fd
meshcore-wch/User/meshcore/packets/encrypted.c
Bruno Rybársky 31dda62474 really still wip
2025-12-28 12:23:11 +01:00

359 lines
11 KiB
C
Raw Blame History

#include "lib/config.h"
#include "lib/telemetry/telemetry.h"
#include "meshcore/meshframing.h"
#include "meshcore/packets/ack.h"
#include "meshcore/packetstructs.h"
#include "meshcore/stats.h"
#include "util/hexdump.h"
#include "util/log.h"
#include <stdio.h>
#include <string.h>
#include "encrypted.h"
#include "FreeRTOS.h"
#include "task.h"
#include "lib/adc/temperature.h"
#define TICKS_TO_MS(xTicks) (((uint32_t)(xTicks)*1000U) / (uint32_t)configTICK_RATE_HZ)
#define TAG "EncryptedMessage"
void sendEncryptedFrame (NodeEntry *targetNode, uint8_t payloadType, const uint8_t *plain, size_t plainLen) {
FrameStruct frame;
uint8_t offset = 0;
// 1. Header
frame.header =
(targetNode->path.pathLen > 0 ? ROUTE_TYPE_DIRECT : ROUTE_TYPE_FLOOD) | // currently flood
payloadType |
PAYLOAD_VERSION_0;
// 2. Destination + source
frame.payload[offset++] = targetNode->pubKey[0];
frame.payload[offset++] = persistent.pubkey[0];
// 4. Encrypt + MAC
size_t encLen;
encrypt_then_mac (
targetNode->secret,
32,
plain,
plainLen,
frame.payload + offset,
&encLen);
offset += encLen;
// 5. Finalize
frame.payloadLen = offset;
memcpy (&frame.path, &targetNode->path, sizeof (frame.path));
hexdump ("Encrypted frame", frame.payload, frame.payloadLen);
LoRaTransmit (&frame);
}
void sendEncryptedTextMessage (NodeEntry *targetNode, const PlainTextMessagePayload *msg) {
if (targetNode == NULL) {
MESH_LOGW (TAG, "Node is null");
return;
}
if (targetNode->last_seen_lt == 0) {
MESH_LOGW (TAG, "Node is not populated");
return;
}
uint8_t buf[256];
uint8_t index = 0;
uint8_t msgLen = strlen (msg->message) + 1;
buf[index++] = msg->timestamp;
buf[index++] = msg->timestamp >> 8;
buf[index++] = msg->timestamp >> 16;
buf[index++] = msg->timestamp >> 24;
buf[index++] = (msg->textType << 2) | (msg->attempt & 0x03);
memcpy (&buf[index], msg->message, msgLen);
sendEncryptedFrame (
targetNode,
PAYLOAD_TYPE_TXT_MSG,
buf,
index + msgLen);
}
void sendEncryptedResponse (NodeEntry *targetNode, const Response *resp) {
uint8_t buf[256];
uint8_t index = 0;
buf[index++] = (resp->tag) & 0xFF;
buf[index++] = (resp->tag >> 8) & 0xFF;
buf[index++] = (resp->tag >> 16) & 0xFF;
buf[index++] = (resp->tag >> 24) & 0xFF;
memcpy (&(buf[index]), resp->data, resp->dataLen);
index += resp->dataLen;
sendEncryptedFrame (
targetNode,
PAYLOAD_TYPE_RESPONSE,
buf,
index);
}
void sendEncryptedRequest (NodeEntry *targetNode, const Request *req) {
uint8_t buf[256];
uint8_t index = 0;
buf[index++] = req->timestamp;
buf[index++] = req->timestamp >> 8;
buf[index++] = req->timestamp >> 16;
buf[index++] = req->timestamp >> 24;
buf[index++] = req->requestType;
memcpy (&(buf[index]), req->data, req->dataLen);
index += req->dataLen;
sendEncryptedFrame (
targetNode,
PAYLOAD_TYPE_REQ,
buf,
index);
}
void sendEncryptedPathPayload (NodeEntry *targetNode, const ReturnedPathPayload *path) {
uint8_t buf[256];
uint8_t index = 0;
buf[index++] = path->path.pathLen;
memcpy (&buf[index], path->path.path, path->path.pathLen);
index += path->path.pathLen;
buf[index++] = path->extra.type;
memcpy (&buf[index], path->extra.data,
path->extra.dataLen);
sendEncryptedFrame (
targetNode,
PAYLOAD_TYPE_PATH,
buf,
index);
}
void printRequest (const Request *req) {
printf ("Request:\n");
printf (" Timestamp: %u\n", req->timestamp);
printf (" Type: 0x%02X\n", req->requestType);
printf (" Data: ");
hexdump (" Data", req->data, req->dataLen);
}
void printResponse (const Response *resp) {
printf ("Response:\n");
printf (" Tag: %u\n", resp->tag);
printf (" Data: ");
hexdump (" Data", resp->data, resp->dataLen);
}
void printPlainTextMessage (const PlainTextMessagePayload *msg) {
printf ("PlainTextMessage:\n");
printf (" Timestamp: %u\n", msg->timestamp);
printf (" Attempt: %u\n", msg->attempt);
printf (" TextType: %u\n", msg->textType);
printf (" Message: %.*s\n", (int)strlen (msg->message), msg->message);
}
void printReturnedPathPayload (const ReturnedPathPayload *path) {
printf ("ReturnedPathPayload:\n");
printf (" Path Length: %u\n", path->path.pathLen);
printf (" Path: ");
hexdump (" Path:", path->path.path, path->path.pathLen);
printf (" Extra Type: %u\n", path->extra.type);
printf (" Extra Data: ");
hexdump (" Extra data:", path->extra.data, path->extra.dataLen);
}
void printEncryptedPayload (const EncryptedPayloadStruct *enc) {
printf ("EncryptedPayload:\n");
printf (" Type: 0x%02X\n", enc->type);
printf (" DestinationHash: 0x%02X\n", enc->destinationHash);
printf (" SourceHash: 0x%02X\n", enc->sourceHash);
printf (" CipherMAC: 0x%04X\n", enc->cipherMAC);
printf (" PayloadLen: %zu\n", enc->payloadLen);
printf (" Payload: ");
for (size_t i = 0; i < enc->payloadLen; i++) {
printf ("%02X ", enc->payload[i]);
}
printf ("\n");
}
void decodeEncryptedPayload (const FrameStruct *frame) {
EncryptedPayloadStruct enc;
memset (&enc, 0, sizeof (enc));
enc.type = frame->header & PAYLOAD_TYPE_MASK;
unsigned char index = 0;
enc.destinationHash = frame->payload[index++];
enc.sourceHash = frame->payload[index++];
enc.cipherMAC = frame->payload[index];
enc.cipherMAC |= frame->payload[index + 1] << 8;
if (enc.destinationHash != persistent.pubkey[0]) {
return;
}
MESH_LOGI (TAG, "Finding remote node, sourceHash is %d", enc.sourceHash);
NodeEntry *remNode = getNode (enc.sourceHash);
enc.remNode = remNode;
if (remNode == NULL) {
MESH_LOGW (TAG, "Node not in DB");
return;
}
MESH_LOGI (TAG, "Found node with index %d", remNode - persistent.contacts);
if (mac_then_decrypt (remNode->secret, 32, &(frame->payload[index]), frame->payloadLen - index, enc.payload) != 0) {
MESH_LOGW (TAG, "HMAC failed on encrypted message %s", remNode->name);
} else {
enc.payloadLen = frame->payloadLen - HMAC_SIZE;
MESH_LOGI (TAG, "HMAC success from %s, %u bytes long", remNode->name, enc.payloadLen);
sendDiscreteAck (enc.payload, 5 + strlen ((char *)&enc.payload[5]), remNode->pubKey);
}
printf (" Typexdd: 0x%02X\n", enc.type);
if (enc.payloadLen > 0) {
parseEncryptedPayload (&enc);
}
}
void parseEncryptedPayload (const EncryptedPayloadStruct *enc) {
// printEncryptedPayload(&enc);
printf ("EncryptedPayload:\n");
printf (" Type: 0x%02X\n", enc->type);
printf (" DestinationHash: 0x%02X\n", enc->destinationHash);
printf (" SourceHash: 0x%02X\n", enc->sourceHash);
printf (" CipherMAC: 0x%04X\n", enc->cipherMAC);
printf (" PayloadLen: %u\n", enc->payloadLen);
hexdump (" Payload: ", enc->payload, enc->payloadLen);
printf ("\n");
uint8_t index = 0;
if (enc->type == PAYLOAD_TYPE_PATH) {
ReturnedPathPayload retPath;
retPath.path.pathLen = enc->payload[index++];
if (retPath.path.pathLen > 64) {
MESH_LOGW (TAG, "Path too long\n");
return;
}
memcpy (retPath.path.path, &(enc->payload[index]), retPath.path.pathLen);
index += retPath.path.pathLen;
retPath.extra.type = enc->payload[index++];
retPath.extra.dataLen = enc->payloadLen - index;
memcpy (retPath.extra.data, &(enc->payload[index]), retPath.extra.dataLen);
} else if (enc->type == PAYLOAD_TYPE_REQ) {
Request req;
req.timestamp = enc->payload[index++];
req.timestamp |= enc->payload[index++] << 8;
req.timestamp |= enc->payload[index++] << 16;
req.timestamp |= enc->payload[index++] << 24;
req.requestType = enc->payload[index++];
req.dataLen = enc->payloadLen - index;
memcpy (req.data, &(enc->payload[index]), req.dataLen);
printRequest (&req);
switch (req.requestType) {
case REQUEST_GET_STATS: {
Response resp;
resp.tag = RTC_GetCounter();
stats.totalUpTimeSeconds = RTC_GetCounter() - startupTime;
stats.totalAirTimeSeconds = TICKS_TO_MS (tickAirtime / 1000);
memcpy (resp.data, &stats, sizeof (stats));
resp.dataLen = sizeof (stats);
sendEncryptedResponse (enc->remNode, &resp);
break;
}
case REQUEST_KEEPALIVE:
break;
case REQUEST_GET_TELEMETRY_DATA: {
Response resp;
resp.tag = req.timestamp;
uint8_t index2 = 0;
resp.data[index2++] = TELEM_CHANNEL_SELF;
resp.data[index2++] = LPP_TEMPERATURE;
int16_t dataTemp = getDeciTemperature();
printf ("The temperature is %d decicelsius\n", dataTemp);
resp.data[index2++] = (dataTemp >> 8) & 0xFF;
resp.data[index2++] = dataTemp & 0xFF;
resp.data[index2++] = TELEM_CHANNEL_SELF;
resp.data[index2++] = LPP_VOLTAGE;
int16_t dataVolt = stats.millivolts / 10;
resp.data[index2++] = (dataVolt >> 8) & 0xFF;
resp.data[index2++] = dataVolt & 0xFF;
if (enc->remNode->authenticated) {
encode_gps (TELEM_CHANNEL_SELF, persistent.latitude / 1000000.0f, persistent.longitude / 1000000.0f, persistent.altitude / 100.0f, &(resp.data[index2]));
// encode_gps(TELEM_CHANNEL_SELF, 48.1909f, 17.0303f, 234.0f, &(resp.data[index2]));
index2 += LPP_GPS_SIZE;
}
if (enc->remNode->authenticated) {
resp.data[index2++] = 2;
resp.data[index2++] = LPP_TEMPERATURE;
int16_t jokeTemp = 6942;
resp.data[index2++] = (jokeTemp >> 8) & 0xFF;
resp.data[index2++] = jokeTemp & 0xFF;
resp.dataLen = index2;
}
sendEncryptedResponse (enc->remNode, &resp);
break;
}
case REQUEST_GET_MIN_MAX_AVG:
break;
case REQUEST_GET_ACCESS_LIST:
break;
}
} else if (enc->type == PAYLOAD_TYPE_RESPONSE) {
Response resp;
resp.tag = enc->payload[index++];
resp.tag |= enc->payload[index++] << 8;
resp.tag |= enc->payload[index++] << 16;
resp.tag |= enc->payload[index++] << 24;
resp.dataLen = enc->payloadLen - index;
memcpy (resp.data, &(enc->payload[index]), resp.dataLen);
printResponse (&resp);
} else if (enc->type == PAYLOAD_TYPE_TXT_MSG) {
PlainTextMessagePayload plaintext;
plaintext.timestamp = enc->payload[index++];
plaintext.timestamp |= enc->payload[index++] << 8;
plaintext.timestamp |= enc->payload[index++] << 16;
plaintext.timestamp |= enc->payload[index++] << 24;
plaintext.attempt = enc->payload[index] & 0x03;
plaintext.textType = enc->payload[index++] >> 2;
memcpy (plaintext.message, &(enc->payload[index]), enc->payloadLen - index);
printPlainTextMessage (&plaintext);
}
}
Reference in New Issue View Git Blame Copy Permalink