147 lines
4.0 KiB
C++
147 lines
4.0 KiB
C++
//
|
|
// Created by erki on 28.07.22.
|
|
//
|
|
|
|
#include "radio_protocol.hpp"
|
|
|
|
#include <cstring>
|
|
|
|
namespace
|
|
{
|
|
|
|
using namespace radio::protocol;
|
|
|
|
[[nodiscard]] std::uint8_t* serializeAddress(std::uint8_t* data, const Address& address)
|
|
{
|
|
std::memmove(data, &address.pan_id, sizeof(address.pan_id));
|
|
data += sizeof(address.pan_id);
|
|
|
|
if (address.getAddressingMode() == FrameControlField::ADDRESSING_MODE_SHORT)
|
|
{
|
|
const std::uint16_t mac = address.getShortAddress();
|
|
std::memmove(data, &mac, sizeof(mac));
|
|
data += sizeof(mac);
|
|
}
|
|
else
|
|
{
|
|
const std::uint64_t mac = address.getLongAddress();
|
|
std::memmove(data, &mac, sizeof(mac));
|
|
data += sizeof(mac);
|
|
}
|
|
|
|
return data;
|
|
}
|
|
|
|
[[nodiscard]] std::uint8_t* serializePhyHeader(std::uint8_t* data, const PhyHeader& header)
|
|
{
|
|
static_assert(sizeof(PhyHeader) == 1, "PHY Header must be 1 byte/octet.");
|
|
std::memmove(data, &header, sizeof(PhyHeader));
|
|
data += sizeof(PhyHeader);
|
|
|
|
return data;
|
|
}
|
|
|
|
[[nodiscard]] std::uint8_t* serializeMacProtocolDataUnit(std::uint8_t* data, const FrameStructure& frame)
|
|
{
|
|
static_assert(sizeof(frame.frame_control_field) == 2, "FCF must be 2 bytes/octets.");
|
|
std::memmove(data, &frame.frame_control_field, sizeof(frame.frame_control_field));
|
|
data += sizeof(FrameControlField);
|
|
|
|
std::memmove(data, &frame.sequence_number, sizeof(frame.sequence_number));
|
|
data += sizeof(frame.sequence_number);
|
|
|
|
data = serializeAddress(data, frame.destination_address);
|
|
data = serializeAddress(data, frame.source_address);
|
|
|
|
std::memmove(data, frame.payload.data(), frame.payload_length);
|
|
data += frame.payload_length;
|
|
|
|
return data;
|
|
}
|
|
|
|
[[nodiscard]] const std::uint8_t* deserializeAddress(const std::uint8_t* data, const FrameControlField::AddressingMode& addressing_mode, Address& address)
|
|
{
|
|
std::memmove(&address.pan_id, data, sizeof(address.pan_id));
|
|
data += sizeof(address.pan_id);
|
|
|
|
if (addressing_mode == FrameControlField::ADDRESSING_MODE_SHORT)
|
|
{
|
|
std::uint16_t mac = 0;
|
|
std::memmove(&mac, data, sizeof(mac));
|
|
data += sizeof(mac);
|
|
|
|
address.setShortAddress(mac);
|
|
}
|
|
else
|
|
{
|
|
std::uint64_t mac = 0;
|
|
std::memmove(&mac, data, sizeof(mac));
|
|
data += sizeof(mac);
|
|
|
|
address.setLongAddress(mac);
|
|
}
|
|
|
|
return data;
|
|
}
|
|
|
|
[[nodiscard]] const std::uint8_t* deserializePhyHeader(const std::uint8_t* data, PhyHeader& header)
|
|
{
|
|
std::memmove(&header, data, sizeof(PhyHeader));
|
|
data += sizeof(PhyHeader);
|
|
|
|
return data;
|
|
}
|
|
|
|
[[nodiscard]] FrameStructure deserializeMacProtocolDataUnit(const std::uint8_t* data, const std::uint8_t total_frame_length)
|
|
{
|
|
FrameStructure frame;
|
|
std::memmove(&frame.frame_control_field, data, sizeof(frame.frame_control_field));
|
|
data += sizeof(frame.frame_control_field);
|
|
|
|
std::memmove(&frame.sequence_number, data, sizeof(frame.sequence_number));
|
|
data += sizeof(frame.sequence_number);
|
|
|
|
data = deserializeAddress(data, frame.frame_control_field.destination_addressing_mode, frame.destination_address);
|
|
data = deserializeAddress(data, frame.frame_control_field.source_addressing_mode, frame.source_address);
|
|
|
|
frame.payload_length = frame.calculatePayloadLength(total_frame_length);
|
|
std::memmove(frame.payload.data(), data, frame.payload_length);
|
|
data += frame.payload_length;
|
|
|
|
std::memmove(&frame.frame_checksum, data, sizeof(frame.frame_checksum));
|
|
|
|
return frame;
|
|
}
|
|
|
|
}
|
|
|
|
namespace radio::protocol
|
|
{
|
|
|
|
std::size_t composeFrameBuffer(std::uint8_t* data, const FrameStructure& frame)
|
|
{
|
|
std::uint8_t* const data_start = data;
|
|
|
|
PhyHeader header;
|
|
std::memset(&header, 0, sizeof(PhyHeader));
|
|
header.frame_length = frame.getTotalFrameLength();
|
|
data = serializePhyHeader(data, header);
|
|
|
|
data = serializeMacProtocolDataUnit(data, frame);
|
|
|
|
return data - data_start;
|
|
}
|
|
|
|
FrameStructure decomposeFrameBuffer(const std::uint8_t* data)
|
|
{
|
|
PhyHeader header;
|
|
data = deserializePhyHeader(data, header);
|
|
|
|
const std::uint8_t total_frame_length = header.frame_length;
|
|
const FrameStructure frame = deserializeMacProtocolDataUnit(data, total_frame_length);
|
|
|
|
return frame;
|
|
}
|
|
|
|
}
|