IflytopCom/src/protocol/zfpga_commander/zfpga_commander.cpp

#include "zfpga_commander.hpp"

#include <stdlib.h>
#include <string.h>

//
#include "zqui/base/logger.hpp"
#include "zqui/base/zexception.hpp"
#include "zqui/channelmgr/channelmgr.hpp"

using namespace iflytop;

#define TAG "ZFPGACommander"

QTSerialChannel *serial_ch;

#define DEBUG

#define PACKET_LEN(__packet) (sizeof(zaf_packet_header_t) + (__packet->ndata) * sizeof(uint32_t) + 1 /*checksum*/ + 2 /*tail*/)
#define DO_CMD(exptr)                          \
  {                                            \
    zaf_error_code_t ecode = exptr;            \
    if (ecode != kaf_ec_success) return ecode; \
  }

static string hex2str(uint8_t *hex, size_t len) {
  string str;
  for (size_t i = 0; i < len; i++) {
    char buf[3];
    sprintf(buf, "%02X", hex[i]);
    str += buf;
  }
  return str;
}

void ZFPGACommander::initialize() {
  //
  serial_ch = &ChannelMgr::ins()->serialCh;
  serial_ch->regRxListener([this](uint8_t *data, size_t len) {
    {
      lock_guard<mutex> lock(lock_);
      if (len + m_rxlen > sizeof(m_rxcache)) {
        m_rxlen = 0;
      }
      memcpy(m_rxcache + m_rxlen, data, len);
      m_rxlen += len;
    }
  });

  m_thread.reset(new thread([this]() {
    uint32_t last_rx_cnt = 0;
    uint8_t  rx_process_cache[1024];
    uint32_t rx_process_cache_len = 0;
    while (true) {
      this_thread::sleep_for(chrono::milliseconds(4));

      {
        lock_guard<mutex> lock(lock_);
        if (last_rx_cnt == m_rxlen && m_rxlen != 0) {
          memcpy(rx_process_cache, m_rxcache, m_rxlen);
          rx_process_cache_len = m_rxlen;

          m_rxlen     = 0;
          last_rx_cnt = 0;
        }
      }

      if (rx_process_cache_len != 0) {
        processRxData(rx_process_cache, rx_process_cache_len);
        memset(rx_process_cache, 0, sizeof(rx_process_cache));
        rx_process_cache_len = 0;
      }

      last_rx_cnt = m_rxlen;
    }
  }));
  m_online_detect_thread.reset(new thread([this]() {
    while (true) {
      if (!isconnected) {
        this_thread::sleep_for(chrono::milliseconds(100));
      } else {
        this_thread::sleep_for(chrono::milliseconds(3000));
      }
      bool tostate = false;
      if (ping()) {
        tostate = true;
      } else {
        tostate = false;
      }

      if (tostate != isconnected) {
        if (tostate) {
          ZLOGI(TAG, "device connected.");
        } else {
          ZLOGI(TAG, "device disconnected.");
          RegInfo_Reset();
        }
        isconnected = tostate;
        if (m_stateCbFn) m_stateCbFn(isconnected);
      }
    }
  }));
}
void ZFPGACommander::regRawDataListener(binary_cb_t cb) { m_raw_data_cb = cb; }
void ZFPGACommander::regStateCbFn(StateCbFn_t cbfn) { this->m_stateCbFn = cbfn; }

bool ZFPGACommander::chIsOn() { return serial_ch->isOpen(); }

