301 lines
9.4 KiB
C
301 lines
9.4 KiB
C
#include "iot_frame_parse.h"
|
|
#include "iot_io.h"
|
|
#include "iot_utils.h"
|
|
|
|
void* find_str(const uint8_t *pSour, const uint8_t *pDest,
|
|
int SourLen, int DestLen)
|
|
{
|
|
int i = 0, j = 0;
|
|
while (i < SourLen && j < DestLen) {
|
|
if (*(pSour + i) == *(pDest + j)) {
|
|
i++;
|
|
j++;
|
|
} else {
|
|
i = i - j + 1;
|
|
j = 0;
|
|
}
|
|
}
|
|
|
|
if (j == DestLen) {
|
|
return (void*)(pSour + (i - DestLen));
|
|
} else {
|
|
return NULL;
|
|
}
|
|
}
|
|
|
|
|
|
void* find_str_ringbuf(ringbuf_t *r, const uint8_t *pDest,
|
|
uint32_t DestLen)
|
|
{
|
|
uint32_t SourLen = iot_ringbuf_elements(r);
|
|
uint32_t i = 0, j = 0;
|
|
uint8_t c;
|
|
uint32_t get_ptr = r->get_ptr;
|
|
|
|
if (!r || !pDest){
|
|
IOT_ASSERT(0);
|
|
return NULL;
|
|
}
|
|
while (i < SourLen && j < DestLen) {
|
|
c = r->data[get_ptr];
|
|
get_ptr = iot_get_ring_pos((get_ptr + 1), r);
|
|
if (c == *(pDest + j)) {
|
|
i++;
|
|
j++;
|
|
} else {
|
|
i = i - j + 1;
|
|
get_ptr = get_ptr - j;
|
|
j = 0;
|
|
}
|
|
}
|
|
|
|
if (j == DestLen) {
|
|
return (void*)(r->data+ iot_get_ring_pos((r->get_ptr + i - DestLen), r));
|
|
} else {
|
|
return NULL;
|
|
}
|
|
}
|
|
|
|
|
|
//uint32_t has_full_frame(uint8_t* buffer, uint32_t* bufferlen, iot_frame_fmt fmt)
|
|
//{
|
|
// uint8_t *pdest = NULL;
|
|
// unsigned long nFrameEnd;
|
|
//
|
|
////framebegin:
|
|
// pdest = (uint8_t*)find_str(buffer, (uint8_t*)fmt.preamble_code, *bufferlen, fmt.preamle_codelen);
|
|
// if (pdest)
|
|
// {
|
|
// unsigned long nFrameStart = (unsigned long)(pdest - buffer);// move buffer to the beginning.
|
|
// os_mem_cpy(buffer, buffer + nFrameStart, *bufferlen-nFrameStart);
|
|
// os_mem_set(buffer + *bufferlen-nFrameStart, 0, nFrameStart);
|
|
// *bufferlen = *bufferlen - nFrameStart;
|
|
// }
|
|
// else
|
|
// return 0;
|
|
////frameend:
|
|
// pdest = (uint8_t*)find_str(buffer, (uint8_t*)fmt.backcode, *bufferlen, fmt.backcode_Len);
|
|
// if (pdest)
|
|
// {
|
|
// nFrameEnd = (unsigned long)(pdest - buffer + fmt.backcode_Len);
|
|
// //printf("found @@, (nCurrectPos=%d, dwRealRead=%d)\n", nCurrectPos, dwRealRead);
|
|
//
|
|
// return nFrameEnd;
|
|
// }
|
|
// else
|
|
// {
|
|
// return 0;
|
|
// }
|
|
//}
|
|
|
|
uint32_t has_full_frame(uint8_t* buffer, uint32_t* bufferlen, iot_frame_fmt fmt,
|
|
bool_t* has_preamble_code)
|
|
{
|
|
uint8_t *pdest = NULL;
|
|
//datalen size cannot be large than 4
|
|
IOT_ASSERT(fmt.datalen_size <= 4);
|
|
|
|
if (has_preamble_code) {
|
|
*has_preamble_code = false;
|
|
}
|
|
|
|
framebegin:
|
|
if (fmt.preamble_codelen > 0) {
|
|
pdest = (uint8_t*)find_str(buffer, (uint8_t*)fmt.preamble_code,
|
|
*bufferlen, fmt.preamble_codelen);
|
|
if (pdest) {
|
|
// move buffer to the beginning.
|
|
unsigned long nFrameStart = (unsigned long)(pdest - buffer);
|
|
if (nFrameStart > 0) {
|
|
os_mem_cpy(buffer, buffer + nFrameStart, *bufferlen - nFrameStart);
|
|
os_mem_set(buffer + *bufferlen - nFrameStart, 0, nFrameStart);
|
|
*bufferlen = *bufferlen - nFrameStart;
|
|
}
|
|
|
|
if (has_preamble_code) {
|
|
*has_preamble_code = true;
|
|
}
|
|
} else {
|
|
if (*bufferlen) {
|
|
iot_printf("has_full_frame func didn't find the preamble\n");
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
uint32_t len = 0;
|
|
uint8_t backcode[MAX_FRAME_CODE_LEN] = { 0 };
|
|
if ((uint32_t)(fmt.preamble_codelen + fmt.datalen_offset ) > *bufferlen) {
|
|
// need receive more data
|
|
iot_printf("has_full_frame func need get more buffer for buffer len\n");
|
|
return 0;
|
|
}
|
|
|
|
os_mem_cpy(&len, buffer+ fmt.preamble_codelen + fmt.datalen_offset,
|
|
fmt.datalen_size);
|
|
if (fmt.datalen_endian_mode == DATALEN_ENDIAN_BIG) {
|
|
iot_data_reverse((uint8_t *)&len, fmt.datalen_size);
|
|
}
|
|
if (fmt.datalen_mask) {
|
|
len &= fmt.datalen_mask;
|
|
}
|
|
|
|
//after get the len, try to get the back code...
|
|
len = fmt.preamble_codelen + fmt.datalen_offset + fmt.datalen_size +
|
|
len + fmt.backcode_offset; //get back code start position
|
|
len -= fmt.datalen_fix;
|
|
|
|
if ((len + fmt.backcode_len) > *bufferlen) {
|
|
iot_printf("has_full_frame func need get more buffer for back code\n");
|
|
return 0;
|
|
}
|
|
|
|
if (fmt.backcode_len == 0) {
|
|
iot_printf("has_full_frame func got a full buffer, no backcode\n");
|
|
return len;
|
|
}
|
|
os_mem_cpy(backcode, buffer + len , fmt.backcode_len);
|
|
if (os_mem_cmp(backcode, fmt.backcode, fmt.backcode_len) == 0) {
|
|
iot_printf("has_full_frame func got a full buffer\n");
|
|
return len + fmt.backcode_len;
|
|
} else {
|
|
// drop the data till next preamle code
|
|
iot_printf("has_full_frame func the backcode is not match\n");
|
|
pdest = (uint8_t*)find_str(buffer+ fmt.preamble_codelen,
|
|
(uint8_t*)fmt.preamble_code, *bufferlen, fmt.preamble_codelen);
|
|
if (pdest) {
|
|
unsigned long nFrameStart = (unsigned long)(pdest - buffer);// move buffer to the beginning.
|
|
if (nFrameStart > 0) {
|
|
os_mem_cpy(buffer, buffer + nFrameStart, *bufferlen - nFrameStart);
|
|
os_mem_set(buffer + *bufferlen - nFrameStart, 0, nFrameStart);
|
|
*bufferlen = *bufferlen - nFrameStart;
|
|
}
|
|
|
|
goto framebegin;
|
|
} else {
|
|
os_mem_set(buffer, 0, *bufferlen);
|
|
*bufferlen = 0;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
uint32_t has_full_frame_ringbuf(ringbuf_t *r, iot_frame_fmt fmt,
|
|
uint32_t *ec_len)
|
|
{
|
|
uint32_t preamble_offest = 0;
|
|
uint8_t *preamble_pos = NULL;
|
|
uint32_t invalid_data_len, valid_data_len;
|
|
uint32_t len_d = 0, backcode_field_offset;
|
|
uint8_t backcode[MAX_FRAME_CODE_LEN] = { 0 };
|
|
/* according to the frame format, the offset of the length field
|
|
* in the data frame is calculated
|
|
*/
|
|
uint32_t len_field_offset = fmt.preamble_codelen + fmt.datalen_offset;
|
|
ringbuf_t ringbuf_st;
|
|
bool_t push = false;
|
|
/* datalen size cannot be large than 4 */
|
|
IOT_ASSERT(fmt.datalen_size <= 4);
|
|
if (!r || !fmt.preamble_codelen
|
|
|| !iot_ringbuf_elements(r))
|
|
goto out;
|
|
again:
|
|
preamble_pos = (uint8_t*)find_str_ringbuf(r, (uint8_t*)fmt.preamble_code,
|
|
fmt.preamble_codelen);
|
|
if (push) {
|
|
os_mem_cpy(r, &ringbuf_st, sizeof(ringbuf_t));
|
|
}
|
|
if (!preamble_pos) {
|
|
/* if no preamble is found */
|
|
goto out;
|
|
}
|
|
/* calculated the data length before the preamble code */
|
|
invalid_data_len = (uint32_t)iot_get_ring_pos(\
|
|
(preamble_pos - (r->data + r->get_ptr)), r);
|
|
/* calculated the data length after the preamble code, included Preamble */
|
|
valid_data_len = (uint32_t)iot_get_ring_pos(\
|
|
((r->put_ptr + r->data) - preamble_pos), r);
|
|
if ((uint32_t)(fmt.preamble_codelen + fmt.datalen_offset) > valid_data_len)
|
|
{
|
|
goto out;
|
|
}
|
|
/* calculated the offset of the preamble in the ringbuffer */
|
|
preamble_offest = (uint32_t)(preamble_pos - r->data);
|
|
|
|
/* gets the length of the data field */
|
|
iot_ringbuf_dummy_gets(r, (uint8_t *)&len_d, fmt.datalen_size,
|
|
iot_ringbuf_offset_add(r, preamble_offest, len_field_offset));
|
|
|
|
if (fmt.datalen_endian_mode == DATALEN_ENDIAN_BIG) {
|
|
iot_data_reverse((uint8_t *)&len_d, fmt.datalen_size);
|
|
}
|
|
if (fmt.datalen_mask) {
|
|
len_d &= fmt.datalen_mask;
|
|
}
|
|
/* calculate the offset of back code in the data frame */
|
|
backcode_field_offset = fmt.preamble_codelen + fmt.datalen_offset +
|
|
fmt.datalen_size + len_d + fmt.backcode_offset - fmt.datalen_fix;
|
|
|
|
if ((backcode_field_offset + fmt.backcode_len) > valid_data_len) {
|
|
goto try;
|
|
}
|
|
if (fmt.backcode_len == 0) {
|
|
iot_printf("has_full_frame func got a full buffer, no backcode\n");
|
|
(*ec_len) = invalid_data_len;
|
|
return backcode_field_offset;
|
|
}
|
|
iot_ringbuf_dummy_gets(r, backcode, fmt.backcode_len,
|
|
iot_ringbuf_offset_add(r, preamble_offest, backcode_field_offset));
|
|
|
|
if (os_mem_cmp(backcode, fmt.backcode, fmt.backcode_len) == 0) {
|
|
iot_printf("has_full_frame func got a full buffer\n");
|
|
(*ec_len) = invalid_data_len;
|
|
return (backcode_field_offset + fmt.backcode_len);
|
|
}
|
|
/* If the preamble is found, it is unsuccessful to try the
|
|
* frame format check, and find the next preamble.
|
|
*/
|
|
try:
|
|
os_mem_cpy(&ringbuf_st, r, sizeof(ringbuf_t));
|
|
iot_ringbuf_drop(r,
|
|
iot_get_ring_pos((iot_ringbuf_offset_add(r, preamble_offest, \
|
|
fmt.preamble_codelen) - r->get_ptr), r));
|
|
push = true;
|
|
goto again;
|
|
|
|
out:
|
|
return 0;
|
|
}
|
|
|
|
bool_t build_frame_buf(iot_pkt_t* buffer, iot_frame_fmt fmt)
|
|
{
|
|
if (!buffer) {
|
|
return false;
|
|
}
|
|
|
|
if (fmt.preamble_codelen > 0) {
|
|
uint8_t * newdata = iot_pkt_push(buffer, fmt.preamble_codelen);
|
|
if (newdata) {
|
|
os_mem_cpy(newdata, fmt.preamble_code, fmt.preamble_codelen);
|
|
} else {
|
|
IOT_ASSERT(0);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
if (fmt.backcode_len > 0) {
|
|
uint8_t * newdata = iot_pkt_put(buffer, fmt.backcode_len);
|
|
if (newdata) {
|
|
os_mem_cpy(newdata + (iot_pkt_data_len(buffer) - fmt.backcode_len),
|
|
fmt.backcode, fmt.backcode_len);
|
|
} else {
|
|
IOT_ASSERT(0);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|