hand_acid_mainboard/stm32components/zcanreceiver/zcanreceiver.cpp

#include "zcanreceiver.hpp"

#include "stm32basic/mutex.hpp"
#include "stm32halport/stm32halport.hpp"
#define TAG                "zcan"
#define CANHANDLER         &hcan1
#define CAN_FILTER_INDEX   0
#define CAN_MAX_FILTER_NUM 7
#define CAN_FIFO_NUM       CAN_RX_FIFO0
#define OVER_TIME_MS       30
#define HEART_OVERTIME     (30 * 1000)

using namespace iflytop;

static uint8_t                m_deviceId;
static zcanbus_on_rx_t        m_rxlistener[30];
static uint32_t               m_numListener = 0;
static zcanbus_on_connected_t m_connectedlistener;

static canrxbuffer_t m_rxbufcache;

static uint16_t reportIndex;

static uint8_t rxdata[200];
static uint8_t txbuff[200];

static uint32_t lastpacket_ticket;
static bool     m_is_connected;

static uint8_t m_priority;

zmutex m_lock = {"ZCANReceiver"};

static void _oncanpacket(CAN_HandleTypeDef *hcan);
static void _processOneCanPacket(CAN_RxHeaderTypeDef *pHeader, uint8_t *aData);
static bool _getRxMessage(CAN_RxHeaderTypeDef *pHeader, uint8_t aData[] /*8byte table*/);

extern "C" {
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox0CompleteCallback"); }
void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox1CompleteCallback"); }
void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox2CompleteCallback"); }
void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox0AbortCallback"); }
void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox1AbortCallback"); }
void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox2AbortCallback"); }
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { _oncanpacket(hcan); }
void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_RxFifo0FullCallback"); }
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_RxFifo1MsgPendingCallback"); }
void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_RxFifo1FullCallback"); }
void HAL_CAN_SleepCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_SleepCallback"); }
void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_WakeUpFromRxMsgCallback"); }
void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_ErrorCallback"); }
}

/***********************************************************************************************************************
 *                                                    ��?��?消息处理                                                     *
 ***********************************************************************************************************************/

static void _oncanpacket(CAN_HandleTypeDef *hcan) {
  if (CANHANDLER != hcan) return;
  CAN_RxHeaderTypeDef pHeader;
  uint8_t             aData[8] /*8byte table*/;
  while (_getRxMessage(&pHeader, aData)) {
    _processOneCanPacket(&pHeader, aData);
  }
}

static bool _getRxMessage(CAN_RxHeaderTypeDef *pHeader, uint8_t aData[] /*8byte table*/) {
  /**
   * @brief 读取当前FIFO��?缓存了�?�少帧的数据
   */
  uint32_t level = HAL_CAN_GetRxFifoFillLevel(CANHANDLER, CAN_FIFO_NUM);
  if (level == 0) {
    return false;
  }
  HAL_StatusTypeDef HAL_RetVal;
  HAL_RetVal = HAL_CAN_GetRxMessage(CANHANDLER, CAN_FIFO_NUM, pHeader, aData);
  if (HAL_OK == HAL_RetVal) {
    // 处理接收到的can总线数据
    return true;
  }
  return false;
}

static void _processOneCanPacket(CAN_RxHeaderTypeDef *pHeader, uint8_t *aData) {
  /**
   * @brief 消息格式 12bit from,1bit emergency
   *
   * [1] [4bit]  [8bit] [8bit]  [4bit/4bit]
   *          ,   from    to    frameNum/frameId
   */
  uint8_t from    = (pHeader->ExtId & 0x00FF0000) >> 16;
  uint8_t to      = (pHeader->ExtId & 0x0000FF00) >> 8;
  uint8_t nframe  = (pHeader->ExtId & 0x000000F0) >> 4;
  uint8_t frameId = (pHeader->ExtId & 0x0000000F);

  // ZLOGI(TAG, "from:%d to:%d nframe:%d frameId:%d", from, to, nframe, frameId);

  if (pHeader->IDE == CAN_ID_STD) {
    return;
  }

  // ��?接收主机消息
  if (from != 1) {
    return;
  }

  if (to != m_deviceId && to != 0xff) {
    return;
  }

  // 上�?�接收到的消��?还没有来的急�?�理
  if (m_rxbufcache.dataIsReady) {
    ZLOGI(TAG, "discard rx packet ,last packet not processed");
    return;
  }

  if (frameId == 0) {
    m_rxbufcache.canPacketNum = 0;
  }

  if (frameId != m_rxbufcache.canPacketNum) {
    m_rxbufcache.canPacketNum = 0;
    ZLOGI(TAG, "discard rx packet ,due to lost packet");
    return;
  }

  if (m_rxbufcache.canPacketNum < ZARRAY_SIZE(m_rxbufcache.canPacket)) {
    if (m_rxbufcache.canPacketNum == 0) {
      m_rxbufcache.header = *pHeader;
      m_rxbufcache.from   = from;
      m_rxbufcache.to     = to;
    }
    m_rxbufcache.canPacket[m_rxbufcache.canPacketNum].datalen = pHeader->DLC;
    memcpy(m_rxbufcache.canPacket[m_rxbufcache.canPacketNum].aData, aData, 8);
    m_rxbufcache.canPacketNum++;
  } else {
    ZLOGI(TAG, "discard rx packet ,due to buffer full");
    m_rxbufcache.canPacketNum = 0;
    return;
  }

  if (nframe == frameId + 1) {
    if (m_rxbufcache.canPacketNum != nframe) {
      m_rxbufcache.canPacketNum = 0;
      ZLOGI(TAG, "discard rx packet ,due to lost packet");
      return;
    } else {
      m_rxbufcache.dataIsReady = true;
    }
  }
}

