path: root/can_hal/src/driver_can__CWORD31_.c

/*
 * @copyright Copyright (c) 2020 TOYOTA MOTOR CORPORATION.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


#include <stdio.h>
#include <string.h>
#include "can_mng_api.h"

#include <poll.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <errno.h>
#include <stdio.h>
#include <string.h>

#include <linux/can/raw.h>
#include <net/if.h>


#define DUMP_DIR	"/nv/driver-can"
#define DUMP_RCVID	"/nv/driver-can/dump_rcvid"
#define CAN_DATA_SIZE (sizeof(CanData))

static int fd_can = -1;
static struct sockaddr_can tx_address;
static CanCtlRcvId RcvId;
const char* device_name = "vcan0";

static UINT32 getData(CanCtlRcvId*, CanCtlApiCmd*);
static UINT32 getCanData(unsigned char*);
static UINT32 copyEnableCandata(UINT16* , unsigned char*, unsigned char*);
static UINT32 isOpendDriver(void);

UINT32  CanCtlApiOpen(CanCtlApiObj* pClientObj)
{
  int err_no;
  int ret;

  CAN_MNG_API_LOGT("FUNC IN");

  /*----------------------------------------------------------------------*/
  /*  Clear Object                                                        */
  /*----------------------------------------------------------------------*/
  if (pClientObj != NULL) {
    memset(pClientObj, 0, sizeof(*pClientObj));
  }

  if (fd_can != -1) {
    CAN_MNG_API_LOGT("Already Opened");
    CAN_MNG_API_LOGT("FUNC OUT");
    return CAN_CTL_RET_SUCCESS;
  }

  // open socket
  CAN_MNG_API_LOGT("open socket start");
  struct ifreq ifr = {0};
  fd_can = socket(PF_CAN, SOCK_RAW, CAN_RAW);
  err_no = errno;
  CAN_MNG_API_LOGT("open socket end");
  if (fd_can == -1) {
    CAN_MNG_API_LOGE("Socket Open Error.");
    CAN_MNG_API_LOGE("(errno[%d]).", err_no);
    return CAN_CTL_RET_ERR_ERR;
  }

  // ioctl(SIOCGIFINDEX)
  strcpy(ifr.ifr_name, device_name);
  ret = ioctl(fd_can, SIOCGIFINDEX, &ifr);
  err_no = errno;
  if (ret < 0) {
    CAN_MNG_API_LOGE("ioctl(SIOCGIFINDEX) Error.");
    CAN_MNG_API_LOGE("(errno[%d]).", err_no);
    (void)close(fd_can);
    fd_can = -1;
    return CAN_CTL_RET_ERR_ERR;
  }

  // bind
  tx_address.can_family = AF_CAN;
  tx_address.can_ifindex = ifr.ifr_ifindex;

  ret = bind(fd_can, (struct sockaddr *)&tx_address, sizeof(tx_address));
  err_no = errno;
  if (ret < 0) {
    CAN_MNG_API_LOGE("Socket Bind Error.");
    CAN_MNG_API_LOGE("(errno[%d]).", err_no);
    (void)close(fd_can);
    fd_can = -1;
    return -1;
  }

  // Initialize RcvId Map
  memset(&RcvId, 0, sizeof(RcvId));

  CAN_MNG_API_LOGT("FUNC OUT");

  return CAN_CTL_RET_SUCCESS;
}

UINT32  CanCtlApiClose(CanCtlApiObj* pClientObj)
{
  /*----------------------------------------------------------------------*/
  /*  Clear Object                                                        */
  /*----------------------------------------------------------------------*/
  if (pClientObj != NULL) {
    memset(pClientObj, 0, sizeof(*pClientObj));
  }

  if (fd_can != -1) {
    (void)close(fd_can);
  }

  // Initialize fd info
  fd_can = -1;

  CAN_MNG_API_LOGT("FUNC OUT");

  return CAN_CTL_RET_SUCCESS;
}

