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\n",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\n"); 222 mds.dispatch(muti_msg_buf,sizeof(muti_msg_buf)); 223 224 printf("test small\n"); 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\n"); 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\n"); 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