/***********************************************************************************************************************
 *                                                        FUNC                                                         *
 ***********************************************************************************************************************/

static HAL_StatusTypeDef activateRxIT() {
  HAL_StatusTypeDef hal_status = HAL_ERROR;
  if (CAN_FIFO_NUM == CAN_RX_FIFO0) {
    hal_status = HAL_CAN_ActivateNotification(CANHANDLER, CAN_IT_RX_FIFO0_MSG_PENDING);
  } else if (CAN_FIFO_NUM == CAN_RX_FIFO1) {
    hal_status = HAL_CAN_ActivateNotification(CANHANDLER, CAN_IT_RX_FIFO1_MSG_PENDING);
  } else {
    ZLOGE(TAG, "start can HAL_CAN_ActivateNotification CAN_IT_RX_FIFO0_MSG_PENDING fail\r\n");
    return hal_status;
  }
  return hal_status;
}
// static HAL_StatusTypeDef deactivateRxIT() {
//   HAL_StatusTypeDef hal_status = HAL_ERROR;
//   if (CAN_FIFO_NUM == CAN_RX_FIFO0) {
//     hal_status = HAL_CAN_DeactivateNotification(CANHANDLER, CAN_IT_RX_FIFO0_MSG_PENDING);
//   } else if (CAN_FIFO_NUM == CAN_RX_FIFO1) {
//     hal_status = HAL_CAN_DeactivateNotification(CANHANDLER, CAN_IT_RX_FIFO1_MSG_PENDING);
//   } else {
//     ZLOGE(TAG, "start can HAL_CAN_ActivateNotification CAN_IT_RX_FIFO0_MSG_PENDING fail\r\n");
//     return hal_status;
//   }
//   return hal_status;
// }

HAL_StatusTypeDef initializeFilter() {
  HAL_StatusTypeDef HAL_Status;
  CAN_FilterTypeDef sFilterConfig;

  uint32_t filterId;
  uint32_t mask;

  memset(&sFilterConfig, 0, sizeof(sFilterConfig));
  sFilterConfig.FilterMode           = CAN_FILTERMODE_IDMASK;  // 设为MASK模式
  sFilterConfig.FilterScale          = CAN_FILTERSCALE_32BIT;  // CAN_FILTERSCALE_16BIT
  sFilterConfig.FilterFIFOAssignment = CAN_FIFO_NUM;           // 关联过滤器到rxfifoNum
  sFilterConfig.FilterActivation     = ENABLE;                 // 激活过滤器
  sFilterConfig.SlaveStartFilterBank = CAN_MAX_FILTER_NUM;     // slave filter start index

  /*******************************************************************************
   *                        接收所有消��?                                          *
   *******************************************************************************/
  filterId                       = (0);                            //
  mask                           = (0);                            //
  sFilterConfig.FilterBank       = CAN_FILTER_INDEX;               //
  sFilterConfig.FilterMaskIdLow  = mask & 0xffff;                  //
  sFilterConfig.FilterMaskIdHigh = (mask & 0xffff0000) >> 16;      //
  sFilterConfig.FilterIdLow      = filterId & 0xffff;              //
  sFilterConfig.FilterIdHigh     = (filterId & 0xffff0000) >> 16;  //
  HAL_Status                     = HAL_CAN_ConfigFilter(CANHANDLER, &sFilterConfig);
  if (HAL_Status != HAL_OK) {
    ZLOGE(TAG, "HAL_CAN_ConfigFilter filter0 fail");
    return HAL_Status;
  }
  // ZLOGI(TAG, "HAL_CAN_ConfigFilter filterID1 %08x", filterId >> 3);
  return HAL_Status;
}

