110 lines
1.7 KiB
C
110 lines
1.7 KiB
C
#include "ParsePack.h"
|
|
|
|
void Packer_init(Packer_t *pack)
|
|
{
|
|
pack->buff[0] = head1;
|
|
pack->buff[1] = head2;
|
|
}
|
|
|
|
size_t Packer_pack(Packer_t *pack, uint8_t id, uint8_t pkg[], uint32_t len)
|
|
{
|
|
size_t j;
|
|
size_t i;
|
|
uint8_t sum;
|
|
|
|
pack->buff[2] = id;
|
|
|
|
|
|
// unsigned char *pt = (unsigned char *)&len;
|
|
|
|
// pack->buff[3] = pt[0];
|
|
// pack->buff[4] = pt[1];
|
|
// pack->buff[5] = pt[2];
|
|
// pack->buff[6] = pt[3];
|
|
|
|
|
|
pack->buff[3] = (len & 0x000000FF) >> 0;
|
|
pack->buff[4] = (len & 0x0000FF00) >> 8;
|
|
pack->buff[5] = (len & 0x00FF0000) >> 16;
|
|
pack->buff[6] = (len & 0xFF000000) >> 24;
|
|
|
|
|
|
sum = 0u;
|
|
for (i=7,j=0u;j<len;++i,++j)
|
|
{
|
|
pack->buff[i] = pkg[j];
|
|
sum += pkg[j];
|
|
}
|
|
pack->buff[i++] = sum;
|
|
return i;
|
|
}
|
|
|
|
void Parser_init(Parser_t *parser)
|
|
{
|
|
parser->stage = 0;
|
|
}
|
|
|
|
|
|
int Parser_char(Parser_t *parser, uint8_t c)
|
|
{
|
|
int rslt = -1;
|
|
|
|
switch (parser->stage)
|
|
{
|
|
case 0:
|
|
if (c == head1)
|
|
{
|
|
parser->stage++;
|
|
}
|
|
break;
|
|
case 1:
|
|
if (c == head2)
|
|
{
|
|
parser->stage++;
|
|
}
|
|
else
|
|
{
|
|
parser->stage = 0;
|
|
}
|
|
break;
|
|
case 2:
|
|
parser->id = c;
|
|
parser->len_count = 0;
|
|
parser->len = 0;
|
|
parser->stage++;
|
|
break;
|
|
case 3:
|
|
|
|
parser->len += ((uint32_t)c << (8 * parser->len_count));
|
|
|
|
parser->len_count++;
|
|
if(parser->len_count >= 4)
|
|
{
|
|
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;
|
|
}
|