Msg DisPatch

一天写了个Carlife 协议数据分流器

#include <stdio.h>
#include <string.h>

typedef unsigned char uint8_t;
typedef unsigned int uint32_t;

void print_array_data(uint8_t* data, size_t len)
{
    int i;
    char log_buffer[1000]; 
    strcpy(log_buffer," ");
    for(i=0;i<len;i++)
    {   
        char num_buffer[10];
        sprintf(num_buffer,"%02x ",data[i]);
        strncat(log_buffer,num_buffer,3);
        if(strlen(log_buffer) > 980) break;
    }   
    printf("%s
",log_buffer);
}

class MsgReceiver
{
    public:
        void processData(uint8_t channel, uint8_t* buf, size_t size)
        {
            printf("channel %d, data is ", channel);
            print_array_data(buf,size);
        }
};
    
class MsgDispatch
{
    public:
        MsgDispatch(){
            itsBufferData = new uint8_t[1024*16];
            itsCachedBufferSize = 0;
            itsMode = eParseHeader;
            itsChannel = 0;
            itsCachedHeaderSize = 0;
            itsHeaderData = new uint8_t[MSG_HEADER_SIZE];
            itsMsgRecv = new MsgReceiver;
        }
        ~MsgDispatch(){
            delete [] itsBufferData;
            delete [] itsHeaderData;
            delete itsMsgRecv;
        }
        void dispatch(uint8_t* buf, size_t size)
        {
            uint32_t curProcessIndex = 0;
            //printf("dispatch mode = %d, buffer size %lu, buffer data is ",itsMode,size);
            //print_array_data(buf,size);
            if(itsMode == eParseHeader)
            {
                if(size >= MSG_HEADER_SIZE) /* we can process header size */
                {
                    if(isValidHeader(buf))
                    {
                        uint8_t channel = buf[3];
                        uint32_t dataLen = readBig32(buf + 4);

                        curProcessIndex += MSG_HEADER_SIZE;
                        if (curProcessIndex + dataLen <= size)
                        {
                            itsMsgRecv -> processData(channel, buf + curProcessIndex, dataLen);
                            curProcessIndex += dataLen;
                        }
                        else    /* we need to cache data */
                        {
                            cacheData(buf + curProcessIndex, size - curProcessIndex, dataLen, channel);
                            itsMode = eReadData;
                            curProcessIndex = size;
                        }
                    }
                    else /* header is not correct, shift one byte */
                    {
                        curProcessIndex++;
                    }
                }
                else /* process data is less than header size, cache header */
                {
                    memcpy(itsHeaderData,buf,size);
                    itsCachedHeaderSize = size;
                    curProcessIndex = size;
                    itsMode = eReadHeader;
                }
            }
            else if (itsMode == eReadData) /* read data */
            {
                if(size >= itsNeedReadSize) /* we can read all data from current buf */
                {
                    memcpy(itsBufferData + itsCachedBufferSize, buf, itsNeedReadSize);
                    itsCachedBufferSize += itsNeedReadSize;
                    itsMsgRecv -> processData(itsChannel, itsBufferData, itsCachedBufferSize);
                    itsMode = eParseHeader;
                    curProcessIndex = itsNeedReadSize;
                }
                else /* can't read all data from current buffer, put all data to cache buffer */
                {
                    memcpy(itsBufferData + itsCachedBufferSize, buf, size);
                    itsCachedBufferSize += size;
                    itsNeedReadSize -= size;
                    curProcessIndex = size;
                }
            }
            else /* read header */
            {
                uint8_t needReadHeaderSize =  MSG_HEADER_SIZE - itsCachedHeaderSize;
                if(needReadHeaderSize < size) /* we can read header from current buffer */
                {
                    memcpy(itsHeaderData+itsCachedHeaderSize,buf,needReadHeaderSize);
                    if(isValidHeader(itsHeaderData))
                    {
                        uint8_t channel = itsHeaderData[3];
                        uint32_t dataLen = readBig32(itsHeaderData+4);

                        curProcessIndex = needReadHeaderSize;
                        if (curProcessIndex + dataLen <= size) /* we read data from current buffer */
                        {

                            itsMsgRecv -> processData(channel, buf + curProcessIndex, dataLen);
                            itsMode = eParseHeader;
                            curProcessIndex += dataLen;
                        }
                        else /* we need to cache data */
                        {
                            cacheData(buf + curProcessIndex, size - curProcessIndex, dataLen, channel);
                            curProcessIndex = size;
                            itsMode = eReadData;
                        }
                    }
                    else /* error header, shift one byte */
                    {
                        memcpy(itsHeaderData,itsHeaderData + 1,MSG_HEADER_SIZE - 1);
                        itsCachedHeaderSize = MSG_HEADER_SIZE - 1;
                        curProcessIndex = needReadHeaderSize;
                    }
                }
                else /* can't fill a header size */
                {
                    memcpy(itsHeaderData+itsCachedHeaderSize,buf,size);
                    itsCachedHeaderSize += size;
                    curProcessIndex = size;
                }
            }
            if(curProcessIndex < size)
            {
                this->dispatch(buf+curProcessIndex,size-curProcessIndex);
            }
            else { } /* data process Done */
        }
    private:
        uint32_t readBig32(uint8_t* ptr)
        {
            return ( (uint32_t)( 
                ( ( (uint32_t)( (uint8_t *)(ptr))[ 0 ] ) << 24 ) | 
                ( ( (uint32_t)( (uint8_t *)(ptr))[ 1 ] ) << 16 ) | 
                ( ( (uint32_t)( (uint8_t *)(ptr))[ 2 ] ) <<  8 ) | 
                  ( (uint32_t)( (uint8_t *)(ptr))[ 3 ] ) ) );
        }

        bool isValidHeader(uint8_t* buf)
        {
            return (buf[0] == 0x00 && buf[1] == 0x00 && buf[2] == 0x00 && buf[3] < 0x07);
        }

        void cacheData(uint8_t* buf, size_t cachedSize, size_t totalDataLen, uint8_t channel)
        {
            memcpy(itsBufferData,buf,cachedSize);
            itsCachedBufferSize = cachedSize;
            itsNeedReadSize = totalDataLen - itsCachedBufferSize;
            itsChannel = channel;
        }

        enum {
            MSG_HEADER_SIZE = 8,
        };
        enum {
            eParseHeader,
            eReadHeader,
            eReadData,
        };
        uint8_t itsChannel;
        uint8_t* itsBufferData;
        size_t itsCachedBufferSize;
        size_t itsNeedReadSize;
        uint8_t itsMode;

        uint8_t* itsHeaderData;
        uint8_t itsCachedHeaderSize;
        MsgReceiver* itsMsgRecv;
};



int main()
{
    uint8_t muti_msg_buf[] = {0x00,0x00,0x00,0x01, 0x00,0x00,0x00,0x04, 0x01,0x02,0x03,0x04, 
                        0x00,0x00,0x00,0x02, 0x00,0x00,0x00,0x02, 0x02,0x02,
                            0x00,0x00,0x00,0x01, 0x00,0x00,0x00,0x03, 0x05,0x06,0x07};
    uint8_t small_msg_buf_1[] = {0x00,0x00,0x00,0x01, 0x00,0x00,0x00,0x05, 0x09,0x08,0x07,0x06};
    uint8_t small_msg_buf_2[] = {0x05, 0x00,0x00,0x00,0x02, 0x00,0x00,0x00,0x02, 0x01,0x01};
    uint8_t small_msg_buf_3[] = {0x00,0x00,0x00,0x05, 0x00,0x00,0x00,0x0A, 0x01,0x02,0x03};
    uint8_t small_msg_buf_4[] = {0x04,0x05,0x06};
    uint8_t small_msg_buf_5[] = {0x07,0x08,0x09,0x0A,0x00};
    uint8_t small_msg_buf_6[] = {0x00,0x00,0x04, 0x00,0x00,0x00,0x03, 0x07,0x07,0x07, 0x00,0x00,0x00,0x05, 0x00,0x00,0x00,0x03};
    uint8_t small_msg_buf_7[] = {0x09,0x09,0x09};
    uint8_t error_msg_buf_1[] = {0x01,0x00,0x00,0x00,0x06, 0x00,0x00,0x00,0x01, 0x01,0x02};
    uint8_t error_msg_buf_2[] = {0x00,0x00,0x00,0x05, 0x00,0x00,0x00,0x02, 0x02,0x03};
    uint8_t error_msg_buf_3[] = {0x00,0x00,0x00,0x04, 0x00,0x00,0x00,0x01, 0x04,0x05,0x05};
    uint8_t error_msg_buf_4[] = {0x00,0x00,0x00,0x03, 0x00,0x00,0x00,0x02, 0x05,0x06};
    uint8_t error_msg_buf_5[] = {0x00,0x00,0x00,0x02, 0x00,0x00,0x00,0x01, 0x07,0x06,0x05,0x06,0x08,0x00,0x00,0x00};
    uint8_t error_msg_buf_6[] = {0x01, 0x00,0x00,0x00,0x03, 0x08,0x09,0x0A, 0x10,0x11};
    uint8_t error_msg_buf_7[] = {0x01, 0x04,0x00,0x00,0x00,0x03, 0x00,0x00,0x00,0x04, 0x0B,0x0C,0x0D,0x0E};
    uint8_t header_msg_buf_1[] = {0x01,0x00,0x00,0x00,0x06, 0x00,0x00,0x00,0x01, 0x01,0x00};
    uint8_t header_msg_buf_2[] = {0x00, 0x00, 0x01, 0x00, 0x00};
    uint8_t header_msg_buf_3[] = {0x00, 0x03, 0x01, 0x02, 0x03, 0x01,0x00,0x00,0x00,0x06, 0x00,0x00,0x00,0x01, 0x11};

    MsgDispatch mds;
    printf("test muti
");
    mds.dispatch(muti_msg_buf,sizeof(muti_msg_buf));

    printf("test small
");
    mds.dispatch(small_msg_buf_1,sizeof(small_msg_buf_1));
    mds.dispatch(small_msg_buf_2,sizeof(small_msg_buf_2));
    mds.dispatch(small_msg_buf_3,sizeof(small_msg_buf_3));
    mds.dispatch(small_msg_buf_4,sizeof(small_msg_buf_4));
    mds.dispatch(small_msg_buf_5,sizeof(small_msg_buf_5));
    mds.dispatch(small_msg_buf_6,sizeof(small_msg_buf_6));
    mds.dispatch(small_msg_buf_7,sizeof(small_msg_buf_7));

    printf("test error
");
    mds.dispatch(error_msg_buf_1,sizeof(error_msg_buf_1));
    mds.dispatch(error_msg_buf_2,sizeof(error_msg_buf_2));
    mds.dispatch(error_msg_buf_3,sizeof(error_msg_buf_3));
    mds.dispatch(error_msg_buf_4,sizeof(error_msg_buf_4));
    mds.dispatch(error_msg_buf_5,sizeof(error_msg_buf_5));
    mds.dispatch(error_msg_buf_6,sizeof(error_msg_buf_6));
    mds.dispatch(error_msg_buf_7,sizeof(error_msg_buf_7));
    printf("test header
");
    mds.dispatch(header_msg_buf_1,sizeof(header_msg_buf_1));
    mds.dispatch(header_msg_buf_2,sizeof(header_msg_buf_2));
    mds.dispatch(header_msg_buf_3,sizeof(header_msg_buf_3));
    return 0;
}

输出：

test muti
channel 1, data is  01 02 03 04 
channel 2, data is  02 02 
channel 1, data is  05 06 07 
test small
channel 1, data is  09 08 07 06 05 
channel 2, data is  01 01 
channel 5, data is  01 02 03 04 05 06 07 08 09 0a 
channel 4, data is  07 07 07 
channel 5, data is  09 09 09 
test error
channel 6, data is  01 
channel 5, data is  02 03 
channel 4, data is  04 
channel 3, data is  05 06 
channel 2, data is  07 
channel 1, data is  08 09 0a 
channel 3, data is  0b 0c 0d 0e 
test header
channel 6, data is  01 
channel 1, data is  01 02 03 
channel 6, data is  11