nodeAccess.c

Go to the documentation of this file.

#include "nodeAccess.h"

typedef unsigned char uchar;
typedef unsigned short ushort;


// 構造体のアドレスと ID の割り付けを覚えておく
void initNodeInfo(nodeInfo_t *node) {
  node->num = 0;
}


int registerStructInfo(nodeInfo_t *node,
                       unsigned char *top, long target_id) {
  if (node->num >= NODE_INFO_MAX-1) {
    return -1;
  }
  node->address[node->num] = top;
  node->target_id[node->num] = target_id;
  ++node->num;

  return 0;
}


long getStructId(nodeInfo_t *node, unsigned char *top) {
  int i;

  for (i = 0; i < node->num; ++i) {
    if (node->address[i] == top) {
      return node->target_id[i];
    }
  }
  return -1;
}


void initPacketInfo(packetInfo_t *pa, unsigned char *buffer,
                    unsigned char *top,
                    unsigned char type, long target_id, long unique_id) {
  pa->top = top;
  pa->buffer = buffer;
  pa->first_tag = PACKET_FIRST_TAG;
  pa->length = 0;
  pa->target_id = target_id;
  pa->unique_id = unique_id;
  pa->type = type;
  pa->reply = 0;
  pa->header_checksum = 0;
  pa->data_num = 0;

  // data_num 用のバイト幅の 2
  pa->data_top = buffer + 0x10 + 2;
}


int terminatePacketInfo(packetInfo_t *pa) {
  unsigned int i;

  pa->buffer[0] = (uchar)(pa->first_tag >> 16);
  pa->buffer[1] = (uchar)(pa->first_tag >> 8);
  pa->buffer[2] = (uchar)(pa->first_tag);
  pa->buffer[3] = pa->type;

  pa->length = (unsigned long)((pa->data_top - pa->buffer) + 1);
  pa->buffer[4] = (uchar)(pa->length >> 16);
  pa->buffer[5] = (uchar)(pa->length >> 8);
  pa->buffer[6] = (uchar)(pa->length);
  pa->buffer[7] = pa->reply;

  pa->buffer[8] = (uchar)(pa->target_id >> 24);
  pa->buffer[9] = (uchar)(pa->target_id >> 16);
  pa->buffer[10] = (uchar)(pa->target_id >> 8);
  pa->buffer[11] = (uchar)(pa->target_id);

  pa->buffer[12] = (uchar)(pa->unique_id >> 16);
  pa->buffer[13] = (uchar)(pa->unique_id >> 8);
  pa->buffer[14] = (uchar)(pa->unique_id);

  // header_checksum
  pa->buffer[15] = 0;
  for (i = 0; i < 15; ++i) {
    pa->buffer[15] += pa->buffer[i];
  }

  pa->buffer[16] = (uchar)(pa->data_num >> 8);
  pa->buffer[17] = (uchar)(pa->data_num);

  // packet_checksum
  pa->buffer[pa->length -1] = 0;
  for (i = 0; i < pa->length -1; ++i) {
    pa->buffer[pa->length -1] += pa->buffer[i];
  }
  return pa->length;
}


char getPacketType(unsigned char *packet_data) {
  return packet_data[3];
}


long getPacketLength(unsigned char *packet_data) {
  return ((packet_data[4] << 16) | (packet_data[5] << 8) | packet_data[6]);
}


long getPacketUniqueId(unsigned char *packet_data) {
  return ((packet_data[12] << 16) | (packet_data[13] << 8) | packet_data[14]);
}


static void getDataInfo(int *data_num, unsigned char **data_set,
                        unsigned char packet_data[]) {
  *data_num = (packet_data[16] << 8) | packet_data[17];
  *data_set = packet_data + 18;
}


void  writeFromPacketData(unsigned char *packet_data,
                          unsigned char *top) {
  int data_num;
  unsigned char *data_set = 0x0;
  unsigned short *shortValue;
  unsigned long *longValue;
  int i;

  getDataInfo(&data_num, &data_set, packet_data);
  for (i = 0; i < data_num; ++i) {
    int offset =
      (*data_set << 16) | (*(data_set + 1) << 8) | *(data_set + 2);
    char size = *(data_set + 3);
    switch (size) {
    case 1:
      *(top + offset) = *(data_set + 4);
      data_set += 5;
      break;

    case 2:
      shortValue = (unsigned short *)(top + offset);
      *shortValue = (*(data_set + 4) << 8) | *(data_set + 5);
      data_set += 6;
      break;

    case 4:
      longValue = (unsigned long *)(top + offset);
      *longValue =
        (*(data_set + 4) << 24) | (*(data_set + 5) << 16) |
        (*(data_set + 6) << 8) | *(data_set + 7);
      data_set += 8;
      break;
    }
  }
}


