126 lines
1.9 KiB
C
126 lines
1.9 KiB
C
#include "packet.h"
|
|
|
|
#include <stdint.h>
|
|
|
|
#include "cpustatusui.h"
|
|
#include "decoder.h"
|
|
#include "protocol.h"
|
|
|
|
|
|
void encoder_build_packet(
|
|
Encoder *enc,
|
|
uint8_t *data,
|
|
uint8_t length)
|
|
{
|
|
uint32_t p=0;
|
|
|
|
|
|
memcpy(
|
|
&enc->packet[p],
|
|
PACKET_MAGIC,
|
|
MAGIC_SIZE);
|
|
|
|
p+=MAGIC_SIZE;
|
|
|
|
|
|
enc->packet[p++]=length;
|
|
|
|
|
|
memcpy(
|
|
&enc->packet[p],
|
|
data,
|
|
length);
|
|
|
|
p+=length;
|
|
|
|
|
|
uint16_t crc =
|
|
crc16(
|
|
&enc->packet[0],
|
|
p);
|
|
|
|
|
|
enc->packet[p++]=crc&0xff;
|
|
enc->packet[p++]=crc>>8;
|
|
|
|
|
|
enc->packetSize=p;
|
|
}
|
|
|
|
bool decoder_parse_packet(Decoder *dec)
|
|
{
|
|
if(dec->bufferSize < MAGIC_SIZE + 1 + 2)
|
|
return false;
|
|
|
|
if(memcmp(dec->buffer,
|
|
PACKET_MAGIC,
|
|
MAGIC_SIZE))
|
|
{
|
|
printf("Bad magic\n");
|
|
return false;
|
|
}
|
|
|
|
uint8_t length =
|
|
dec->buffer[MAGIC_SIZE];
|
|
|
|
uint32_t packetSize =
|
|
MAGIC_SIZE +
|
|
1 +
|
|
length +
|
|
2;
|
|
|
|
if(dec->bufferSize < packetSize)
|
|
return false;
|
|
|
|
uint16_t received =
|
|
dec->buffer[packetSize-2] |
|
|
(dec->buffer[packetSize-1] << 8);
|
|
|
|
uint16_t calculated =
|
|
crc16(dec->buffer,
|
|
packetSize-2);
|
|
|
|
if(received != calculated)
|
|
{
|
|
printf("CRC FAIL\n");
|
|
return true;
|
|
}
|
|
|
|
printf("PACKET OK len=%d\n", length);
|
|
|
|
printf("Payload:\n");
|
|
|
|
for(int i=0;i<length;i++)
|
|
printf("%02X ",
|
|
dec->buffer[MAGIC_SIZE+1+i]);
|
|
|
|
printf("\n");
|
|
|
|
return true;
|
|
}
|
|
|
|
uint16_t crc16(
|
|
const uint8_t *data,
|
|
uint32_t len)
|
|
{
|
|
uint16_t crc = 0xFFFF;
|
|
|
|
|
|
for(uint32_t i=0;i<len;i++)
|
|
{
|
|
crc ^= data[i];
|
|
|
|
|
|
for(int b=0;b<8;b++)
|
|
{
|
|
if(crc & 1)
|
|
crc =
|
|
(crc >> 1) ^ 0xA001;
|
|
else
|
|
crc >>= 1;
|
|
}
|
|
}
|
|
|
|
|
|
return crc;
|
|
} |