void ZFPGACommander::processRxData(uint8_t *rx, uint32_t rxlen) {
#ifdef DEBUG
  ZLOGI(TAG, "RX %s", hex2str((uint8_t *)rx, rxlen).c_str());
#endif

  for (uint32_t i = 0; i < rxlen; i++) {
    zaf_packet_header_t *header   = (zaf_packet_header_t *)(&rx[i]);
    uint8_t             *packetu8 = &rx[i];

    if (header->packet_header == PACKET_HEADER) {
      uint8_t check = packetu8[header->ndata * 4 + sizeof(zaf_packet_header_t) + 0];
      uint8_t tail0 = packetu8[header->ndata * 4 + sizeof(zaf_packet_header_t) + 1];
      uint8_t tail1 = packetu8[header->ndata * 4 + sizeof(zaf_packet_header_t) + 2];

      uint16_t tail = (tail1 << 8) | tail0;

      uint8_t expectcheck = 0;
      for (uint32_t j = 2; j < header->ndata * 4 + sizeof(zaf_packet_header_t); j++) {
        expectcheck += packetu8[j];
      }

      if (tail == PACKET_TAIL) {
        if (expectcheck == check) {
          processRxPacket(header);
        } else {
          ZLOGE(TAG, "Rx packet check error %d != %d", expectcheck, check);
        }
      }
    }
  }
}
void ZFPGACommander::processRxPacket(zaf_packet_header_t *packet) {
  //
  // ZLOGI(TAG, "RX packet");
  // ZLOGI(TAG, "  type      :%d", packet->packet_type);
  // ZLOGI(TAG, "  index     :%d", packet->index);
  // ZLOGI(TAG, "  cmd       :%d", packet->cmd);
  // ZLOGI(TAG, "  ndata     :%d", packet->ndata);
  // for (uint32_t i = 0; i < packet->ndata; i++) {
  //   ZLOGI(TAG, "    data[%d]:%d", i, packet->data[i]);
  // }
  // ZLOGI(TAG, "Rx.....");
  uint32_t packetlen = sizeof(zaf_packet_header_t) + packet->ndata * 4 + 3;

  if (m_raw_data_cb) {
    m_raw_data_cb(kuart_raw_rx, (uint8_t *)packet, packetlen);
  }
  if (packet->packet_type == kzaf_packet_type_receipt) {
    lock_guard<mutex> lock(m_rxReceiptContext_lock);
    if (m_rxReceiptContext.waittingForReceipt) {
      if (m_rxReceiptContext.waittingIndex == packet->index) {
        m_rxReceiptContext.receiptIsReady = true;
        m_rxReceiptContext.receiptLen     = PACKET_LEN(packet);
        memcpy(m_rxReceiptContext.receipt, packet, PACKET_LEN(packet));
      }
      m_rxReceiptContext.waittingForReceipt = false;
    }
  } else if (packet->packet_type == kzaf_packet_type_heart) {
  }
}

void ZFPGACommander::resetRxContext(int32_t cmdIndex) {
  lock_guard<mutex> lock(m_rxReceiptContext_lock);
  m_rxReceiptContext.waittingIndex      = cmdIndex;
  m_rxReceiptContext.waittingForReceipt = true;
  m_rxReceiptContext.receiptIsReady     = false;
  m_rxReceiptContext.receiptLen         = 0;
}

shared_ptr<Receipt> ZFPGACommander::sendPacket(zaf_packet_header_t *packet, uint32_t len, uint32_t overtime) {
  lock_guard<recursive_mutex> lock(m_tx_lock);

  zaf_packet_header_t *rxpacket = (zaf_packet_header_t *)&m_rxReceiptContext.receipt[0];

  resetRxContext(packet->index);

  if (serial_ch->isOpen() == false) {
    throw zexception(ke_invalid_packet_format, "channel is not open");
  }
  if (m_raw_data_cb) {
    m_raw_data_cb(kuart_raw_tx, (uint8_t *)packet, PACKET_LEN(packet));
  }
#ifdef DEBUG
  ZLOGI(TAG, "TX %s", hex2str((uint8_t *)packet, len).c_str());
#endif
  serial_ch->send((uint8_t *)packet, len);
  for (size_t i = 0; i < overtime; i++) {
    {
      lock_guard<mutex> lock(m_rxReceiptContext_lock);
      if (m_rxReceiptContext.receiptIsReady) {
        break;
      }
    }
    this_thread::sleep_for(chrono::milliseconds(1));
  }

  if (m_rxReceiptContext.receiptIsReady) {
    if (rxpacket->data[0] != 0) {
      throw zexception(rxpacket->data[0], zaf_ecode_to_string((zaf_error_code_t)rxpacket->data[0]));
    } else {
      shared_ptr<Receipt> receipt;
      receipt.reset(new Receipt(m_rxReceiptContext.receipt, m_rxReceiptContext.receiptLen));
      return receipt;
    }
  } else {
  }

  throw zexception(ke_overtime, "overtime");
}

