nodeAccess.c

Go to the documentation of this file.
00001 
00010 #include "nodeAccess.h"
00011 
00012 typedef unsigned char uchar;
00013 typedef unsigned short ushort;
00014 
00015 
00016 // 構造体のアドレスと ID の割り付けを覚えておく
00017 void initNodeInfo(nodeInfo_t *node) {
00018   node->num = 0;
00019 }
00020 
00021 
00022 int registerStructInfo(nodeInfo_t *node,
00023                        unsigned char *top, long target_id) {
00024   if (node->num >= NODE_INFO_MAX-1) {
00025     return -1;
00026   }
00027   node->address[node->num] = top;
00028   node->target_id[node->num] = target_id;
00029   ++node->num;
00030 
00031   return 0;
00032 }
00033 
00034 
00035 long getStructId(nodeInfo_t *node, unsigned char *top) {
00036   int i;
00037 
00038   for (i = 0; i < node->num; ++i) {
00039     if (node->address[i] == top) {
00040       return node->target_id[i];
00041     }
00042   }
00043   return -1;
00044 }
00045 
00046 
00047 void initPacketInfo(packetInfo_t *pa, unsigned char *buffer,
00048                     unsigned char *top,
00049                     unsigned char type, long target_id, long unique_id) {
00050   pa->top = top;
00051   pa->buffer = buffer;
00052   pa->first_tag = PACKET_FIRST_TAG;
00053   pa->length = 0;
00054   pa->target_id = target_id;
00055   pa->unique_id = unique_id;
00056   pa->type = type;
00057   pa->reply = 0;
00058   pa->header_checksum = 0;
00059   pa->data_num = 0;
00060 
00061   // data_num 用のバイト幅の 2
00062   pa->data_top = buffer + 0x10 + 2;
00063 }
00064 
00065 
00066 int terminatePacketInfo(packetInfo_t *pa) {
00067   unsigned int i;
00068 
00069   pa->buffer[0] = (uchar)(pa->first_tag >> 16);
00070   pa->buffer[1] = (uchar)(pa->first_tag >> 8);
00071   pa->buffer[2] = (uchar)(pa->first_tag);
00072   pa->buffer[3] = pa->type;
00073 
00074   pa->length = (unsigned long)((pa->data_top - pa->buffer) + 1);
00075   pa->buffer[4] = (uchar)(pa->length >> 16);
00076   pa->buffer[5] = (uchar)(pa->length >> 8);
00077   pa->buffer[6] = (uchar)(pa->length);
00078   pa->buffer[7] = pa->reply;
00079 
00080   pa->buffer[8] = (uchar)(pa->target_id >> 24);
00081   pa->buffer[9] = (uchar)(pa->target_id >> 16);
00082   pa->buffer[10] = (uchar)(pa->target_id >> 8);
00083   pa->buffer[11] = (uchar)(pa->target_id);
00084 
00085   pa->buffer[12] = (uchar)(pa->unique_id >> 16);
00086   pa->buffer[13] = (uchar)(pa->unique_id >> 8);
00087   pa->buffer[14] = (uchar)(pa->unique_id);
00088 
00089   // header_checksum
00090   pa->buffer[15] = 0;
00091   for (i = 0; i < 15; ++i) {
00092     pa->buffer[15] += pa->buffer[i];
00093   }
00094 
00095   pa->buffer[16] = (uchar)(pa->data_num >> 8);
00096   pa->buffer[17] = (uchar)(pa->data_num);
00097 
00098   // packet_checksum
00099   pa->buffer[pa->length -1] = 0;
00100   for (i = 0; i < pa->length -1; ++i) {
00101     pa->buffer[pa->length -1] += pa->buffer[i];
00102   }
00103   return pa->length;
00104 }
00105 
00106 
00107 char getPacketType(unsigned char *packet_data) {
00108   return packet_data[3];
00109 }
00110 
00111 
00112 long getPacketLength(unsigned char *packet_data) {
00113   return ((packet_data[4] << 16) | (packet_data[5] << 8) | packet_data[6]);
00114 }
00115 
00116 
00117 long getPacketUniqueId(unsigned char *packet_data) {
00118   return ((packet_data[12] << 16) | (packet_data[13] << 8) | packet_data[14]);
00119 }
00120 
00121 
00122 static void getDataInfo(int *data_num, unsigned char **data_set,
00123                         unsigned char packet_data[]) {
00124   *data_num = (packet_data[16] << 8) | packet_data[17];
00125   *data_set = packet_data + 18;
00126 }
00127 
00128 
00129 void  writeFromPacketData(unsigned char *packet_data,
00130                           unsigned char *top) {
00131   int data_num;
00132   unsigned char *data_set = 0x0;
00133   unsigned short *shortValue;
00134   unsigned long *longValue;
00135   int i;
00136 
00137   getDataInfo(&data_num, &data_set, packet_data);
00138   for (i = 0; i < data_num; ++i) {
00139     int offset =
00140       (*data_set << 16) | (*(data_set + 1) << 8) | *(data_set + 2);
00141     char size = *(data_set + 3);
00142     switch (size) {
00143     case 1:
00144       *(top + offset) = *(data_set + 4);
00145       data_set += 5;
00146       break;
00147 
00148     case 2:
00149       shortValue = (unsigned short *)(top + offset);
00150       *shortValue = (*(data_set + 4) << 8) | *(data_set + 5);
00151       data_set += 6;
00152       break;
00153 
00154     case 4:
00155       longValue = (unsigned long *)(top + offset);
00156       *longValue =
00157         (*(data_set + 4) << 24) | (*(data_set + 5) << 16) |
00158         (*(data_set + 6) << 8) | *(data_set + 7);
00159       data_set += 8;
00160       break;
00161     }
00162   }
00163 }
00164 
00165 
00166 void addPacketData(packetInfo_t *pa, unsigned char *offset,
00167                    unsigned char size) {
00168   ++pa->data_num;
00169 
00170   *(pa->data_top)++ = (uchar)((offset - pa->top) >> 16);
00171   *(pa->data_top)++ = (uchar)((offset - pa->top) >> 8);
00172   *(pa->data_top)++ = (uchar)(offset - pa->top);
00173   *(pa->data_top)++ = size;
00174 
00175   switch (size) {
00176   case 1:
00177     *(pa->data_top)++ = *((unsigned char *)offset);
00178     break;
00179 
00180   case 2:
00181     *(pa->data_top)++ = (uchar)(*((unsigned short *)offset) >> 8);
00182     *(pa->data_top)++ = (uchar)(*((unsigned short *)offset));
00183     break;
00184 
00185   case 4:
00186     *(pa->data_top)++ = (uchar)(*((unsigned long *)offset) >> 24);
00187     *(pa->data_top)++ = (uchar)(*((unsigned long *)offset) >> 16);
00188     *(pa->data_top)++ = (uchar)(*((unsigned long *)offset) >> 8);
00189     *(pa->data_top)++ = (uchar)(*((unsigned long *)offset));
00190     break;
00191   }
00192 }
00193 
00194 
00195 void setPacketReply(packetInfo_t *pa, unsigned char reply) {
00196   pa->reply = reply;
00197 }
00198 
00199 
00200 int checkFirstTag(unsigned char *buffer) {
00201   if ((buffer[0] != (unsigned char)(PACKET_FIRST_TAG >> 16)) ||
00202       (buffer[1] != (unsigned char)(PACKET_FIRST_TAG >> 8)) ||
00203       (buffer[2] != (unsigned char)(PACKET_FIRST_TAG))) {
00204     return -1;
00205   }
00206   return 0;
00207 }
00208 
00209 
00210 int checkHeaderFormat(unsigned char *buffer) {
00211   unsigned char checksum = 0;
00212   int i;
00213 
00214   // first_tag のチェック
00215   if (checkFirstTag(buffer) < 0) {
00216     return -1;
00217   }
00218 
00219   for (i = 0; i < NODE_ACCESS_PACKET_HEADER_SIZE-1; ++i) {
00220     checksum += buffer[i];
00221   }
00222   if (checksum != buffer[NODE_ACCESS_PACKET_HEADER_SIZE-1]) {
00223     return -1;
00224   }
00225   return 0;
00226 }
00227 
00228 
00229 int checkPacketFormat(unsigned char *buffer, int length) {
00230   int i;
00231   unsigned char checksum;
00232   int ret_value;
00233 
00234   ret_value = checkHeaderFormat(buffer);
00235   if (ret_value < 0) {
00236     return ret_value;
00237   }
00238 
00239   // packet_checksum
00240   checksum = 0;
00241   for (i = 0; i < length -1; ++i) {
00242     checksum += buffer[i];
00243   }
00244   if (checksum != buffer[length -1]) {
00245     return -2;
00246   }
00247   return 0;
00248 }
00249 
00250 
00251 static void initPacketInfoFromPacket(packetInfo_t *pa, unsigned char buffer[],
00252                                      unsigned char *top) {
00253   pa->top = top;
00254   pa->first_tag = (buffer[0] << 16) | (buffer[1] << 8) | buffer[2];
00255   pa->buffer = buffer;
00256   pa->length = 0;
00257   pa->target_id =
00258     (buffer[8] << 24) | (buffer[9] << 16) | (buffer[10] << 8) | buffer[11];
00259   pa->unique_id = (buffer[12] << 16) | (buffer[13] << 8) | buffer[14];
00260   pa->type = buffer[3];
00261   pa->reply = 0;
00262   pa->header_checksum = 0;
00263   pa->data_num = 0;
00264 
00265   // data_num 用のバイト幅の 2
00266   pa->data_top = buffer + 0x10 + 2;
00267 }
00268 
00269 
00270 long createReadResponsePacket(unsigned char *buffer, unsigned char *top) {
00271   int i;
00272   int data_num;
00273   unsigned char *data_set = 0x0;
00274   int offset;
00275   char size;
00276   packetInfo_t pa;
00277 
00278   initPacketInfoFromPacket(&pa, buffer, top);
00279   pa.type = PACKET_READ_RESPONSE;
00280   getDataInfo(&data_num, &data_set, buffer);
00281   for (i = 0; i < data_num; ++i) {
00282     offset = (*data_set << 16) | (*(data_set + 1) << 8) | *(data_set + 2);
00283     size = *(data_set + 3);
00284     addPacketData(&pa, (unsigned char *)top + offset, size);
00285     data_set += size + 4;
00286   }
00287   return terminatePacketInfo(&pa);
00288 }
00289 
00290 
00291 long createWriteResponsePacket(unsigned char *buffer,
00292                                unsigned char *top) {
00293   packetInfo_t pa;
00294 
00295   initPacketInfoFromPacket(&pa, buffer, top);
00296   pa.type = PACKET_WRITE_RESPONSE;
00297   pa.data_top = buffer + 0x10;
00298   return terminatePacketInfo(&pa);
00299 }
00300 
00301 
00302 static void copy(unsigned char *dest, unsigned char *src, int n) {
00303   unsigned char *end = dest + n;
00304 
00305   while (dest != end) {
00306     *dest++ = *src++;
00307   }
00308 }
00309 
00310 
00311 int removeInvalidPacketHeader(unsigned char *recv_buffer, int filled) {
00312   int i, j;
00313 
00314   if (checkFirstTag(recv_buffer) >= 0) {
00315     return filled;
00316   }
00317 
00318   // パケットの先頭を探す
00319   for (i = 1; i < filled-2; ++i) {
00320     if (checkFirstTag(&recv_buffer[i]) >= 0) {
00321       copy(recv_buffer, &recv_buffer[i], filled - i);
00322       return filled - i;
00323     }
00324   }
00325 
00326   j = filled - 2;
00327   if (j > 0) {
00328     copy(recv_buffer, &recv_buffer[j], 2);
00329     return 2;
00330   }
00331 
00332   return filled;
00333 }
00334 

Generated on Mon Apr 13 22:52:04 2009 by  doxygen 1.5.7.1