初始提交
This commit is contained in:
300
common/utils/iot_frame_parse.c
Normal file
300
common/utils/iot_frame_parse.c
Normal file
@@ -0,0 +1,300 @@
|
||||
#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;
|
||||
}
|
||||
Reference in New Issue
Block a user