BRNSystems/meshcore-wch
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

really still wip

Browse Source
This commit is contained in:
Bruno Rybársky
2025-12-28 12:23:11 +01:00
parent a48ef9d5e0
commit 31dda62474
33 changed files with 1721 additions and 589 deletions
Download Patch File Download Diff File Expand all files Collapse all files

344
User/meshcore/meshframing.c
View File

@@ -1,12 +1,110 @@
#include "meshframing.h"
#include "ch32v30x_gpio.h"
#include "lib/config.h"
#include "meshcore/stats.h"
#include "string.h"
#include "stdio.h"
#include "sx1262.h"
#include "util/hexdump.h"
#include "util/log.h"
#include "FreeRTOS.h"
#include "task.h"
#include "lib/cifra/sha2.h"
#define TAG "Meshframing"
int ReadFrame (FrameStruct *frame, int8_t *rssiPacket, int8_t *snrPacket, int8_t *rawSnr) {
uint16_t irqRegs = GetIrqStatus();
// uint8_t status = GetStatus();
if (irqRegs & SX126X_IRQ_RX_DONE) {
// ClearIrqStatus(SX126X_IRQ_RX_DONE);
ClearIrqStatus (SX126X_IRQ_ALL);
uint8_t offset = 0;
uint8_t payloadLength = 0;
GetRxBufferStatus (&payloadLength, &offset);
if (payloadLength == 0) {
return 0;
}
GetPacketStatus (rssiPacket, snrPacket, rawSnr);
memset (frame, 0, sizeof (FrameStruct));
WaitForIdle (BUSY_WAIT, "start ReadBuffer", 1);
uint8_t cmd[3] = {SX126X_CMD_READ_BUFFER, offset, SX126X_CMD_NOP};
GPIO_WriteBit (GPIOA, GPIO_Pin_4, 0);
uint16_t curPayloadIndex = 0;
uint8_t pathIndex = 0;
uint8_t transportIndex = 0;
uint8_t state = 0; // 0=header, 1=transport, 2=pathLen, 3=path, 4=payload
for (uint16_t i = 0; i < payloadLength + sizeof (cmd); i++) {
uint8_t out = (i < sizeof (cmd)) ? cmd[i] : 0xFF;
// Wait TX ready
while (!SPI_I2S_GetFlagStatus (SPI1, SPI_I2S_FLAG_TXE));
SPI_I2S_SendData (SPI1, out);
// Wait RX ready
while (!SPI_I2S_GetFlagStatus (SPI1, SPI_I2S_FLAG_RXNE));
uint8_t in = (uint8_t)SPI_I2S_ReceiveData (SPI1);
// Only process payload bytes
if (i >= sizeof (cmd)) {
switch (state) {
case 0: // header
frame->header = in;
state = ((frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_DIRECT ||
(frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_FLOOD)
? 1
: 2;
break;
case 1: // transportCodes[4]
frame->transportCodes[transportIndex++] = in;
if (transportIndex >= 4)
state = 2;
break;
case 2: // pathLen
frame->path.pathLen = in;
if (frame->path.pathLen > 64) {
frame->path.pathLen = 64;
}
pathIndex = 0;
state = (frame->path.pathLen > 0) ? 3 : 4;
break;
case 3: // path
frame->path.path[pathIndex++] = in;
if (pathIndex >= frame->path.pathLen)
state = 4;
break;
case 4: // payload
frame->payload[curPayloadIndex++] = in;
break;
}
}
}
frame->payloadLen = curPayloadIndex;
GPIO_WriteBit (GPIOA, GPIO_Pin_4, 1);
WaitForIdle (BUSY_WAIT, "end ReadBuffer", 0);
return payloadLength;
}
return 0;
}
/*
FrameStruct decodeFrame (unsigned char *data, unsigned char dataLen) {
hexdump ("RxDump", data, dataLen);
FrameStruct frame;
memset (&frame, 0, sizeof (frame));
unsigned char index = 0;
@@ -25,9 +123,9 @@ FrameStruct decodeFrame (unsigned char *data, unsigned char dataLen) {
return frame;
}
void printFrameHeader (FrameStruct frame) {
switch (frame.header & ROUTE_TYPE_MASK) {
*/
void printFrameHeader (const FrameStruct *frame) {
switch (frame->header & ROUTE_TYPE_MASK) {
case ROUTE_TYPE_TRANSPORT_FLOOD:
printf ("transport flood");
break;
@@ -47,7 +145,7 @@ void printFrameHeader (FrameStruct frame) {
printf (", payload type is ");
switch (frame.header & PAYLOAD_TYPE_MASK) {
switch (frame->header & PAYLOAD_TYPE_MASK) {
case PAYLOAD_TYPE_REQ:
printf ("request");
break;
@@ -101,67 +199,204 @@ void printFrameHeader (FrameStruct frame) {
break;
}
char version[2];
version[0] = (frame.header >> 6) + '0';
version[0] = (frame->header >> 6) + '0';
version[1] = 0;
printf (", payload version is %s ", version);
if ((frame.header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_DIRECT ||
(frame.header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_FLOOD) {
printf ("Transport codes: %d %d\n", *((uint16_t *)frame.transportCodes),
*((uint16_t *)&(frame.transportCodes[2])));
if ((frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_DIRECT ||
(frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_FLOOD) {
printf ("Transport codes: %d %d\n", *((uint16_t *)frame->transportCodes),
*((uint16_t *)&(frame->transportCodes[2])));
}
printf ("Path is %d nodes long", frame.path.pathLen);
printf ("Path is %d nodes long", frame->path.pathLen);
for (uint8_t pathIndex = 0; pathIndex < frame.path.pathLen; pathIndex++) {
printf ("node %d - %02X, ", pathIndex, frame.path.path[pathIndex]);
for (uint8_t pathIndex = 0; pathIndex < frame->path.pathLen; pathIndex++) {
printf ("node %d - %02X, ", pathIndex, frame->path.path[pathIndex]);
}
putchar ('\n');
}
void sendFrame (FrameStruct frame) {
uint8_t txBuf[256];
size_t offset = 0;
void LoRaTransmit (const FrameStruct *frame) {
uint8_t len = 2; // header + path_len
txBuf[offset++] = frame.header;
if ((frame.header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_DIRECT ||
(frame.header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_FLOOD) {
memcpy (txBuf + offset, frame.transportCodes, 4);
offset += 4;
if ((frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_DIRECT ||
(frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_FLOOD) {
len += 4;
}
if (frame.path.pathLen > 64) {
frame.path.pathLen = 64;
}
len += frame->path.pathLen;
len += frame->payloadLen;
txBuf[offset++] = frame.path.pathLen;
uint16_t irqStatus;
char rv = 0;
memcpy (txBuf + offset, frame.path.path, frame.path.pathLen);
offset += frame.path.pathLen;
if (txActive == 0) {
txActive = 1;
uint16_t maxPayloadLen = 256 - offset;
if (PacketParams[2] == 0x00) { // explicit header, variable length
PacketParams[3] = len;
}
uint16_t payloadLen = frame.payloadLen > maxPayloadLen ? maxPayloadLen : frame.payloadLen;
WriteCommand (SX126X_CMD_SET_PACKET_PARAMS, PacketParams, 6);
ClearIrqStatus (SX126X_IRQ_ALL);
memcpy (txBuf + offset, frame.payload, payloadLen);
offset += payloadLen;
WaitForIdle (BUSY_WAIT, "start WriteBuffer", 1);
hexdump ("TxDump", txBuf, offset);
LoRaSend (txBuf, offset, SX126x_TXMODE_SYNC);
}
uint8_t cmdBuf[2] = {SX126X_CMD_WRITE_BUFFER, 0x00};
uint8_t cmdLen = sizeof (cmdBuf);
void retransmitFrame (FrameStruct frame) {
if (frame.header & ROUTE_TYPE_FLOOD || frame.header & ROUTE_TYPE_TRANSPORT_FLOOD) {
if (frame.header != DONT_RETRANSMIT_HEADER && frame.path.pathLen + 1 < MAX_FLOOD_TTL) {
frame.path.path[frame.path.pathLen++] = persistent.pubkey[0];
GPIO_WriteBit (GPIOA, GPIO_Pin_4, 0);
uint16_t payloadIndex = 0;
uint8_t state = 0;
uint8_t pathIndex = 0;
uint8_t transportIndex = 0;
for (uint16_t i = 0; i < len + cmdLen; i++) {
uint8_t out = 0xFF;
if (i < cmdLen) {
out = cmdBuf[i];
} else {
switch (state) {
case 0: // header
out = frame->header;
state = ((frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_DIRECT ||
(frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_FLOOD)
? 1
: 2;
break;
case 1: // transport codes
out = frame->transportCodes[transportIndex++];
if (transportIndex >= 4)
state = 2;
break;
case 2: // path length
out = frame->path.pathLen;
pathIndex = 0;
state = (frame->path.pathLen > 0) ? 3 : 4;
break;
case 3: // path
out = frame->path.path[pathIndex++];
if (pathIndex >= frame->path.pathLen)
state = 4;
break;
case 4: // payload
out = frame->payload[payloadIndex++];
break;
}
}
while (SPI_I2S_GetFlagStatus (SPI1, SPI_I2S_FLAG_TXE) == RESET);
SPI_I2S_SendData (SPI1, out);
while (SPI_I2S_GetFlagStatus (SPI1, SPI_I2S_FLAG_RXNE) == RESET);
(void)SPI_I2S_ReceiveData (SPI1); // discard
}
GPIO_WriteBit (GPIOA, GPIO_Pin_4, 1);
WaitForIdle (BUSY_WAIT, "end WriteBuffer", 0);
SetTx (5000);
irqStatus = GetIrqStatus();
while (!(irqStatus & (SX126X_IRQ_TX_DONE | SX126X_IRQ_TIMEOUT))) {
vTaskDelay (1);
irqStatus = GetIrqStatus();
}
txActive = 0;
SetRx (0xFFFFFF);
if (irqStatus & SX126X_IRQ_TX_DONE) {
rv = 1;
}
}
if (frame.header & ROUTE_TYPE_DIRECT || frame.header & ROUTE_TYPE_TRANSPORT_DIRECT) {
if (rv == 0) {
txLost++;
}
}
/*
void sendFrame (const FrameStruct *frame) {
uint8_t txBuf[256];
size_t offset = 0;
txBuf[offset++] = frame->header;
if ((frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_DIRECT ||
(frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_FLOOD) {
memcpy (txBuf + offset, frame->transportCodes, 4);
offset += 4;
}
if ((frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_FLOOD ||
(frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_FLOOD) {
stats.sentFloodCount++;
}
if ((frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_DIRECT ||
(frame->header & ROUTE_TYPE_MASK) == ROUTE_TYPE_TRANSPORT_DIRECT) {
stats.sentDirectCount++;
}
stats.packetsSentCount++;
uint8_t pathLen = frame->path.pathLen;
if (pathLen > 64) {
pathLen = 64;
}
txBuf[offset++] = pathLen;
memcpy (txBuf + offset, frame->path.path, pathLen);
hexdump ("TxDump Path", frame->path.path, frame->path.pathLen);
offset += pathLen;
uint16_t maxPayloadLen = 256 - offset;
uint16_t payloadLen = frame->payloadLen > maxPayloadLen ? maxPayloadLen : frame->payloadLen;
memcpy (txBuf + offset, frame->payload, payloadLen);
offset += payloadLen;
hexdump ("TxDump", txBuf, offset);
hexdump ("TxDumpPayload", frame->payload, frame->payloadLen);
TickType_t start, end;
start = xTaskGetTickCount();
LoRaSend (txBuf, offset, SX126x_TXMODE_SYNC);
end = xTaskGetTickCount();
tickAirtime += end - start;
}
*/
void retransmitFrame (FrameStruct *frame) {
MESH_LOGD (TAG, "Going to check ReTx");
if (frame->header & ROUTE_TYPE_FLOOD || frame->header & ROUTE_TYPE_TRANSPORT_FLOOD) {
MESH_LOGD (TAG, "Header is flood");
if (frame->header != DONT_RETRANSMIT_HEADER && frame->path.pathLen + 1 < MAX_FLOOD_TTL) {
MESH_LOGD (TAG, "Writing pubkey");
frame->path.path[frame->path.pathLen++] = persistent.pubkey[0];
MESH_LOGD (TAG, "Flooding");
vTaskDelay (10);
LoRaTransmit (frame); // TODO check if correct
MESH_LOGD (TAG, "Flooded");
}
}
if (frame->header & ROUTE_TYPE_DIRECT || frame->header & ROUTE_TYPE_TRANSPORT_DIRECT) {
}
}
// Verify MAC + Decrypt
@@ -173,8 +408,8 @@ int encrypt_then_mac (const uint8_t *aes_key, const uint8_t keySize, const uint8
// prepare padded buffer
uint8_t padded[padded_len];
memset(padded, 0, padded_len); // zero padding
memcpy(padded, plaintext, plen); // copy plaintext
memset (padded, 0, padded_len); // zero padding
memcpy (padded, plaintext, plen); // copy plaintext
// ciphertext will go right after HMAC
uint8_t *ciphertext = output + HMAC_SIZE;
@@ -202,7 +437,8 @@ int mac_then_decrypt (const uint8_t *aes_key, const uint8_t keySize, const uint8
const uint8_t *ciphertext = input + HMAC_SIZE;
size_t clen = ilen - HMAC_SIZE;
if (clen % 16 != 0) return -2; // must be multiple of block size
if (clen % 16 != 0)
return -2; // must be multiple of block size
uint8_t calc_mac[32]; // full SHA-256
hmac_sha256 (aes_key, keySize, ciphertext, clen, calc_mac);
@@ -211,4 +447,26 @@ int mac_then_decrypt (const uint8_t *aes_key, const uint8_t keySize, const uint8
return -2;
return aes_decrypt_ecb (aes_key, 16, ciphertext, clen, plaintext);
}
uint16_t getTransportCode (const FrameStruct *frame) {
uint16_t code;
/*
// compute HMAC over ciphertext
uint8_t mac[32]; // full SHA-256
cf_hmac_ctx ctx;
cf_hmac_init (&ctx, &cf_sha256, key, sizeof(key));
cf_hmac_update (&ctx, &(frame->header), sizeof (frame->header));
cf_hmac_update (&ctx, frame->payload, frame->payloadLen);
cf_hmac_finish (&ctx, mac);
// copy only HMAC_SIZE bytes of MAC
memcpy (&code, mac, HMAC_SIZE);
*/
if (code == 0) { // reserve codes 0000 and FFFF
code++;
} else if (code == 0xFFFF) {
code--;
}
return code;
}