/* * Function to encode an an_packet */ void an_packet_encode(an_packet_t *an_packet) { uint16_t crc; an_packet->header[1] = an_packet->id; an_packet->header[2] = an_packet->length; crc = calculate_crc16(an_packet->data, an_packet->length); memcpy(&an_packet->header[3], &crc, sizeof(uint16_t)); an_packet->header[0] = calculate_header_lrc(&an_packet->header[1]); }
/* * Function to decode an_packets from raw data * Returns a pointer to the packet decoded or NULL if no packet was decoded */ an_packet_t *an_packet_decode(an_decoder_t *an_decoder) { uint16_t decode_iterator = 0; an_packet_t *an_packet = NULL; uint8_t header_lrc, id, length; uint16_t crc; while (decode_iterator + AN_PACKET_HEADER_SIZE <= an_decoder->buffer_length) { header_lrc = an_decoder->buffer[decode_iterator++]; if (header_lrc == calculate_header_lrc(&an_decoder->buffer[decode_iterator])) { id = an_decoder->buffer[decode_iterator++]; length = an_decoder->buffer[decode_iterator++]; crc = an_decoder->buffer[decode_iterator++]; crc |= an_decoder->buffer[decode_iterator++] << 8; if (decode_iterator + length > an_decoder->buffer_length) { decode_iterator -= AN_PACKET_HEADER_SIZE; break; } if (crc == calculate_crc16(&an_decoder->buffer[decode_iterator], length)) { an_packet = an_packet_allocate(length, id); if (an_packet != NULL) { memcpy(an_packet->header, &an_decoder->buffer[decode_iterator - AN_PACKET_HEADER_SIZE], AN_PACKET_HEADER_SIZE * sizeof(uint8_t)); memcpy(an_packet->data, &an_decoder->buffer[decode_iterator], length * sizeof(uint8_t)); } decode_iterator += length; break; } else { decode_iterator -= (AN_PACKET_HEADER_SIZE - 1); an_decoder->crc_errors++; } } } if (decode_iterator < an_decoder->buffer_length) { if (decode_iterator > 0) { memmove(&an_decoder->buffer[0], &an_decoder->buffer[decode_iterator], (an_decoder->buffer_length - decode_iterator) * sizeof(uint8_t)); an_decoder->buffer_length -= decode_iterator; } } else an_decoder->buffer_length = 0; return an_packet; }
bool lock_config_zone (int fd, enum DEVICE_STATE state) { if (STATE_FACTORY != state) return true; struct octet_buffer config = get_config_zone (fd); uint16_t crc = calculate_crc16 (config.ptr, config.len); return lock (fd, CONFIG_ZONE, crc); }
unsigned int serialize_command (struct Command_ATSHA204 *c, uint8_t **serialized) { unsigned int total_len = 0; unsigned int crc_len = 0; unsigned int crc_offset = 0; uint8_t *data; uint16_t *crc; assert (NULL != c); assert (NULL != serialized); total_len = sizeof (c->command) + sizeof (c->count) +sizeof (c->opcode) + sizeof (c->param1) + sizeof (c->param2) + c->data_len + sizeof (c->checksum); crc_len = total_len - sizeof (c->command) - sizeof (c->checksum); crc_offset = total_len - sizeof (c->checksum); c->count = total_len - sizeof (c->command); data = (uint8_t*)malloc (total_len); assert (NULL != data); print_command (c); CTX_LOG (DEBUG, "Total len: %d, count: %d, CRC_LEN: %d, CRC_OFFSET: %d\n", total_len, c->count, crc_len, crc_offset); /* copy over the command */ data[0] = c->command; data[1] = c->count; data[2] = c->opcode; data[3] = c->param1; data[4] = c->param2[0]; data[5] = c->param2[1]; if (c->data_len > 0) memcpy (&data[6], c->data, c->data_len); crc = (uint16_t *)&data[crc_offset]; *crc = calculate_crc16 (&data[1], crc_len); *serialized = data; return total_len; }
uint16_t crc_data_otp_zone (struct octet_buffer data, struct octet_buffer otp) { const unsigned int len = otp.len + data.len; uint8_t *buf = malloc_wipe (len); memcpy (buf, data.ptr, data.len); memcpy (buf + data.len, otp.ptr, otp.len); uint16_t crc = calculate_crc16 (buf, len); free_wipe (buf, len); return crc; }
bool is_crc_16_valid(const uint8_t *data, unsigned int data_len, const uint8_t *crc) { uint16_t result = 0; assert(NULL != data); assert(NULL != crc); print_hex_string("crc", crc, 2); result = calculate_crc16(data, data_len); print_hex_string("Calculated crc", (uint8_t*)&result, 2); return ((0 == memcmp(&result, crc, sizeof(result))) ? true : false); }