void ZFPGACommander::writeReg(uint32_t regadd, uint32_t regvalue) {
  uint32_t regbackvalue;
  writeReg(regadd, regvalue, regbackvalue, 50);
}

void ZFPGACommander::writeReg(uint32_t regadd, uint32_t regvalue, uint32_t &regbackvalue, int32_t overtime_ms) {
  lock_guard<recursive_mutex> lock(m_tx_lock);

  Reginfo *reg = GetRegInfo(regadd);

  if (reg->flag & kreg_flag_force_write) {
    _writeReg(regadd, regvalue, regbackvalue, overtime_ms);
  } else if (reg->dirty) {
    _writeReg(regadd, regvalue, regbackvalue, overtime_ms);
  } else if (reg->regshadow != regvalue) {
    _writeReg(regadd, regvalue, regbackvalue, overtime_ms);
  } else {
    regbackvalue = reg->regshadow;
  }
  reg->dirty     = false;
  reg->regshadow = regvalue;
  return;
}
void ZFPGACommander::readReg(uint32_t regadd, uint32_t &regvalue, int32_t overtime_ms) {
  lock_guard<recursive_mutex> lock(m_tx_lock);

  Reginfo *reg = GetRegInfo(regadd);

  if ((reg->flag & kreg_flag_force_read)) {
    _readReg(regadd, regvalue, overtime_ms);
  } else if (reg->dirty) {
    _readReg(regadd, regvalue, overtime_ms);
  } else {
    regvalue = reg->regshadow;
  }
  reg->dirty     = false;
  reg->regshadow = regvalue;
}

void ZFPGACommander::_writeReg(uint32_t regadd, uint32_t regvalue, uint32_t &regbackvalue, int32_t overtime_ms) {
  //

  uint8_t txdata[128] = {0};

  zaf_packet_header_t *txpacket = (zaf_packet_header_t *)txdata;
  zaf_packet_header_t *rxpacket = (zaf_packet_header_t *)&m_rxReceiptContext.receipt[0];

  txpacket->packet_header = PACKET_HEADER;
  txpacket->packet_type   = kzaf_packet_type_cmd;
  txpacket->index         = ++txindex;
  txpacket->cmd           = kzaf_cmd_reg_write;
  txpacket->ndata         = 2;
  txpacket->data[0]       = regadd;
  txpacket->data[1]       = regvalue;

  uint32_t txpacklen = PACKET_LEN(txpacket);
  uint8_t  checksum  = 0;
  for (uint32_t i = 2; i < txpacklen - 3; i++) {
    checksum += txdata[i];
  }
  txdata[txpacklen - 3] = checksum;
  txdata[txpacklen - 2] = PACKET_TAIL & 0xFF;
  txdata[txpacklen - 1] = (PACKET_TAIL >> 8) & 0xFF;

  sendPacket(txpacket, txpacklen, overtime_ms);
  regbackvalue = rxpacket->data[1];
}
void ZFPGACommander::_readReg(uint32_t regadd, uint32_t &regvalue, int32_t overtime_ms) {
  uint8_t txdata[128] = {0};

  zaf_packet_header_t *txpacket = (zaf_packet_header_t *)txdata;
  zaf_packet_header_t *rxpacket = (zaf_packet_header_t *)&m_rxReceiptContext.receipt[0];

  txpacket->packet_header = PACKET_HEADER;
  txpacket->packet_type   = kzaf_packet_type_cmd;
  txpacket->index         = ++txindex;
  txpacket->cmd           = kzaf_cmd_reg_read;
  txpacket->ndata         = 1;
  txpacket->data[0]       = regadd;

  uint32_t txpacklen = PACKET_LEN(txpacket);
  uint8_t  checksum  = 0;
  for (uint32_t i = 2; i < txpacklen - 3; i++) {
    checksum += txdata[i];
  }
  txdata[txpacklen - 3] = checksum;
  txdata[txpacklen - 2] = PACKET_TAIL & 0xFF;
  txdata[txpacklen - 1] = (PACKET_TAIL >> 8) & 0xFF;

  sendPacket(txpacket, txpacklen, overtime_ms);
  regvalue = rxpacket->data[1];

  // ZLOGI(TAG, "RX packet");
  // ZLOGI(TAG, "  type      :%d", rxpacket->packet_type);
  // ZLOGI(TAG, "  index     :%d", rxpacket->index);
  // ZLOGI(TAG, "  cmd       :%d", rxpacket->cmd);
  // ZLOGI(TAG, "  ndata     :%d", rxpacket->ndata);
  // for (uint32_t i = 0; i < rxpacket->ndata; i++) {
  //   ZLOGI(TAG, "    data[%d]:%d", i, rxpacket->data[i]);
  // }
  // ZLOGI(TAG, "RX:%d", regvalue);
}