UINT32  CanCtlApiSndCmd(CanCtlApiObj* pClientObj, CanCtlApiCmd* pSndCmd)
{
  int i;
  printf("%s: call_id=0x%x len=%d\n", 
    __func__, pClientObj->call_id, pSndCmd->len);
  for (i = 0; i < pSndCmd->len; i++) {
    printf("[%d]0x%x ", i, pSndCmd->data[i]);
    if ((i != 0) && ((i % 7) == 0))
      printf("\n"); 
  }
  printf("\n");


  int ret;
  int err_no;

  CAN_MNG_API_LOGT("FUNC IN");
  /*----------------------------------------------------------------------*/
  /*  Check Input Value                                                   */
  /*----------------------------------------------------------------------*/
  if (NULL == pSndCmd)
  {
    CAN_MNG_API_LOGE("%s(%p) invalid parameter.", __func__, pSndCmd);
    return CAN_CTL_RET_ERR_PARAM;
  }

  ret = isOpendDriver();
  if (ret != CAN_CTL_RET_SUCCESS) {
    CAN_MNG_API_LOGE("Not Open Driver.");
    return CAN_CTL_RET_ERR_ERR;
  }

  struct can_frame frame = {0};

  frame.can_id = pSndCmd->data[1] & 0x7F;
  frame.can_id <<= 4;
  frame.can_id |= (pSndCmd->data[2] & 0xF0) >> 4;
  frame.can_dlc = pSndCmd->data[3];
  frame.data[0] = pSndCmd->data[4];
  frame.data[1] = pSndCmd->data[5];
  frame.data[2] = pSndCmd->data[6];
  frame.data[3] = pSndCmd->data[7];
  frame.data[4] = pSndCmd->data[8];
  frame.data[5] = pSndCmd->data[9];
  frame.data[6] = pSndCmd->data[10];
  frame.data[7] = pSndCmd->data[11];

  CAN_MNG_API_LOGT("write() CanDataExtSendNotif start size = %d", (int)sizeof(frame));
  ret = write(fd_can, &frame, sizeof(frame));
  err_no = errno;
  CAN_MNG_API_LOGE("write() ret = %d).", ret);

  CAN_MNG_API_LOGT("write() CanDataExtSendNotif end");
  if (ret == -1) {
    CAN_MNG_API_LOGE("write() error(errno[%d]).", err_no);
    return CAN_CTL_RET_ERR_ERR;
  }

  CAN_MNG_API_LOGT("FUNC OUT");

  return CAN_CTL_RET_SUCCESS;
}

UINT32  CanCtlApiSetRcvId(CanCtlApiObj* pClientObj, CanCtlRcvId* pRcvId)
{
  FILE* fp;
  size_t nmemb;

  CAN_MNG_API_LOGT("FUNC IN");
  /*----------------------------------------------------------------------*/
  /*  Check Input Value                                                   */
  /*----------------------------------------------------------------------*/
  if (NULL == pRcvId)
  {
    CAN_MNG_API_LOGE("%s(%p) invalid parameter.", __func__, pRcvId);
    return CAN_CTL_RET_ERR_PARAM;
  }

  struct stat st;
  int ret = 0;
  if (stat(DUMP_DIR, &st) != 0) {
    ret = mkdir(DUMP_DIR, 0755);
  }
  if (ret != 0) {
    CAN_MNG_API_LOGE("Can not Created RcvId Dump dir.");
    return CAN_CTL_RET_ERR_ERR;
  }
  CAN_MNG_API_LOGT("fopen(DUMP_RCVID) start");
  fp = fopen(DUMP_RCVID, "wb");
  CAN_MNG_API_LOGT("fopen(DUMP_RCVID) end");
  if (fp == NULL) {
    CAN_MNG_API_LOGE("Can not Opened RcvId Dump file.");
    return CAN_CTL_RET_ERR_ERR;
  }

  CAN_MNG_API_LOGT("fwrite(pRcvId) start");
  nmemb = fwrite(pRcvId, sizeof(CanCtlRcvId), 1, fp);
  CAN_MNG_API_LOGT("fwrite(pRcvId) end");
  if (nmemb != 1) {
    CAN_MNG_API_LOGE("RcvId Dump file write error.");
    (void)fclose(fp);
    return CAN_CTL_RET_ERR_ERR;
  }

  CAN_MNG_API_LOGT("fclose() start");
  (void)fclose(fp);
  CAN_MNG_API_LOGT("fclose() end");

  CAN_MNG_API_LOGT("FUNC OUT");

  return CAN_CTL_RET_SUCCESS;
}

