mirror of
https://github.com/ByteWelder/Tactility.git
synced 2026-10-17 19:05:07 +00:00
79 lines
3.1 KiB
C++
79 lines
3.1 KiB
C++
// SPDX-License-Identifier: Apache-2.0
|
|
#include <mesh_meshcore/meshcore_crypto.h>
|
|
#include <mesh_meshcore/meshcore_packet.h>
|
|
|
|
#include <cstring>
|
|
|
|
bool meshcore_packet_parse(const uint8_t* data, size_t length, MeshCorePacket* packet) {
|
|
size_t position = 0;
|
|
if (length < 2) {
|
|
return false;
|
|
}
|
|
|
|
const uint8_t header = data[position++];
|
|
packet->route = static_cast<MeshCoreRoute>(header & 0x03);
|
|
packet->payloadType = static_cast<MeshCorePayload>((header >> 2) & 0x0F);
|
|
packet->payloadVersion = (header >> 6) & 0x03;
|
|
|
|
if (packet->hasTransportCodes()) {
|
|
if (position + 4 > length) {
|
|
return false;
|
|
}
|
|
packet->transportCodes[0] = data[position] | (data[position + 1] << 8);
|
|
packet->transportCodes[1] = data[position + 2] | (data[position + 3] << 8);
|
|
position += 4;
|
|
}
|
|
|
|
if (position >= length) {
|
|
return false;
|
|
}
|
|
// The upper 2 bits hold the hash size minus one, the lower 6 bits the hop count
|
|
const uint8_t pathByte = data[position++];
|
|
packet->pathLength = pathByte & 0x3F;
|
|
packet->pathHashSize = (pathByte >> 6) + 1;
|
|
const size_t pathBytes = (size_t)packet->pathLength * packet->pathHashSize;
|
|
if (pathBytes > MESHCORE_MAX_PATH_SIZE || position + pathBytes > length) {
|
|
return false;
|
|
}
|
|
memcpy(packet->path, data + position, pathBytes);
|
|
position += pathBytes;
|
|
|
|
const size_t payloadLength = length - position;
|
|
if (payloadLength > MESHCORE_MAX_PAYLOAD_SIZE) {
|
|
return false;
|
|
}
|
|
packet->payloadLength = (uint8_t)payloadLength;
|
|
memcpy(packet->payload, data + position, payloadLength);
|
|
return true;
|
|
}
|
|
|
|
size_t meshcore_packet_serialize(const MeshCorePacket& packet, uint8_t* data, size_t capacity) {
|
|
const size_t pathBytes = (size_t)packet.pathLength * packet.pathHashSize;
|
|
const size_t needed = 1 + (packet.hasTransportCodes() ? 4 : 0) + 1 + pathBytes + packet.payloadLength;
|
|
if (needed > capacity || pathBytes > MESHCORE_MAX_PATH_SIZE || packet.pathHashSize == 0 || packet.pathHashSize > 4) {
|
|
return 0;
|
|
}
|
|
|
|
size_t position = 0;
|
|
data[position++] = (uint8_t)(((uint8_t)packet.route & 0x03) | (((uint8_t)packet.payloadType & 0x0F) << 2) | ((packet.payloadVersion & 0x03) << 6));
|
|
if (packet.hasTransportCodes()) {
|
|
data[position++] = (uint8_t)packet.transportCodes[0];
|
|
data[position++] = (uint8_t)(packet.transportCodes[0] >> 8);
|
|
data[position++] = (uint8_t)packet.transportCodes[1];
|
|
data[position++] = (uint8_t)(packet.transportCodes[1] >> 8);
|
|
}
|
|
data[position++] = (uint8_t)((packet.pathLength & 0x3F) | ((packet.pathHashSize - 1) << 6));
|
|
memcpy(data + position, packet.path, pathBytes);
|
|
position += pathBytes;
|
|
memcpy(data + position, packet.payload, packet.payloadLength);
|
|
position += packet.payloadLength;
|
|
return position;
|
|
}
|
|
|
|
uint32_t meshcore_packet_hash(const MeshCorePacket& packet) {
|
|
const uint8_t type = (uint8_t)packet.payloadType;
|
|
uint8_t hash[4];
|
|
meshcore_sha256_pair(hash, sizeof(hash), &type, 1, packet.payload, packet.payloadLength);
|
|
return hash[0] | (hash[1] << 8) | (hash[2] << 16) | ((uint32_t)hash[3] << 24);
|
|
}
|