protocol.c (1703B)
1 #include "protocol.h" 2 #include "shared/crc16.h" 3 #include <stdbool.h> 4 5 static bool protocol_validate_packet(uint8_t *packet, bool isFirmware) { 6 if (isFirmware && crc32_calculate((uint8_t *)packet, packet[PACK_LENGTH_INDEX] + 6) == 0) { 7 return true; 8 } 9 if (!isFirmware && crc16_calculate((uint8_t *)packet, packet[PACK_LENGTH_INDEX] + 4) == 0) { 10 return true; 11 } 12 return false; 13 } 14 15 ProtocolPacketAttr_t protocol_extract_packet_attr(uint8_t *packet, bool isFirmware) { 16 ProtocolPacketAttr_t attr = { 17 .offset = 0, 18 .length = 0, 19 }; 20 if (isFirmware) { 21 if (protocol_validate_packet(packet, isFirmware)) { 22 attr.tag = packet[PACK_TAG_INDEX]; 23 attr.length = packet[PACK_LENGTH_INDEX]; 24 } else attr.tag = PROTOCOL_TAG_INVALID; 25 return attr; 26 } 27 if (!protocol_validate_packet(packet, isFirmware)) { 28 attr.tag = PROTOCOL_TAG_INVALID; 29 return attr; 30 } 31 attr.tag = packet[PACK_TAG_INDEX]; 32 if (attr.tag == PROTOCOL_TAG_PW) { 33 attr.offset = 0; 34 attr.length = packet[PACK_LENGTH_INDEX]; 35 return attr; 36 } 37 attr.offset = packet[PACK_DATA_INDEX]; 38 attr.length = packet[PACK_DATA_INDEX + 1]; 39 return attr; 40 } 41 42 void protocol_create_packet(uint8_t *buffer, ProtocolTag_t tag, uint8_t length) { 43 buffer[PACK_TAG_INDEX] = tag; 44 buffer[PACK_LENGTH_INDEX] = length; 45 uint16_t crcMsbIndex = length + 4 - 2; 46 uint16_t crcLsbIndex = length + 4 - 1; 47 buffer[crcMsbIndex] = 0; 48 buffer[crcLsbIndex] = 0; 49 uint16_t crcVal = crc16_calculate(buffer, length + 4); 50 buffer[crcMsbIndex] = (crcVal >> 8 ) & 0xFF; 51 buffer[crcLsbIndex] = crcVal & 0xFF; 52 }