mirror of https://github.com/jeelabs/esp-link.git
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
128 lines
2.7 KiB
128 lines
2.7 KiB
#include "proto.h"
|
|
|
|
int8_t ICACHE_FLASH_ATTR PROTO_Init(PROTO_PARSER *parser, PROTO_PARSE_CALLBACK *completeCallback, uint8_t *buf, uint16_t bufSize)
|
|
{
|
|
parser->buf = buf;
|
|
parser->bufSize = bufSize;
|
|
parser->dataLen = 0;
|
|
parser->callback = completeCallback;
|
|
parser->isEsc = 0;
|
|
return 0;
|
|
}
|
|
|
|
int8_t ICACHE_FLASH_ATTR PROTO_ParseByte(PROTO_PARSER *parser, uint8_t value)
|
|
{
|
|
switch(value){
|
|
case 0x7D:
|
|
parser->isEsc = 1;
|
|
break;
|
|
|
|
case 0x7E:
|
|
parser->dataLen = 0;
|
|
parser->isEsc = 0;
|
|
parser->isBegin = 1;
|
|
break;
|
|
|
|
case 0x7F:
|
|
if (parser->callback != NULL)
|
|
parser->callback();
|
|
parser->isBegin = 0;
|
|
return 0;
|
|
break;
|
|
|
|
default:
|
|
if(parser->isBegin == 0) break;
|
|
|
|
if(parser->isEsc){
|
|
value ^= 0x20;
|
|
parser->isEsc = 0;
|
|
}
|
|
|
|
if(parser->dataLen < parser->bufSize)
|
|
parser->buf[parser->dataLen++] = value;
|
|
|
|
break;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
int8_t ICACHE_FLASH_ATTR PROTO_Parse(PROTO_PARSER *parser, uint8_t *buf, uint16_t len)
|
|
{
|
|
while(len--)
|
|
PROTO_ParseByte(parser, *buf++);
|
|
return 0;
|
|
}
|
|
int16_t ICACHE_FLASH_ATTR PROTO_ParseRb(RINGBUF* rb, uint8_t *bufOut, uint16_t* len, uint16_t maxBufLen)
|
|
{
|
|
uint8_t c;
|
|
|
|
PROTO_PARSER proto;
|
|
PROTO_Init(&proto, NULL, bufOut, maxBufLen);
|
|
while(RINGBUF_Get(rb, &c) == 0){
|
|
if(PROTO_ParseByte(&proto, c) == 0){
|
|
*len = proto.dataLen;
|
|
return 0;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
int16_t ICACHE_FLASH_ATTR PROTO_Add(uint8_t *buf, const uint8_t *packet, int16_t bufSize)
|
|
{
|
|
uint16_t i = 2;
|
|
uint16_t len = *(uint16_t*) packet;
|
|
|
|
if (bufSize < 1) return -1;
|
|
|
|
*buf++ = 0x7E;
|
|
bufSize--;
|
|
|
|
while (len--) {
|
|
switch (*packet) {
|
|
case 0x7D:
|
|
case 0x7E:
|
|
case 0x7F:
|
|
if (bufSize < 2) return -1;
|
|
*buf++ = 0x7D;
|
|
*buf++ = *packet++ ^ 0x20;
|
|
i += 2;
|
|
bufSize -= 2;
|
|
break;
|
|
default:
|
|
if (bufSize < 1) return -1;
|
|
*buf++ = *packet++;
|
|
i++;
|
|
bufSize--;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (bufSize < 1) return -1;
|
|
*buf++ = 0x7F;
|
|
|
|
return i;
|
|
}
|
|
|
|
int16_t ICACHE_FLASH_ATTR PROTO_AddRb(RINGBUF *rb, const uint8_t *packet, int16_t len)
|
|
{
|
|
uint16_t i = 2;
|
|
if(RINGBUF_Put(rb, 0x7E) == -1) return -1;
|
|
while (len--) {
|
|
switch (*packet) {
|
|
case 0x7D:
|
|
case 0x7E:
|
|
case 0x7F:
|
|
if(RINGBUF_Put(rb, 0x7D) == -1) return -1;
|
|
if(RINGBUF_Put(rb, *packet++ ^ 0x20) == -1) return -1;
|
|
i += 2;
|
|
break;
|
|
default:
|
|
if(RINGBUF_Put(rb, *packet++) == -1) return -1;
|
|
i++;
|
|
break;
|
|
}
|
|
}
|
|
if(RINGBUF_Put(rb, 0x7F) == -1) return -1;
|
|
|
|
return i;
|
|
}
|
|
|
|
|