UINT32  CanCtlApiRcvCmd(CanCtlApiObj* pClientObj, CanCtlApiCmd* pRcvCmd)
{
  int ret;
  int enable_ret;

  CanCtlRcvId InitialRcvId;
  CanCtlRcvId RcvId;
  FILE* fp;
  size_t nmemb;

  CAN_MNG_API_LOGT("FUNC IN");
  /*----------------------------------------------------------------------*/
  /*  Check Input Value                                                   */
  /*----------------------------------------------------------------------*/
  if (NULL == pRcvCmd)
  {
    CAN_MNG_API_LOGE("%s(%p) invalid parameter.", __func__, pRcvCmd);
    return CAN_CTL_RET_ERR_PARAM;
  }

  ret = isOpendDriver();
  if (ret != CAN_CTL_RET_SUCCESS) {
    CAN_MNG_API_LOGE("Not Open Driver.");
    return CAN_CTL_RET_ERR_ERR;
  }

  // Restore RcvId
  CAN_MNG_API_LOGT("fopen(DUMP_RCVID) start");
  fp = fopen(DUMP_RCVID, "rb");
  CAN_MNG_API_LOGT("fopen(DUMP_RCVID) end");
  if (fp == NULL) {
    CAN_MNG_API_LOGE("Can not Opened RcvId Dump file.");
    return CAN_CTL_RET_ERR_ERR;
  }

  CAN_MNG_API_LOGT("fread(RcvId) start");
  nmemb = fread(&RcvId, sizeof(CanCtlRcvId), 1, fp);
  CAN_MNG_API_LOGT("fread(RcvId) end");
  if (nmemb != 1) {
    CAN_MNG_API_LOGE("RcvId Dump file read error.");
    (void)fclose(fp);
    return CAN_CTL_RET_ERR_ERR;
  }

  CAN_MNG_API_LOGT("fclose() start");
  (void)fclose(fp);
  CAN_MNG_API_LOGT("fclose() end");

  memset(&InitialRcvId.id, 0, sizeof(InitialRcvId.id));

  if (memcmp(InitialRcvId.id, RcvId.id, sizeof(RcvId.id)) == 0) {
    CAN_MNG_API_LOGE("No RecvId Maps.");
    return CAN_CTL_RET_ERR_ERR;
  }

  enable_ret = getData(&RcvId, pRcvCmd);

  if (enable_ret != CAN_CTL_RET_SUCCESS) {
    CAN_MNG_API_LOGE("No data founds.");
    return enable_ret;
  }

  CAN_MNG_API_LOGT("FUNC OUT");

  return CAN_CTL_RET_SUCCESS;
}

static UINT32 getData(CanCtlRcvId* pRcvId, CanCtlApiCmd* pRcvCmd) {
  int ret;
  int total_cnt = 0;
  int copy_cnt = 0;
  int i;
  unsigned char can_data[CAN_CTL_CMD_LEN_MAX+1];
  unsigned char enable_can_data[CAN_CTL_CMD_LEN_MAX];
  unsigned char store_can_data[CAN_CTL_CMD_LEN_MAX];
  unsigned char* scd;


  UINT16 id_map[CAN_CTL_CMD_ID_HI_NUM];

  CAN_MNG_API_LOGT("FUNC IN");

  memcpy(id_map, pRcvId->id, sizeof(id_map));

  memset(store_can_data, 0, sizeof(store_can_data));
  scd = store_can_data;

  while(1) {
    memset(can_data, 0, sizeof(can_data));
    memset(enable_can_data, 0, sizeof(enable_can_data));
    ret = getCanData(can_data);
    if (ret != CAN_CTL_RET_SUCCESS) {
      // In case of error, read next data
      CAN_MNG_API_LOGE("getCanData error.");
      continue;
    }

    // Extract data of "CAN data extended reception notification 4"
    copy_cnt = copyEnableCandata(id_map, can_data, enable_can_data);
    if( copy_cnt == 0 ){
      continue;
    }

    // Store in work buffer
    for (i = 0; i < copy_cnt; i++) {
      if (total_cnt >= CAN_CTL_MAX_RCV_CAN_SIZE) {
        CAN_MNG_API_LOGE("buffer over");
        break;
      }
      memcpy(scd, &enable_can_data[CAN_DATA_SIZE * i], CAN_DATA_SIZE);
      scd = scd + CAN_DATA_SIZE;
      total_cnt++;
    }

    // Check work buffer overflow
    if (total_cnt >= CAN_CTL_MAX_RCV_CAN_SIZE) {
      CAN_MNG_API_LOGE("buffer over");
      break;
    }
  }

  // If data exists, set and return
  if (total_cnt != 0) {
    pRcvCmd->len = 1 + (total_cnt * CAN_DATA_SIZE);
    pRcvCmd->data[0] = total_cnt;
    memcpy( &(pRcvCmd->data[1]), store_can_data, (total_cnt * CAN_DATA_SIZE));
  } else {
  // If there is no data, return with length 0
    pRcvCmd->len = 0;
    memset( &(pRcvCmd->data[0]), 0, sizeof(UINT8) * CAN_DAT_LEN_MAX );
    CAN_MNG_API_LOGE("data not found");
  }

  CAN_MNG_API_LOGT("FUNC OUT");

  return CAN_CTL_RET_SUCCESS;
}

