23 lines
723 B
C++
23 lines
723 B
C++
#include "Protocol.h"
|
|
#include <stddef.h>
|
|
|
|
uint16_t packetCrc(const ProtocolPacket &p) {
|
|
const uint8_t *data = reinterpret_cast<const uint8_t *>(&p);
|
|
uint16_t crc = 0xFFFF;
|
|
for (size_t i = 0; i < offsetof(ProtocolPacket, crc); ++i) {
|
|
crc ^= static_cast<uint16_t>(data[i]) << 8;
|
|
for (uint8_t b = 0; b < 8; ++b) crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : crc << 1;
|
|
}
|
|
return crc;
|
|
}
|
|
|
|
void finalizePacket(ProtocolPacket &p) {
|
|
p.magic = PROTOCOL_MAGIC; p.version = PROTOCOL_VERSION; p.crc = packetCrc(p);
|
|
}
|
|
|
|
bool validPacket(const ProtocolPacket &p) {
|
|
return p.magic == PROTOCOL_MAGIC && p.version == PROTOCOL_VERSION &&
|
|
p.type <= static_cast<uint8_t>(MessageType::ABORT) && p.crc == packetCrc(p);
|
|
}
|
|
|