/*
* 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);
}
