Msg DisPatch

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

  1 #include <stdio.h>
  2 #include <string.h>
  3 
  4 typedef unsigned char uint8_t;
  5 typedef unsigned int uint32_t;
  6 
  7 void print_array_data(uint8_t* data, size_t len)
  8 {
  9     int i;
 10     char log_buffer[1000]; 
 11     strcpy(log_buffer," ");
 12     for(i=0;i<len;i++)
 13     {   
 14         char num_buffer[10];
 15         sprintf(num_buffer,"%02x ",data[i]);
 16         strncat(log_buffer,num_buffer,3);
 17         if(strlen(log_buffer) > 980) break;
 18     }   
 19     printf("%s
",log_buffer);
 20 }
 21 
 22 class MsgReceiver
 23 {
 24     public:
 25         void processData(uint8_t channel, uint8_t* buf, size_t size)
 26         {
 27             printf("channel %d, data is ", channel);
 28             print_array_data(buf,size);
 29         }
 30 };
 31     
 32 class MsgDispatch
 33 {
 34     public:
 35         MsgDispatch(){
 36             itsBufferData = new uint8_t[1024*16];
 37             itsCachedBufferSize = 0;
 38             itsMode = eParseHeader;
 39             itsChannel = 0;
 40             itsCachedHeaderSize = 0;
 41             itsHeaderData = new uint8_t[MSG_HEADER_SIZE];
 42             itsMsgRecv = new MsgReceiver;
 43         }
 44         ~MsgDispatch(){
 45             delete [] itsBufferData;
 46             delete [] itsHeaderData;
 47             delete itsMsgRecv;
 48         }
 49         void dispatch(uint8_t* buf, size_t size)
 50         {
 51             uint32_t curProcessIndex = 0;
 52             //printf("dispatch mode = %d, buffer size %lu, buffer data is ",itsMode,size);
 53             //print_array_data(buf,size);
 54             if(itsMode == eParseHeader)
 55             {
 56                 if(size >= MSG_HEADER_SIZE) /* we can process header size */
 57                 {
 58                     if(isValidHeader(buf))
 59                     {
 60                         uint8_t channel = buf[3];
 61                         uint32_t dataLen = readBig32(buf + 4);
 62 
 63                         curProcessIndex += MSG_HEADER_SIZE;
 64                         if (curProcessIndex + dataLen <= size)
 65                         {
 66                             itsMsgRecv -> processData(channel, buf + curProcessIndex, dataLen);
 67                             curProcessIndex += dataLen;
 68                         }
 69                         else    /* we need to cache data */
 70                         {
 71                             cacheData(buf + curProcessIndex, size - curProcessIndex, dataLen, channel);
 72                             itsMode = eReadData;
 73                             curProcessIndex = size;
 74                         }
 75                     }
 76                     else /* header is not correct, shift one byte */
 77                     {
 78                         curProcessIndex++;
 79                     }
 80                 }
 81                 else /* process data is less than header size, cache header */
 82                 {
 83                     memcpy(itsHeaderData,buf,size);
 84                     itsCachedHeaderSize = size;
 85                     curProcessIndex = size;
 86                     itsMode = eReadHeader;
 87                 }
 88             }
 89             else if (itsMode == eReadData) /* read data */
 90             {
 91                 if(size >= itsNeedReadSize) /* we can read all data from current buf */
 92                 {
 93                     memcpy(itsBufferData + itsCachedBufferSize, buf, itsNeedReadSize);
 94                     itsCachedBufferSize += itsNeedReadSize;
 95                     itsMsgRecv -> processData(itsChannel, itsBufferData, itsCachedBufferSize);
 96                     itsMode = eParseHeader;
 97                     curProcessIndex = itsNeedReadSize;
 98                 }
 99                 else /* can't read all data from current buffer, put all data to cache buffer */
100                 {
101                     memcpy(itsBufferData + itsCachedBufferSize, buf, size);
102                     itsCachedBufferSize += size;
103                     itsNeedReadSize -= size;
104                     curProcessIndex = size;
105                 }
106             }
107             else /* read header */
108             {
109                 uint8_t needReadHeaderSize =  MSG_HEADER_SIZE - itsCachedHeaderSize;
110                 if(needReadHeaderSize < size) /* we can read header from current buffer */
111                 {
112                     memcpy(itsHeaderData+itsCachedHeaderSize,buf,needReadHeaderSize);
113                     if(isValidHeader(itsHeaderData))
114                     {
115                         uint8_t channel = itsHeaderData[3];
116                         uint32_t dataLen = readBig32(itsHeaderData+4);
117 
118                         curProcessIndex = needReadHeaderSize;
119                         if (curProcessIndex + dataLen <= size) /* we read data from current buffer */
120                         {
121 
122                             itsMsgRecv -> processData(channel, buf + curProcessIndex, dataLen);
123                             itsMode = eParseHeader;
124                             curProcessIndex += dataLen;
125                         }
126                         else /* we need to cache data */
127                         {
128                             cacheData(buf + curProcessIndex, size - curProcessIndex, dataLen, channel);
129                             curProcessIndex = size;
130                             itsMode = eReadData;
131                         }
132                     }
133                     else /* error header, shift one byte */
134                     {
135                         memcpy(itsHeaderData,itsHeaderData + 1,MSG_HEADER_SIZE - 1);
136                         itsCachedHeaderSize = MSG_HEADER_SIZE - 1;
137                         curProcessIndex = needReadHeaderSize;
138                     }
139                 }
140                 else /* can't fill a header size */
141                 {
142                     memcpy(itsHeaderData+itsCachedHeaderSize,buf,size);
143                     itsCachedHeaderSize += size;
144                     curProcessIndex = size;
145                 }
146             }
147             if(curProcessIndex < size)
148             {
149                 this->dispatch(buf+curProcessIndex,size-curProcessIndex);
150             }
151             else { } /* data process Done */
152         }
153     private:
154         uint32_t readBig32(uint8_t* ptr)
155         {
156             return ( (uint32_t)( 
157                 ( ( (uint32_t)( (uint8_t *)(ptr))[ 0 ] ) << 24 ) | 
158                 ( ( (uint32_t)( (uint8_t *)(ptr))[ 1 ] ) << 16 ) | 
159                 ( ( (uint32_t)( (uint8_t *)(ptr))[ 2 ] ) <<  8 ) | 
160                   ( (uint32_t)( (uint8_t *)(ptr))[ 3 ] ) ) );
161         }
162 
163         bool isValidHeader(uint8_t* buf)
164         {
165             return (buf[0] == 0x00 && buf[1] == 0x00 && buf[2] == 0x00 && buf[3] < 0x07);
166         }
167 
168         void cacheData(uint8_t* buf, size_t cachedSize, size_t totalDataLen, uint8_t channel)
169         {
170             memcpy(itsBufferData,buf,cachedSize);
171             itsCachedBufferSize = cachedSize;
172             itsNeedReadSize = totalDataLen - itsCachedBufferSize;
173             itsChannel = channel;
174         }
175 
176         enum {
177             MSG_HEADER_SIZE = 8,
178         };
179         enum {
180             eParseHeader,
181             eReadHeader,
182             eReadData,
183         };
184         uint8_t itsChannel;
185         uint8_t* itsBufferData;
186         size_t itsCachedBufferSize;
187         size_t itsNeedReadSize;
188         uint8_t itsMode;
189 
190         uint8_t* itsHeaderData;
191         uint8_t itsCachedHeaderSize;
192         MsgReceiver* itsMsgRecv;
193 };
194 
195 
196 
197 int main()
198 {
199     uint8_t muti_msg_buf[] = {0x00,0x00,0x00,0x01, 0x00,0x00,0x00,0x04, 0x01,0x02,0x03,0x04, 
200                         0x00,0x00,0x00,0x02, 0x00,0x00,0x00,0x02, 0x02,0x02,
201                             0x00,0x00,0x00,0x01, 0x00,0x00,0x00,0x03, 0x05,0x06,0x07};
202     uint8_t small_msg_buf_1[] = {0x00,0x00,0x00,0x01, 0x00,0x00,0x00,0x05, 0x09,0x08,0x07,0x06};
203     uint8_t small_msg_buf_2[] = {0x05, 0x00,0x00,0x00,0x02, 0x00,0x00,0x00,0x02, 0x01,0x01};
204     uint8_t small_msg_buf_3[] = {0x00,0x00,0x00,0x05, 0x00,0x00,0x00,0x0A, 0x01,0x02,0x03};
205     uint8_t small_msg_buf_4[] = {0x04,0x05,0x06};
206     uint8_t small_msg_buf_5[] = {0x07,0x08,0x09,0x0A,0x00};
207     uint8_t small_msg_buf_6[] = {0x00,0x00,0x04, 0x00,0x00,0x00,0x03, 0x07,0x07,0x07, 0x00,0x00,0x00,0x05, 0x00,0x00,0x00,0x03};
208     uint8_t small_msg_buf_7[] = {0x09,0x09,0x09};
209     uint8_t error_msg_buf_1[] = {0x01,0x00,0x00,0x00,0x06, 0x00,0x00,0x00,0x01, 0x01,0x02};
210     uint8_t error_msg_buf_2[] = {0x00,0x00,0x00,0x05, 0x00,0x00,0x00,0x02, 0x02,0x03};
211     uint8_t error_msg_buf_3[] = {0x00,0x00,0x00,0x04, 0x00,0x00,0x00,0x01, 0x04,0x05,0x05};
212     uint8_t error_msg_buf_4[] = {0x00,0x00,0x00,0x03, 0x00,0x00,0x00,0x02, 0x05,0x06};
213     uint8_t error_msg_buf_5[] = {0x00,0x00,0x00,0x02, 0x00,0x00,0x00,0x01, 0x07,0x06,0x05,0x06,0x08,0x00,0x00,0x00};
214     uint8_t error_msg_buf_6[] = {0x01, 0x00,0x00,0x00,0x03, 0x08,0x09,0x0A, 0x10,0x11};
215     uint8_t error_msg_buf_7[] = {0x01, 0x04,0x00,0x00,0x00,0x03, 0x00,0x00,0x00,0x04, 0x0B,0x0C,0x0D,0x0E};
216     uint8_t header_msg_buf_1[] = {0x01,0x00,0x00,0x00,0x06, 0x00,0x00,0x00,0x01, 0x01,0x00};
217     uint8_t header_msg_buf_2[] = {0x00, 0x00, 0x01, 0x00, 0x00};
218     uint8_t header_msg_buf_3[] = {0x00, 0x03, 0x01, 0x02, 0x03, 0x01,0x00,0x00,0x00,0x06, 0x00,0x00,0x00,0x01, 0x11};
219 
220     MsgDispatch mds;
221     printf("test muti
");
222     mds.dispatch(muti_msg_buf,sizeof(muti_msg_buf));
223 
224     printf("test small
");
225     mds.dispatch(small_msg_buf_1,sizeof(small_msg_buf_1));
226     mds.dispatch(small_msg_buf_2,sizeof(small_msg_buf_2));
227     mds.dispatch(small_msg_buf_3,sizeof(small_msg_buf_3));
228     mds.dispatch(small_msg_buf_4,sizeof(small_msg_buf_4));
229     mds.dispatch(small_msg_buf_5,sizeof(small_msg_buf_5));
230     mds.dispatch(small_msg_buf_6,sizeof(small_msg_buf_6));
231     mds.dispatch(small_msg_buf_7,sizeof(small_msg_buf_7));
232 
233     printf("test error
");
234     mds.dispatch(error_msg_buf_1,sizeof(error_msg_buf_1));
235     mds.dispatch(error_msg_buf_2,sizeof(error_msg_buf_2));
236     mds.dispatch(error_msg_buf_3,sizeof(error_msg_buf_3));
237     mds.dispatch(error_msg_buf_4,sizeof(error_msg_buf_4));
238     mds.dispatch(error_msg_buf_5,sizeof(error_msg_buf_5));
239     mds.dispatch(error_msg_buf_6,sizeof(error_msg_buf_6));
240     mds.dispatch(error_msg_buf_7,sizeof(error_msg_buf_7));
241     printf("test header
");
242     mds.dispatch(header_msg_buf_1,sizeof(header_msg_buf_1));
243     mds.dispatch(header_msg_buf_2,sizeof(header_msg_buf_2));
244     mds.dispatch(header_msg_buf_3,sizeof(header_msg_buf_3));
245     return 0;
246 }

输出：

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

转载于:https://www.cnblogs.com/zhangyonghugo/p/7499068.html