void zcanbus_init(uint8_t deviceId) {
  m_deviceId = deviceId;

  HAL_StatusTypeDef hal_status;

  m_rxbufcache.dataIsReady  = false;
  m_rxbufcache.id           = 0;  // ��?接收来自主机的消��?
  m_rxbufcache.canPacketNum = 0;

  hal_status = initializeFilter();
  if (hal_status != HAL_OK) {
    ZLOGE(TAG, "start can initializeFilter fail\r\n");
    return;
  }

  hal_status = HAL_CAN_Start(CANHANDLER);  // 开启CAN
  if (hal_status != HAL_OK) {
    ZLOGE(TAG, "start can fail\r\n");
    return;
  }
  m_lock.init();

  HAL_StatusTypeDef status = activateRxIT();
  if (status != HAL_OK) {
    ZLOGE(TAG, "activateRxIT fail\r\n");
    ZASSERT(0);
    return;
  }
  ZLOGI(TAG, "zcanbus init done");
}
void zcanbus_reglistener(zcanbus_on_rx_t rxlistener) {
  ZASSERT(m_numListener < ZARRAY_SIZE(m_rxlistener));
  m_rxlistener[m_numListener++] = rxlistener;
}

void zcanbus_reg_on_connected_listener(zcanbus_on_connected_t connectedlistener) { m_connectedlistener = connectedlistener; }

bool zcanbus_send_packet(uint8_t to, uint8_t *packet, size_t len) { return zcanbus_send_packet(to, packet, len, OVER_TIME_MS); }

static char *hex2str(uint8_t *data, size_t len) {
  static char buf[200];
  memset(buf, 0, sizeof(buf));
  for (size_t i = 0; i < len; i++) {
    sprintf(buf + i * 2, "%02x", data[i]);
  }
  return buf;
}

bool zcanbus_send_packet(uint8_t to, uint8_t *packet, size_t len, int overtimems) {
  ZLOGI(TAG, "sendPacket to:%d, %s(%d)", to, hex2str(packet, len), len);
  int npacket = len / 8 + (len % 8 == 0 ? 0 : 1);
  if (npacket > 255) {
    ZLOGE(TAG, "sendPacket fail, len:%d", len);
    return false;
  }
  int finalpacketlen = len % 8 == 0 ? 8 : len % 8;

  for (uint8_t i = 0; i < npacket; i++) {
    bool suc = false;
    if (i == npacket - 1) {
      suc = zcanbus_send_sub_packet(to, npacket, i, packet + i * 8, finalpacketlen, overtimems);
    } else {
      suc = zcanbus_send_sub_packet(to, npacket, i, packet + i * 8, 8, overtimems);
    }
    if (!suc) {
      // ZLOGE(TAG, "sendPacket fail, packet(%d:%d)", npacket, i);
      return false;
    }
  }
  return true;
}

// static const char *canpacket_dump(uint8_t *data, int size) {
//   static char buf[20];
//   memset(buf, 0, sizeof(buf));
//   for (int i = 0; i < size; i++) {
//     sprintf(buf + i * 2, "%02x", data[i]);
//   }
//   return buf;
// }

bool zcanbus_send_sub_packet(uint8_t to, int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems) {
  CAN_TxHeaderTypeDef pHeader;
  uint8_t             aData[8] /*8byte table*/;
  uint32_t            txMailBox = 0;

  uint32_t enterticket = zget_ticket();

  memset(&pHeader, 0, sizeof(pHeader));
  memset(aData, 0, sizeof(aData));
  pHeader.StdId = 0x00;
  pHeader.ExtId = (m_deviceId << 16) | (to << 8) | (npacket << 4) | (packetIndex);
  pHeader.ExtId |= (m_priority & 0x0f) << 24;
  pHeader.ExtId |= (0x01 << 28);
  pHeader.IDE                = CAN_ID_EXT;
  pHeader.RTR                = CAN_RTR_DATA;
  pHeader.DLC                = len;
  pHeader.TransmitGlobalTime = DISABLE;

  memcpy(aData, packet, len);
  // ZLOGI(TAG, "tx %s", canpacket_dump(aData, len));

  HAL_StatusTypeDef lastTransmitStatus = HAL_CAN_AddTxMessage(CANHANDLER, &pHeader, aData, &txMailBox);
  if (lastTransmitStatus != HAL_OK) {
    ZLOGE(TAG, "HAL_CAN_AddTxMessage fail");
    return false;
  }

  while (HAL_CAN_IsTxMessagePending(CANHANDLER, txMailBox)) {
    if (zhas_passedms(enterticket) > (uint32_t)overtimems) {
      lastTransmitStatus = HAL_TIMEOUT;
      HAL_CAN_AbortTxRequest(CANHANDLER, txMailBox);
      return false;
    }
    zos_delay_ms(1);
  }
  return true;
}

