231 lines
3.3 KiB
C
231 lines
3.3 KiB
C
/*
|
|
* ParserPack.c
|
|
*
|
|
* Created on: Jun 24, 2020
|
|
* Author: matth
|
|
*/
|
|
|
|
#include "ParsePack.h"
|
|
|
|
static uint16_t CRC_CheckSum (uint8_t *pBuffer, uint16_t len)
|
|
{
|
|
uint16_t poly = 0x8408;
|
|
uint16_t crc = 0;
|
|
uint8_t carry;
|
|
uint8_t i_bits;
|
|
uint16_t j;
|
|
|
|
for (j = 0; j < len; j++) {
|
|
crc = (uint16_t) (crc ^ (uint8_t) pBuffer[j]);
|
|
for (i_bits = 0; i_bits < 8; i_bits++) {
|
|
carry = (uint8_t) (crc & 1);
|
|
crc = (uint16_t) (crc / 2);
|
|
if (carry) {
|
|
crc = (uint16_t) (crc ^ poly);
|
|
}
|
|
}
|
|
}
|
|
return crc;
|
|
}
|
|
|
|
|
|
void Packer_init(Packer_t *pack)
|
|
{
|
|
pack->buff[0] = 0xEB;
|
|
pack->buff[1] = 0x90;
|
|
pack->seq = 0;
|
|
}
|
|
|
|
size_t Packer_pack(Packer_t *pack, uint8_t id, uint8_t pkg[], uint16_t len)
|
|
{
|
|
uint16_t j;
|
|
size_t i;
|
|
|
|
pack->buff[2] = pack->seq++;
|
|
pack->buff[3] = id;
|
|
pack->buff[4] = (len >> 8) & 0xFF;
|
|
pack->buff[5] = len & 0xFF;
|
|
for (i=6,j=0u;j<len;++i,++j)
|
|
{
|
|
pack->buff[i] = pkg[j];
|
|
}
|
|
uint16_t crc16 = CRC_CheckSum(&pack->buff[6], len);
|
|
pack->buff[i++] = (crc16 & 0xFF);
|
|
pack->buff[i++] = (crc16 >> 8)&0xFF;
|
|
return i;
|
|
}
|
|
|
|
void Parser_init(Parser_t *parser)
|
|
{
|
|
parser->stage = 0;
|
|
parser->crc_err_cnt = 0;
|
|
parser->head_err_cnt = 0;
|
|
}
|
|
|
|
int Parser_char(Parser_t *parser, uint8_t c)
|
|
{
|
|
int rslt = -1;
|
|
|
|
switch (parser->stage)
|
|
{
|
|
case 0:
|
|
if (c == 0xEB)
|
|
{
|
|
parser->stage++;
|
|
}
|
|
else
|
|
{
|
|
parser->head_err_cnt++;
|
|
}
|
|
break;
|
|
case 1:
|
|
if (c == 0x90)
|
|
{
|
|
parser->stage++;
|
|
}
|
|
else
|
|
{
|
|
parser->stage = 0;
|
|
parser->head_err_cnt++;
|
|
}
|
|
break;
|
|
case 2:
|
|
parser->seq = c;
|
|
parser->stage++;
|
|
break;
|
|
case 3:
|
|
parser->id = c;
|
|
parser->stage++;
|
|
break;
|
|
case 4:
|
|
parser->len = c;
|
|
parser->stage++;
|
|
break;
|
|
case 5:
|
|
parser->len = (parser->len << 8) + c;
|
|
if (parser->len <=PARSER_BUFF_LEN)
|
|
{
|
|
parser->idx = 0;
|
|
parser->stage++;
|
|
}
|
|
else
|
|
{
|
|
parser->stage = 0;
|
|
}
|
|
break;
|
|
case 6:
|
|
parser->buff[parser->idx++] = c;
|
|
if (parser->idx == parser->len)
|
|
{
|
|
parser->stage++;
|
|
}
|
|
break;
|
|
case 7:
|
|
parser->crc16 = c;
|
|
parser->stage++;
|
|
break;
|
|
case 8:
|
|
parser->crc16 += (c<<8);
|
|
if (parser->crc16 == CRC_CheckSum(parser->buff, parser->len))
|
|
{
|
|
rslt = parser->id;
|
|
}
|
|
else
|
|
{
|
|
parser->crc_err_cnt++;
|
|
}
|
|
parser->stage = 0;
|
|
break;
|
|
default:
|
|
parser->stage = 0;
|
|
break;
|
|
}
|
|
|
|
return rslt;
|
|
}
|
|
|
|
void Packer2_init(Packer2_t *pack)
|
|
{
|
|
pack->buff[0] = 0xEB;
|
|
pack->buff[1] = 0x90;
|
|
}
|
|
|
|
size_t Packer2_pack(Packer2_t *pack, uint8_t id, uint8_t pkg[], uint8_t len)
|
|
{
|
|
uint8_t j;
|
|
size_t i;
|
|
uint8_t sum;
|
|
|
|
pack->buff[2] = id;
|
|
pack->buff[3] = len;
|
|
sum = 0u;
|
|
for (i=4,j=0u;j<len;++i,++j)
|
|
{
|
|
pack->buff[i] = pkg[j];
|
|
sum += pkg[j];
|
|
}
|
|
pack->buff[i++] = sum;
|
|
return i;
|
|
}
|
|
|
|
void Parser2_init(Parser2_t *parser)
|
|
{
|
|
parser->stage = 0;
|
|
}
|
|
|
|
|
|
int Parser2_char(Parser2_t *parser, uint8_t c)
|
|
{
|
|
int rslt = -1;
|
|
|
|
switch (parser->stage)
|
|
{
|
|
case 0:
|
|
if (c == 0xEB)
|
|
{
|
|
parser->stage++;
|
|
}
|
|
break;
|
|
case 1:
|
|
if (c == 0x90)
|
|
{
|
|
parser->stage++;
|
|
}
|
|
else
|
|
{
|
|
parser->stage = 0;
|
|
}
|
|
break;
|
|
case 2:
|
|
parser->id = c;
|
|
parser->stage++;
|
|
break;
|
|
case 3:
|
|
parser->len = c;
|
|
parser->stage++;
|
|
parser->idx = 0;
|
|
parser->sum = 0;
|
|
break;
|
|
case 4:
|
|
parser->buff[parser->idx++] = c;
|
|
parser->sum += c;
|
|
if (parser->idx == parser->len)
|
|
{
|
|
parser->stage++;
|
|
}
|
|
break;
|
|
case 5:
|
|
if (parser->sum == c)
|
|
{
|
|
rslt = parser->id;
|
|
}
|
|
parser->stage = 0;
|
|
break;
|
|
default:
|
|
parser->stage = 0;
|
|
break;
|
|
}
|
|
|
|
return rslt;
|
|
}
|