kunlun/common/utils/iot_frame_parse.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;
}