bool zcanbus_send_ack(zcanbus_packet_t *rxpacket, uint8_t *param, size_t len) {
  zlock_guard       l(m_lock);
  zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
  txpacket->index            = rxpacket->index;
  txpacket->function_id      = rxpacket->function_id;
  txpacket->ptype            = kreceipt;
  if (param) memcpy(txpacket->params, param, len);
  return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + len);
}

bool zcanbus_send_ack(zcanbus_packet_t *rxpacket, int32_t val) { return zcanbus_send_ack(rxpacket, (uint8_t *)&val, sizeof(val)); }
bool zcanbus_send_ack(zcanbus_packet_t *rxpacket, bool _val) {
  int32_t val = _val ? 1 : 0;
  return zcanbus_send_ack(rxpacket, (uint8_t *)&val, sizeof(val));
}

bool zcanbus_send_errorack(zcanbus_packet_t *rxpacket, int32_t errcode) {
  zlock_guard l(m_lock);
  m_priority = kpriority_receipt;

  zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
  txpacket->index            = rxpacket->index;
  txpacket->function_id      = rxpacket->function_id;
  txpacket->ptype            = kerror_receipt;
  memcpy(txpacket->params, &errcode, sizeof(errcode));
  return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + 4);
}
bool zcanbus_send_report(uint16_t function_id, uint8_t *param, size_t len, int32_t overtime) {
  zlock_guard l(m_lock);
  m_priority = kpriority_report;

  zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
  txpacket->index            = reportIndex++;
  txpacket->function_id      = function_id;
  txpacket->ptype            = kreport;
  memcpy(txpacket->params, param, len);
  return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + len, overtime);
}

bool zcanbus_send_emergency_report(uint16_t function_id, uint8_t *param, size_t len, int32_t overtime) {
  zlock_guard l(m_lock);
  m_priority = kpriority_emergency_report;

  zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
  txpacket->index            = reportIndex++;
  txpacket->function_id      = function_id;
  txpacket->ptype            = kreport;
  memcpy(txpacket->params, param, len);
  return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + len, overtime);
}

static void process_rx_packet(canrxbuffer_t *canrxbuf, uint8_t *rx, size_t len) {
  zcanbus_packet_t *packet = (zcanbus_packet_t *)rx;

  lastpacket_ticket = zget_ticket();
  if (!m_is_connected) {
    m_is_connected = true;
    if (m_connectedlistener) m_connectedlistener(true);
  }

  if (packet->ptype == kcmd) {
    for (size_t i = 0; i < m_numListener; i++) {
      m_rxlistener[i](canrxbuf->from, canrxbuf->to, rxdata, len);
    }
  }
}

void zcanbus_schedule() {
  canrxbuffer_t *rxbuf = &m_rxbufcache;

  uint16_t fromId = 0;
  uint16_t toId   = 0;

  if (rxbuf->dataIsReady) {
    int dataoff = 0;
    // rxdata[0]   = rxbuf->from;
    // rxdata[1]   = rxbuf->to;
    for (size_t i = 0; i < rxbuf->canPacketNum; i++) {
      memcpy(rxdata + dataoff, rxbuf->canPacket[i].aData, rxbuf->canPacket[i].datalen);
      dataoff += rxbuf->canPacket[i].datalen;
      ZASSERT(dataoff < ZARRAY_SIZE(rxdata));
    }
    process_rx_packet(rxbuf, rxdata, dataoff);
    rxbuf->dataIsReady = false;
  }

  if (m_is_connected && zhas_passedms(lastpacket_ticket) > HEART_OVERTIME) {
    m_is_connected = false;
    if (m_connectedlistener) m_connectedlistener(false);
  }
}

bool zcanbus_is_connected() { return m_is_connected; }