static UINT32 getCanData(unsigned char *can_data) {
  int err_no;
  int i;

  CAN_MNG_API_LOGT("FUNC IN");

  CAN_MNG_API_LOGE("recvfrom start");

  int nbytes = 0;
  struct can_frame frame = {0};
  socklen_t addrlen = sizeof(struct sockaddr_can);
  nbytes = recvfrom(fd_can, &frame, sizeof(frame), 0, (struct sockaddr*)&tx_address, &addrlen);
  err_no = errno;
  CAN_MNG_API_LOGE("recvfrom end ret = %d",nbytes);
  if (nbytes == -1) {
    CAN_MNG_API_LOGE("Not Read CAN Driver(errno[%d]).", err_no);
    return CAN_CTL_RET_ERR_ERR;
  } else if (nbytes != CAN_MTU) {
    CAN_MNG_API_LOGE("Receive Error size: %d.", nbytes);
    return CAN_CTL_RET_ERR_ERR;
  }

  CAN_MNG_API_LOGT("Recvfrom CAN Data(start)");
  can_data[0] = (frame.can_id & 0x7F0) >> 4;
  can_data[1] = (frame.can_id & 0x0F) << 4;
  can_data[2] = frame.can_dlc;
  for (i = 0; i < frame.can_dlc; i++) {
    can_data[3+i] = frame.data[i];
    CAN_MNG_API_LOGE(" 0x%02x", frame.data[i]);
  }
  CAN_MNG_API_LOGT("");
  CAN_MNG_API_LOGT("Recvfrom Data(end)");

  CAN_MNG_API_LOGT("FUNC OUT");
  return CAN_CTL_RET_SUCCESS;
}

static UINT32 copyEnableCandata(UINT16* id_map, unsigned char* can_data, unsigned char* enable_can_data) {
  int id;
  UINT16 val;
  unsigned char* ecd = enable_can_data;
  UINT32 count = 0;

  CAN_MNG_API_LOGT("FUNC IN");
  // Search data of "CAN data extended reception notification 4"
  id  = can_data[0];
  val = can_data[1] >> 4;  // Shift right 4 bits and determine search position

  CAN_MNG_API_LOGE("data id =%x", id);
  CAN_MNG_API_LOGE("data val=%x", val);
  CAN_MNG_API_LOGE("file val=%x", id_map[id]);

  // If the target data is found (If the bit is on ?)
  if ((id_map[id] & (0x01 << val)) != 0) {
    CAN_MNG_API_LOGT("matched.");
    CAN_MNG_API_LOGE("id_map[%u]", id_map[id]);

    // Store data in work buffer
    memcpy(ecd, &can_data[0], CAN_DATA_SIZE);
    count++;
  }

  CAN_MNG_API_LOGT("FUNC OUT");
  return count;
}

UINT32  CanCtlApiRcvSpd(CanCtlApiObj* pClientObj, CanCtlApiCmd* pRcvCmd)
{
  // Note.
  // If vendor needs the special implementation about receiving the vehicle speed,
  // it should be implemented by vendor.
  return CAN_CTL_RET_SUCCESS;
}

static UINT32 isOpendDriver() {
  CAN_MNG_API_LOGT("FUNC IN");

  if (fd_can == -1) {
      CAN_MNG_API_LOGE("Not Open CAN Driver.");
      return CAN_CTL_RET_ERR_ERR;
  }

  CAN_MNG_API_LOGT("FUNC OUT");
  return CAN_CTL_RET_SUCCESS;
}