void addPacketData(packetInfo_t *pa, unsigned char *offset,
                   unsigned char size) {
  ++pa->data_num;

  *(pa->data_top)++ = (uchar)((offset - pa->top) >> 16);
  *(pa->data_top)++ = (uchar)((offset - pa->top) >> 8);
  *(pa->data_top)++ = (uchar)(offset - pa->top);
  *(pa->data_top)++ = size;

  switch (size) {
  case 1:
    *(pa->data_top)++ = *((unsigned char *)offset);
    break;

  case 2:
    *(pa->data_top)++ = (uchar)(*((unsigned short *)offset) >> 8);
    *(pa->data_top)++ = (uchar)(*((unsigned short *)offset));
    break;

  case 4:
    *(pa->data_top)++ = (uchar)(*((unsigned long *)offset) >> 24);
    *(pa->data_top)++ = (uchar)(*((unsigned long *)offset) >> 16);
    *(pa->data_top)++ = (uchar)(*((unsigned long *)offset) >> 8);
    *(pa->data_top)++ = (uchar)(*((unsigned long *)offset));
    break;
  }
}


void setPacketReply(packetInfo_t *pa, unsigned char reply) {
  pa->reply = reply;
}


int checkFirstTag(unsigned char *buffer) {
  if ((buffer[0] != (unsigned char)(PACKET_FIRST_TAG >> 16)) ||
      (buffer[1] != (unsigned char)(PACKET_FIRST_TAG >> 8)) ||
      (buffer[2] != (unsigned char)(PACKET_FIRST_TAG))) {
    return -1;
  }
  return 0;
}


int checkHeaderFormat(unsigned char *buffer) {
  unsigned char checksum = 0;
  int i;

  // first_tag のチェック
  if (checkFirstTag(buffer) < 0) {
    return -1;
  }

  for (i = 0; i < NODE_ACCESS_PACKET_HEADER_SIZE-1; ++i) {
    checksum += buffer[i];
  }
  if (checksum != buffer[NODE_ACCESS_PACKET_HEADER_SIZE-1]) {
    return -1;
  }
  return 0;
}


int checkPacketFormat(unsigned char *buffer, int length) {
  int i;
  unsigned char checksum;
  int ret_value;

  ret_value = checkHeaderFormat(buffer);
  if (ret_value < 0) {
    return ret_value;
  }

  // packet_checksum
  checksum = 0;
  for (i = 0; i < length -1; ++i) {
    checksum += buffer[i];
  }
  if (checksum != buffer[length -1]) {
    return -2;
  }
  return 0;
}


static void initPacketInfoFromPacket(packetInfo_t *pa, unsigned char buffer[],
                                     unsigned char *top) {
  pa->top = top;
  pa->first_tag = (buffer[0] << 16) | (buffer[1] << 8) | buffer[2];
  pa->buffer = buffer;
  pa->length = 0;
  pa->target_id =
    (buffer[8] << 24) | (buffer[9] << 16) | (buffer[10] << 8) | buffer[11];
  pa->unique_id = (buffer[12] << 16) | (buffer[13] << 8) | buffer[14];
  pa->type = buffer[3];
  pa->reply = 0;
  pa->header_checksum = 0;
  pa->data_num = 0;

  // data_num 用のバイト幅の 2
  pa->data_top = buffer + 0x10 + 2;
}


long createReadResponsePacket(unsigned char *buffer, unsigned char *top) {
  int i;
  int data_num;
  unsigned char *data_set = 0x0;
  int offset;
  char size;
  packetInfo_t pa;

  initPacketInfoFromPacket(&pa, buffer, top);
  pa.type = PACKET_READ_RESPONSE;
  getDataInfo(&data_num, &data_set, buffer);
  for (i = 0; i < data_num; ++i) {
    offset = (*data_set << 16) | (*(data_set + 1) << 8) | *(data_set + 2);
    size = *(data_set + 3);
    addPacketData(&pa, (unsigned char *)top + offset, size);
    data_set += size + 4;
  }
  return terminatePacketInfo(&pa);
}


long createWriteResponsePacket(unsigned char *buffer,
                               unsigned char *top) {
  packetInfo_t pa;

  initPacketInfoFromPacket(&pa, buffer, top);
  pa.type = PACKET_WRITE_RESPONSE;
  pa.data_top = buffer + 0x10;
  return terminatePacketInfo(&pa);
}


static void copy(unsigned char *dest, unsigned char *src, int n) {
  unsigned char *end = dest + n;

  while (dest != end) {
    *dest++ = *src++;
  }
}


int removeInvalidPacketHeader(unsigned char *recv_buffer, int filled) {
  int i, j;

  if (checkFirstTag(recv_buffer) >= 0) {
    return filled;
  }

  // パケットの先頭を探す
  for (i = 1; i < filled-2; ++i) {
    if (checkFirstTag(&recv_buffer[i]) >= 0) {
      copy(recv_buffer, &recv_buffer[i], filled - i);
      return filled - i;
    }
  }

  j = filled - 2;
  if (j > 0) {
    copy(recv_buffer, &recv_buffer[j], 2);
    return 2;
  }

  return filled;
}