view/runCtrl/TransmitCtrl.cpp

/*!
  \file
  \brief 走行制御パケットの送受信

  \author Satofumi KAMIMURA

  $Id$
*/

#include "TransmitCtrl.h"
#include "ConnectionInterface.h"
#include <cstring>
#include <cstdio>

using namespace beego;


struct TransmitCtrl::pImpl {
  ConnectionInterface* con;
  unsigned char* top;
  char type;
  std::vector<unsigned char> packet_data;
  size_t unique_id;
  int packet_size;
  size_t items;

  pImpl(ConnectionInterface* con_obj, unsigned char* struct_ptr)
    : con(con_obj), top(struct_ptr), type(UNKNOWN_PACKET), unique_id(0),
      packet_size(0), items(0) {
  }

  // パケットへの書き込み用
  void setData(unsigned char* dst, int value, char byte) {
    for (int i = 0; i < byte; ++i) {
      dst[(byte -1) - i] = value & 0xff;
      value >>= 8;
    }
  }

  // パケットからの読み出し用
  int getValue(unsigned char* data, int size) {
    int value = 0;
    for (int i = 0; i < size; ++i) {
      value <<= 8;
      value |= data[i];
    }
    return value;
  }

  void initPacket(char packet_type) {
    type = packet_type;

    // length 情報(4-6), checksum(15) は、terminatePacket() にて格納する
    packet_data.resize(HEADER_SIZE + 2);
    memmove(&packet_data[0], FirstTag, 3);
    packet_data[3] = type;
    packet_data[7] = REPLY_REQUEST;
    setData(&packet_data[8], 0, 4);
    setData(&packet_data[12], unique_id, 3);

    items = 0;
    packet_size = 0;
  }

  void setChecksum(unsigned char* data, int size) {
    char sum = 0;
    for (int i = 0; i < size -1; ++i) {
      sum ^= data[i];
    }
    data[size -1] = sum;
  }

  void terminatePacket(void) {

    // パケット長の格納
    int packet_size = packet_data.size() + 1;
    setData(&packet_data[4], packet_size, 3);

    // ヘッダ checksum の作成
    setChecksum(&packet_data[0], HEADER_SIZE);

    // データ組数の格納
    setData(&packet_data[HEADER_SIZE], items, 2);

    // 全体 checsum の作成
    packet_data.push_back(0x0);
    setChecksum(&packet_data[0], packet_size);
  }
};


TransmitCtrl::TransmitCtrl(ConnectionInterface* con, unsigned char* struct_ptr)
  : pimpl(new pImpl(con, struct_ptr)) {
}


TransmitCtrl::~TransmitCtrl(void) {
}


void TransmitCtrl::initPacket(char type) {
  pimpl->initPacket(type);
}


void TransmitCtrl::addVariable(unsigned char* ptr, int byte) {

  int offset = ptr - pimpl->top;

  unsigned char buffer[3 + 1 + 4];
  pimpl->setData(&buffer[0], offset, 3);
  pimpl->setData(&buffer[3], byte, 1);

  if (byte == sizeof(char)) {
    pimpl->setData(&buffer[4], *ptr, byte);

  } else if (byte == sizeof(short)) {
    short* short_value = reinterpret_cast<short*>(ptr);
    pimpl->setData(&buffer[4], *short_value, byte);

  } else if (byte == sizeof(long)) {
    long* long_value = reinterpret_cast<long*>(ptr);
    pimpl->setData(&buffer[4], *long_value, byte);
  }

  int add_size = 3 + 1 + byte;
  for (int i = 0; i < add_size; ++i) {
    pimpl->packet_data.push_back(buffer[i]);
  }
  ++pimpl->items;
}


bool TransmitCtrl::send(std::vector<unsigned char>* copy_message) {
  pimpl->terminatePacket();

  int packet_size = pimpl->packet_data.size();
  int n = pimpl->con->send(reinterpret_cast<char*>(&pimpl->packet_data[0]),
                           packet_size);
#if 0
  fprintf(stderr, "send: n = %d\n", packet_size);
  for (int i = 0; i < packet_size; ++i) {
    fprintf(stderr, "%02x ", (unsigned char)pimpl->packet_data[i]);
  }
  fprintf(stderr, "\n\n");
#endif
  if (n < packet_size) {
    // 受信タイムアウト
    fprintf(stderr, " packet recv timeout! : %d\n", n);
    //fprintf(stderr, "con: %s\n", pimpl->con->what());
    return false;
  }
  if (copy_message) {
    *copy_message = pimpl->packet_data;
  }
  ++pimpl->unique_id;
  pimpl->packet_size = packet_size;
  return true;
}


bool TransmitCtrl::recv(int timeout) {

  std::vector<unsigned char> recv_buffer;
  int recv_size = (pimpl->type == WRITE_PACKET)
    ? HEADER_SIZE +1 : pimpl->packet_size;
  recv_buffer.resize(recv_size);
  int n = pimpl->con->recv(reinterpret_cast<char*>(&recv_buffer[0]),
                           recv_size, timeout);
#if 0
  fprintf(stderr, "recv: n = %d\n", n);
  for (int i = 0; i < n; ++i) {
    fprintf(stderr, "%02x ", (unsigned char)recv_buffer[i]);
  }
  fprintf(stderr, "\n\n");
  fprintf(stderr, "recv_buffer.size(): %d, recv_size: %d, n:%d\n",
          recv_buffer.size(), recv_size, n);
#endif
  if ((n <= 0) || (n < recv_size)) {
    return false;
  }

  // 受信情報の処理
  bool ret = false;
  // !!! パース
  char recv_type = recv_buffer[3];
  int items_num = 0;
  unsigned char* p = NULL;

  switch (recv_type) {
  case READ_REPLY_PACKET:
    // 読み込みへの応答
    items_num = pimpl->getValue(&recv_buffer[HEADER_SIZE], 2);
    // !!! このあたりの関数は、ターゲット側のと共通にすべきかと
    p = &recv_buffer[HEADER_SIZE +2];
    for (int i = 0; i < items_num; ++i) {
      int offset = pimpl->getValue(p, 3);
      p += 3;
      int byte = pimpl->getValue(p, 1);
      ++p;
      int value = pimpl->getValue(p, byte);
      p += byte;

      char* address = reinterpret_cast<char*>(&pimpl->top[offset]);
      if (byte == sizeof(char)) {
        *address = value;

      } else if (byte == sizeof(short)) {
        short* short_address = reinterpret_cast<short*>(address);
        *short_address = value;

      } else if (byte == sizeof(long)) {
        long* long_address = reinterpret_cast<long*>(address);
        *long_address = value;
      }
    }
    ret = true;
    break;

  case WRITE_REPLY_PACKET:
    // 書き込みへの応答
    // !!!
    // !!! とりあえず
    ret = true;
    break;

  default:
    break;
  }
  return ret;
}


bool TransmitCtrl::sendWithRetry(std::vector<unsigned char>& last_command,
                                 size_t max_retry, int timeout) {

  bool ret = send(&last_command);
  if (! ret) {
    //fprintf(stderr, "send return: %d\n", ret);
    return false;
  }

  ret = recv(timeout);
  if (! ret) {
    //fprintf(stderr, "recv return: %d\n", ret);
    return false;
  }

  //fprintf(stderr, "true\n");
  return true;
}