Reginfo *ZFPGACommander::GetRegInfo(uint32_t add) {
  auto it = m_reginfoMap.find(add);
  if (it != m_reginfoMap.end()) {
    return &it->second;
  }
  m_reginfoMap.insert(make_pair(add, Reginfo()));
  return &m_reginfoMap[add];
}
void ZFPGACommander::RegInfo_Reset() {
  for (auto &it : m_reginfoMap) {
    it.second.dirty = true;
  }
}

void ZFPGACommander::readFPGAVersion(Version &version) {
  uint32_t version32;
  readReg(kreg_fpga_version, version32, 100);
  version.main = VERSION_MAIN(version32);
  version.sub  = VERSION_SUB(version32);
  version.fix  = VERSION_FIX(version32);
}
void ZFPGACommander::readStm32Version(Version &version) {
  uint32_t version32;
  readReg(kreg_software_version, version32, 10);
  version.main = VERSION_MAIN(version32);
  version.sub  = VERSION_SUB(version32);
  version.fix  = VERSION_FIX(version32);
}

bool ZFPGACommander::ping() {
  try {
    callcmd(kzaf_cmd_ping, 10);
  } catch (const std::exception &e) {
    return false;
  }
  return true;
}

void ZFPGACommander::callcmd(uint32_t cmd, uint32_t delayms) {
  lock_guard<recursive_mutex> lock(m_tx_lock);

  uint8_t txdata[128] = {0};

  zaf_packet_header_t *txpacket = (zaf_packet_header_t *)txdata;
  // zaf_packet_header_t *rxpacket = (zaf_packet_header_t *)&m_rxReceiptContext.receipt[0];

  txpacket->packet_header = PACKET_HEADER;
  txpacket->packet_type   = kzaf_packet_type_cmd;
  txpacket->index         = ++txindex;
  txpacket->cmd           = cmd;
  txpacket->ndata         = 0;

  uint32_t txpacklen = PACKET_LEN(txpacket);
  uint8_t  checksum  = 0;
  for (uint32_t i = 2; i < txpacklen - 3; i++) {
    checksum += txdata[i];
  }
  txdata[txpacklen - 3] = checksum;
  txdata[txpacklen - 2] = PACKET_TAIL & 0xFF;
  txdata[txpacklen - 1] = (PACKET_TAIL >> 8) & 0xFF;

  sendPacket(txpacket, txpacklen, delayms);
}

void ZFPGACommander::factoryReset() { callcmd(kzaf_cmd_factory_reset, 1500); }
void ZFPGACommander::reboot() { callcmd(kzaf_cmd_reboot, 100); }
void ZFPGACommander::storageConfigs() { callcmd(kzaf_cmd_storage_cfg, 1500); }