00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <stdio.h>
00022 #include <stdlib.h>
00023 #include <malloc.h>
00024 #include <errno.h>
00025 #include <memory.h>
00026
00027 #include "lcd.h"
00028 #include "packet.h"
00029 #include "trace.h"
00030
00031
00032 #define IILC_MODULE MODULE_TYPE_PACKET
00033
00034
00035
00036 typedef struct _tag_packet {
00037
00038
00039 unsigned char type;
00040
00041
00042 unsigned char data_length;
00043
00044
00045 unsigned char data[MAX_DATA_LENGTH];
00046
00047
00048 unsigned short CRC;
00049
00050
00051 unsigned char cursor;
00052
00053 } PACKET;
00054
00055 static int calculateCRC(LPPACKET pPacket, unsigned short * pCRC)
00056 {
00057
00058 static const unsigned short crcLookupTable[256] = {
00059 0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
00060 0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7,
00061 0x1081, 0x0108, 0x3393, 0x221A, 0x56A5, 0x472C, 0x75B7, 0x643E,
00062 0x9CC9, 0x8D40, 0xBFDB, 0xAE52, 0xDAED, 0xCB64, 0xF9FF, 0xE876,
00063 0x2102, 0x308B, 0x0210, 0x1399, 0x6726, 0x76AF, 0x4434, 0x55BD,
00064 0xAD4A, 0xBCC3, 0x8E58, 0x9FD1, 0xEB6E, 0xFAE7, 0xC87C, 0xD9F5,
00065 0x3183, 0x200A, 0x1291, 0x0318, 0x77A7, 0x662E, 0x54B5, 0x453C,
00066 0xBDCB, 0xAC42, 0x9ED9, 0x8F50, 0xFBEF, 0xEA66, 0xD8FD, 0xC974,
00067 0x4204, 0x538D, 0x6116, 0x709F, 0x0420, 0x15A9, 0x2732, 0x36BB,
00068 0xCE4C, 0xDFC5, 0xED5E, 0xFCD7, 0x8868, 0x99E1, 0xAB7A, 0xBAF3,
00069 0x5285, 0x430C, 0x7197, 0x601E, 0x14A1, 0x0528, 0x37B3, 0x263A,
00070 0xDECD, 0xCF44, 0xFDDF, 0xEC56, 0x98E9, 0x8960, 0xBBFB, 0xAA72,
00071 0x6306, 0x728F, 0x4014, 0x519D, 0x2522, 0x34AB, 0x0630, 0x17B9,
00072 0xEF4E, 0xFEC7, 0xCC5C, 0xDDD5, 0xA96A, 0xB8E3, 0x8A78, 0x9BF1,
00073 0x7387, 0x620E, 0x5095, 0x411C, 0x35A3, 0x242A, 0x16B1, 0x0738,
00074 0xFFCF, 0xEE46, 0xDCDD, 0xCD54, 0xB9EB, 0xA862, 0x9AF9, 0x8B70,
00075 0x8408, 0x9581, 0xA71A, 0xB693, 0xC22C, 0xD3A5, 0xE13E, 0xF0B7,
00076 0x0840, 0x19C9, 0x2B52, 0x3ADB, 0x4E64, 0x5FED, 0x6D76, 0x7CFF,
00077 0x9489, 0x8500, 0xB79B, 0xA612, 0xD2AD, 0xC324, 0xF1BF, 0xE036,
00078 0x18C1, 0x0948, 0x3BD3, 0x2A5A, 0x5EE5, 0x4F6C, 0x7DF7, 0x6C7E,
00079 0xA50A, 0xB483, 0x8618, 0x9791, 0xE32E, 0xF2A7, 0xC03C, 0xD1B5,
00080 0x2942, 0x38CB, 0x0A50, 0x1BD9, 0x6F66, 0x7EEF, 0x4C74, 0x5DFD,
00081 0xB58B, 0xA402, 0x9699, 0x8710, 0xF3AF, 0xE226, 0xD0BD, 0xC134,
00082 0x39C3, 0x284A, 0x1AD1, 0x0B58, 0x7FE7, 0x6E6E, 0x5CF5, 0x4D7C,
00083 0xC60C, 0xD785, 0xE51E, 0xF497, 0x8028, 0x91A1, 0xA33A, 0xB2B3,
00084 0x4A44, 0x5BCD, 0x6956, 0x78DF, 0x0C60, 0x1DE9, 0x2F72, 0x3EFB,
00085 0xD68D, 0xC704, 0xF59F, 0xE416, 0x90A9, 0x8120, 0xB3BB, 0xA232,
00086 0x5AC5, 0x4B4C, 0x79D7, 0x685E, 0x1CE1, 0x0D68, 0x3FF3, 0x2E7A,
00087 0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1,
00088 0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9,
00089 0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330,
00090 0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78
00091 };
00092
00093
00094 register unsigned short newCRC = 0xFFFF;
00095
00096
00097 unsigned short len;
00098
00099
00100 unsigned char * bufPtr;
00101
00102
00103 IILC_ASSERT(pPacket != NULL);
00104 IILC_ASSERT(pCRC != NULL);
00105
00106
00107 len = pPacket->data_length;
00108 bufPtr = pPacket->data;
00109
00110
00111 newCRC = 0xFF ^ crcLookupTable[(newCRC ^ pPacket->type) & 0xFF];
00112
00113
00114 newCRC = (newCRC >> 8) ^ crcLookupTable[(newCRC ^ pPacket->data_length) & 0xFF];
00115
00116
00117 while (len--) {
00118 newCRC = (newCRC >> 8) ^ crcLookupTable[(newCRC ^ *bufPtr++) & 0xff];
00119 }
00120
00121
00122 *pCRC = ~newCRC;
00123
00124
00125 return 0;
00126 }
00127
00128
00129 int packetCreate(PACKET_TYPE type, LPPACKET * ppPacket)
00130 {
00131 int rc = 0;
00132 int i = 0;
00133 LPPACKET pPacket = NULL;
00134
00135 IILC_TRACE_ENTRY(IILC_MODULE, T_packetCreate);
00136 traceDebug(IILC_TRACE_CONTEXT,
00137 "type " T_FMT_INT ", ppPacket " T_FMT_PTR, type, ppPacket);
00138
00139 IILC_ASSERT(ppPacket != NULL);
00140
00141
00142
00143
00144 pPacket = (LPPACKET) malloc(sizeof(PACKET));
00145 if (pPacket == NULL) {
00146 rc = ERR_MEM_ALLOC_FAILED;
00147 goto end_of_function;
00148 }
00149
00150
00151 pPacket->type = type;
00152 pPacket->data_length = 0;
00153 pPacket->CRC = 0xFFFF;
00154 pPacket->cursor = 0;
00155
00156
00157 for (i = 0; i < MAX_DATA_LENGTH; i++) {
00158 pPacket->data[i] = 0;
00159 }
00160
00161
00162 *ppPacket = pPacket;
00163
00164 end_of_function:
00165
00166 traceDebug(IILC_TRACE_CONTEXT, "rc " T_FMT_INT, rc);
00167 IILC_TRACE_EXIT;
00168 return rc;
00169 }
00170
00171 int packetDispose(LPPACKET * ppPacket)
00172 {
00173 IILC_ASSERT(ppPacket != NULL);
00174
00175 if (ppPacket == NULL) {
00176 abort();
00177 }
00178
00179 if (*ppPacket == NULL) {
00180 abort();
00181 }
00182
00183 memset(*ppPacket, 0x55, sizeof(PACKET));
00184
00185
00186 free(*ppPacket);
00187
00188
00189 *ppPacket = NULL;
00190
00191
00192 return 0;
00193 }
00194
00195 int packetWriteData(LPPACKET pPacket, unsigned char data)
00196 {
00197 int rc = 0;
00198
00199 IILC_ASSERT(pPacket != NULL);
00200
00201
00202 if (pPacket->data_length >= MAX_DATA_LENGTH) {
00203 rc = ERR_PACKET_FULL;
00204 goto end_of_function;
00205 }
00206
00207
00208 pPacket->data[pPacket->data_length++] = data;
00209
00210 end_of_function:
00211
00212 return rc;
00213 }
00214
00215
00216
00217
00218 int packetWriteString(LPPACKET pPacket, char * strData, int strDataLen)
00219 {
00220 int rc = 0;
00221
00222 IILC_ASSERT(pPacket != NULL);
00223 IILC_ASSERT(strData != NULL);
00224 IILC_ASSERT(strDataLen >= 0);
00225
00226
00227 if (pPacket->data_length + strDataLen > MAX_DATA_LENGTH) {
00228 rc = ERR_PACKET_FULL;
00229 goto end_of_function;
00230 }
00231
00232
00233 memcpy(pPacket->data + pPacket->data_length, strData, strDataLen);
00234
00235
00236 pPacket->data_length += strDataLen;
00237
00238 end_of_function:
00239
00240 return rc;
00241 }
00242
00243 int packetReadData(LPPACKET pPacket, unsigned char * data)
00244 {
00245 int rc = 0;
00246
00247 IILC_ASSERT(pPacket != NULL);
00248 IILC_ASSERT(data != NULL);
00249
00250
00251 if (pPacket->cursor >= pPacket->data_length) {
00252 rc = ERR_END_OF_PACKET;
00253 *data = 0;
00254 goto end_of_function;
00255 }
00256
00257
00258 *data = pPacket->data[pPacket->cursor++];
00259
00260 end_of_function:
00261
00262
00263 return rc;
00264 }
00265
00266
00267 int packetReadReset(LPPACKET pPacket)
00268 {
00269 IILC_ASSERT(pPacket != NULL);
00270
00271 pPacket->cursor = 0;
00272
00273
00274 return 0;
00275 }
00276
00277
00278 int packetFlatten(LPPACKET pPacket, unsigned char * buf, int * pDataLen)
00279 {
00280 int rc = 0;
00281 unsigned short CRC = 0xFFFF;
00282
00283 IILC_ASSERT(pPacket != NULL);
00284 IILC_ASSERT(buf != NULL);
00285 IILC_ASSERT(pDataLen != NULL);
00286
00287
00288 if ( *pDataLen < pPacket->data_length + 4 ) {
00289 rc = ERR_BUFFER_TOO_SMALL;
00290 goto end_of_function;
00291 }
00292
00293
00294 rc = calculateCRC(pPacket, &CRC);
00295 if (rc) { goto end_of_function; }
00296
00297
00298 buf[0] = pPacket->type;
00299 buf[1] = pPacket->data_length;
00300 memcpy(buf + 2, &(pPacket->data), buf[1]);
00301
00302
00303 buf[buf[1]+2] = CRC & 0xFF;
00304 buf[buf[1]+3] = CRC >> 8;
00305
00306
00307 *pDataLen = pPacket->data_length + 4;
00308
00309 end_of_function:
00310
00311 return rc;
00312 }
00313
00314 int packetInflate(unsigned char * buf, int len, int * pBytesRead, LPPACKET * ppPacket)
00315 {
00316 int rc = 0;
00317 int bytesRead = 0;
00318 unsigned short CRC = 0;
00319
00320 LPPACKET pPacket = NULL;
00321
00322
00323 pPacket = (LPPACKET) malloc(sizeof(PACKET));
00324 if (pPacket == NULL) {
00325 rc = ERR_MEM_ALLOC_FAILED;
00326 goto end_of_function;
00327 }
00328
00329 while (bytesRead < len) {
00330
00331
00332 if ((len - bytesRead) < 4) {
00333 rc = ERR_INSUFFICIENT_DATA;
00334 break;
00335 }
00336
00337
00338 if (buf[bytesRead] < 0x40) {
00339 bytesRead++;
00340 continue;
00341 }
00342
00343
00344 if (buf[bytesRead+1] > MAX_DATA_LENGTH) {
00345 bytesRead++;
00346 continue;
00347 }
00348
00349
00350 if (bytesRead + buf[bytesRead+1] + 4 > len) {
00351
00352 rc = ERR_INSUFFICIENT_DATA;
00353 break;
00354 }
00355
00356
00357 pPacket->type = buf[bytesRead];
00358 pPacket->data_length = buf[bytesRead+1];
00359 pPacket->cursor = 0;
00360
00361
00362 memcpy(pPacket->data, buf + bytesRead + 2, pPacket->data_length);
00363
00364
00365 pPacket->CRC = buf[bytesRead + 2 + pPacket->data_length];
00366 pPacket->CRC += buf[bytesRead + 3 + pPacket->data_length] << 8;
00367
00368
00369 rc = calculateCRC(pPacket, &CRC);
00370 if (rc) {
00371 printf("Error calculating CRC\n");
00372 break;
00373 }
00374
00375
00376 if (CRC != pPacket->CRC) {
00377 printf("Rejected on bad CRC\n");
00378 bytesRead++;
00379 continue;
00380 }
00381 else {
00382 bytesRead += 4 + pPacket->data_length;
00383 break;
00384 }
00385 }
00386
00387 end_of_function:
00388
00389
00390 if (pBytesRead != NULL) {
00391 *pBytesRead = bytesRead;
00392 }
00393
00394 if (!rc) {
00395 *ppPacket = pPacket;
00396 }
00397 else {
00398 if (pPacket != NULL) {
00399 free(pPacket);
00400 }
00401 }
00402
00403 return rc;
00404 }
00405
00406 int packetGetClass(LPPACKET pPacket)
00407 {
00408 return (pPacket->type & 0xC0) >> 6;
00409 }
00410
00411 int packetGetType(LPPACKET pPacket, PACKET_TYPE * pType)
00412 {
00413 *pType = pPacket->type;
00414 return 0;
00415 }
00416