diff --git a/include/BufferFiller.h b/include/BufferFiller.h index 7f7db41..143a7ba 100644 --- a/include/BufferFiller.h +++ b/include/BufferFiller.h @@ -19,11 +19,11 @@ private: void setPacketSize(uint8_t *buffer, uint8_t); void setConstants(uint8_t *buffer); - void setSourceAddress(uint8_t *buffer, uint16_t); - void setDestinationAddress(uint8_t *buffer, uint16_t); + void setSourceAddress(uint8_t *buffer, uint8_t offset, uint16_t source); + void setDestinationAddress(uint8_t *buffer, uint8_t offset, uint16_t destination); void setRollingCodes(uint8_t *buffer, uint8_t rollingCode1, uint8_t rollingCode2); void setProtocolVersion(uint8_t *buffer, uint8_t); - void setFieldsData(uint8_t *buffer, const FieldsParameters& parameters); + void setFieldsData(uint8_t *buffer, uint8_t offset, const FieldsParameters& parameters); void calculateCRC(uint8_t *buffer); void incrementRollingCodes(); uint8_t calculateTotalFieldSize(const FieldsParameters& parameters); diff --git a/include/PacketTypes.h b/include/PacketTypes.h index 55b6837..5b48f70 100644 --- a/include/PacketTypes.h +++ b/include/PacketTypes.h @@ -34,11 +34,25 @@ struct FieldsParameters { using PacketParameters = std::variant; +struct BroadcastHeader { + uint16_t source; +}; -// Define Message structure -struct Packet { +struct UnicastHeader { uint16_t source; uint16_t destination; +}; + +struct GroupsHeader { + uint16_t source; + std::vector groups; +}; + +using PacketHeader = std::variant; + +// Define Packet structure +struct Packet { + PacketHeader header; PacketType type; PacketParameters parameters; uint8_t rollingCode1; diff --git a/src/BufferFiller.cpp b/src/BufferFiller.cpp index dd9175c..11ff454 100644 --- a/src/BufferFiller.cpp +++ b/src/BufferFiller.cpp @@ -11,45 +11,68 @@ BufferFiller::~BufferFiller() bool BufferFiller::fill(uint8_t *buffer, const Packet* packet) { setConstants(buffer); - setProtocolVersion(buffer, protocolVersion); - setSourceAddress(buffer, packet->source); - setDestinationAddress(buffer, packet->destination); + + int dataOffset = -1; + + if (std::holds_alternative(packet->header)) { + setProtocolVersion(buffer, 0x04); + auto header = std::get(packet->header); + setSourceAddress(buffer, 12, header.source); + dataOffset = 14; + } else if (std::holds_alternative(packet->header)) { + setProtocolVersion(buffer, 0x05); + auto header = std::get(packet->header); + setDestinationAddress(buffer, 12, header.destination); + setSourceAddress(buffer, 14, header.source); + dataOffset = 16; + } else if (std::holds_alternative(packet->header)) { + setProtocolVersion(buffer, 0x06); + auto header = std::get(packet->header); + int groupsOffset = 12; + for (size_t i = 0; i < header.groups.size(); i++) { + buffer[groupsOffset] = header.groups[i]; + groupsOffset++; + } + buffer[groupsOffset] = 0x00; + setSourceAddress(buffer, groupsOffset + 1, header.source); + dataOffset = groupsOffset + 3; + } switch(packet->type) { case PacketType::STOP: setPacketSize(buffer, 0x11); - buffer[16] = 0x52; - buffer[17] = 0x53; - buffer[18] = 0x00; + buffer[dataOffset + 0] = 0x52; + buffer[dataOffset + 1] = 0x53; + buffer[dataOffset + 2] = 0x00; break; case PacketType::CLOSE: setPacketSize(buffer, 0x11); - buffer[16] = 0x52; - buffer[17] = 0x44; - buffer[18] = 0x00; + buffer[dataOffset + 0] = 0x52; + buffer[dataOffset + 1] = 0x44; + buffer[dataOffset + 2] = 0x00; break; case PacketType::OPEN: setPacketSize(buffer, 0x11); - buffer[16] = 0x52; - buffer[17] = 0x55; - buffer[18] = 0x00; + buffer[dataOffset + 0] = 0x52; + buffer[dataOffset + 1] = 0x55; + buffer[dataOffset + 2] = 0x00; break; case PacketType::FIELDS: { FieldsParameters parameters = std::get(packet->parameters); // 0x10 is the number of bytes without any fields setPacketSize(buffer, 0x10 + calculateTotalFieldSize(parameters)); - buffer[16] = 0x21; - buffer[17] = 0x5A; - setFieldsData(buffer, parameters); + buffer[dataOffset + 0] = 0x21; + buffer[dataOffset + 1] = 0x5A; + setFieldsData(buffer, dataOffset + 2, parameters); break; } case PacketType::FIELD_COMMAND: { FieldsParameters parameters = std::get(packet->parameters); // 0x10 is the number of bytes without any fields setPacketSize(buffer, 0x10 + calculateTotalFieldSize(parameters)); - buffer[16] = 0x3F; - buffer[17] = 0x5A; - setFieldsData(buffer, std::get(packet->parameters)); + buffer[dataOffset + 0] = 0x3F; + buffer[dataOffset + 1] = 0x5A; + setFieldsData(buffer, dataOffset + 2, parameters); break; } default: @@ -62,9 +85,7 @@ bool BufferFiller::fill(uint8_t *buffer, const Packet* packet) { return true; } -void BufferFiller::setFieldsData(uint8_t *buffer, const FieldsParameters& parameters) { - uint8_t offset = 18; - +void BufferFiller::setFieldsData(uint8_t *buffer, uint8_t offset, const FieldsParameters& parameters) { for (size_t i = 0; i < parameters.fields.size(); i++) { Field field = parameters.fields[i]; uint8_t fieldSize = calculateFieldSize(field); @@ -113,20 +134,20 @@ void BufferFiller::setConstants(uint8_t *buffer) { buffer[9] = 0x86; // Constant? } -void BufferFiller::setSourceAddress(uint8_t *buffer, uint16_t sourceID) { +void BufferFiller::setSourceAddress(uint8_t *buffer, uint8_t offset, uint16_t sourceID) { // Physical source address (could be the address of a repeater when receiving a packet) buffer[7] = (uint8_t)((sourceID & 0xFF00) >> 8); buffer[8] = (uint8_t)(sourceID & 0x00FF); // Logical source address (usually the same as the physical source address) - buffer[14] = (uint8_t)((sourceID & 0xFF00) >> 8); - buffer[15] = (uint8_t)(sourceID & 0x00FF); + buffer[offset + 0] = (uint8_t)((sourceID & 0xFF00) >> 8); + buffer[offset + 1] = (uint8_t)(sourceID & 0x00FF); } -void BufferFiller::setDestinationAddress(uint8_t *buffer, uint16_t targetID) { +void BufferFiller::setDestinationAddress(uint8_t *buffer, uint8_t offset, uint16_t targetID) { // Logical target address - buffer[12] = (uint8_t)((targetID & 0xFF00) >> 8); - buffer[13] = (uint8_t)(targetID & 0x00FF); + buffer[offset + 0] = (uint8_t)((targetID & 0xFF00) >> 8); + buffer[offset + 1] = (uint8_t)(targetID & 0x00FF); } void BufferFiller::setRollingCodes(uint8_t *buffer, uint8_t rollingCode1, uint8_t rollingCode2) { diff --git a/src/PacketParser.cpp b/src/PacketParser.cpp index 482af5b..fe5216b 100644 --- a/src/PacketParser.cpp +++ b/src/PacketParser.cpp @@ -10,27 +10,56 @@ PacketParser::~PacketParser() bool PacketParser::parsePacket(const uint8_t *buffer, Packet& packet) { - packet.source = (uint16_t)(buffer[14] << 8 | buffer[15]); - packet.destination = (uint16_t)(buffer[12] << 8 | buffer[13]); + int dataOffset = -1; + if (buffer[10] == 0x04) { + BroadcastHeader header{}; + header.source = (uint16_t)(buffer[12] << 8 | buffer[13]); + packet.header = header; + dataOffset = 14; + } else if(buffer[10] == 0x05) { + UnicastHeader header{}; + header.destination = (uint16_t)(buffer[12] << 8 | buffer[13]); + header.source = (uint16_t)(buffer[14] << 8 | buffer[15]); + packet.header = header; + dataOffset = 16; + } else if (buffer[10] == 0x06) { + GroupsHeader header{}; + size_t i = 12; + uint8_t length = buffer[1] + 2; + while (i < length) { + if (buffer[i] == 0x00) { + dataOffset = i + 3; + break; + } else { + header.groups.push_back(buffer[i]); + } + i++; + } + if (i == length) { + return false; + } + header.source = (uint16_t)(buffer[i + 1] << 8 | buffer[i + 2]); + packet.header = header; + } packet.rollingCode1 = buffer[4]; packet.rollingCode2 = buffer[11]; - if (buffer[16] == 0x52 && buffer[17] == 0x53) { + if (buffer[dataOffset + 0] == 0x52 && buffer[dataOffset + 1] == 0x53) { packet.type = PacketType::STOP; packet.parameters = std::monostate{}; - } else if (buffer[16] == 0x52 && buffer[17] == 0x44) { + } else if (buffer[dataOffset + 0] == 0x52 && buffer[dataOffset + 1] == 0x44) { packet.type = PacketType::CLOSE; packet.parameters = std::monostate{}; - } else if (buffer[16] == 0x52 && buffer[17] == 0x55) { + } else if (buffer[dataOffset + 0] == 0x52 && buffer[dataOffset + 1] == 0x55) { packet.type = PacketType::OPEN; packet.parameters = std::monostate{}; - } else if (buffer[16] == 0x21 && buffer[17] == 0x5A) { + } else if (buffer[dataOffset + 0] == 0x21 && buffer[dataOffset + 1] == 0x5A) { packet.type = PacketType::FIELDS; std::vector fields; parseFields(buffer, fields); packet.parameters = FieldsParameters{fields}; - } else if (buffer[16] == 0x3F && buffer[17] == 0x5A) { + } else if (buffer[dataOffset + 0] == 0x3F && buffer[dataOffset + 1] == 0x5A) { packet.type = PacketType::FIELD_COMMAND; std::vector fields; parseFields(buffer, fields);