video_in_hal/can_hal/src/driver_can__CWORD31_.c

   1 /*
   2  * @copyright Copyright (c) 2020 TOYOTA MOTOR CORPORATION.
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  *      http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  */
  16
  17
  18 #include <stdio.h>
  19 #include <string.h>
  20 #include "can_mng_api.h"
  21
  22 #include <poll.h>
  23 #include <sys/types.h>
  24 #include <sys/stat.h>
  25 #include <fcntl.h>
  26 #include <sys/ioctl.h>
  27 #include <unistd.h>
  28 #include <errno.h>
  29 #include <stdio.h>
  30 #include <string.h>
  31
  32 #include <linux/can/raw.h>
  33 #include <net/if.h>
  34
  35
  36 #define DUMP_DIR        "/nv/driver-can"
  37 #define DUMP_RCVID      "/nv/driver-can/dump_rcvid"
  38 #define CAN_DATA_SIZE (sizeof(CanData))
  39
  40 static int fd_can = -1;
  41 static struct sockaddr_can tx_address;
  42 static CanCtlRcvId RcvId;
  43 const char* device_name = "vcan0";
  44
  45 static UINT32 getData(CanCtlRcvId*, CanCtlApiCmd*);
  46 static UINT32 getCanData(unsigned char*);
  47 static UINT32 copyEnableCandata(UINT16* , unsigned char*, unsigned char*);
  48 static UINT32 isOpendDriver(void);
  49
  50 UINT32  CanCtlApiOpen(CanCtlApiObj* pClientObj)
  51 {
  52   int err_no;
  53   int ret;
  54
  55   CAN_MNG_API_LOGT("FUNC IN");
  56
  57   /*----------------------------------------------------------------------*/
  58   /*  Clear Object                                                        */
  59   /*----------------------------------------------------------------------*/
  60   if (pClientObj != NULL) {
  61     memset(pClientObj, 0, sizeof(*pClientObj));
  62   }
  63
  64   if (fd_can != -1) {
  65     CAN_MNG_API_LOGT("Already Opened");
  66     CAN_MNG_API_LOGT("FUNC OUT");
  67     return CAN_CTL_RET_SUCCESS;
  68   }
  69
  70   // open socket
  71   CAN_MNG_API_LOGT("open socket start");
  72   struct ifreq ifr = {0};
  73   fd_can = socket(PF_CAN, SOCK_RAW, CAN_RAW);
  74   err_no = errno;
  75   CAN_MNG_API_LOGT("open socket end");
  76   if (fd_can == -1) {
  77     CAN_MNG_API_LOGE("Socket Open Error.");
  78     CAN_MNG_API_LOGE("(errno[%d]).", err_no);
  79     return CAN_CTL_RET_ERR_ERR;
  80   }
  81
  82   // ioctl(SIOCGIFINDEX)
  83   strcpy(ifr.ifr_name, device_name);
  84   ret = ioctl(fd_can, SIOCGIFINDEX, &ifr);
  85   err_no = errno;
  86   if (ret < 0) {
  87     CAN_MNG_API_LOGE("ioctl(SIOCGIFINDEX) Error.");
  88     CAN_MNG_API_LOGE("(errno[%d]).", err_no);
  89     (void)close(fd_can);
  90     fd_can = -1;
  91     return CAN_CTL_RET_ERR_ERR;
  92   }
  93
  94   // bind
  95   tx_address.can_family = AF_CAN;
  96   tx_address.can_ifindex = ifr.ifr_ifindex;
  97
  98   ret = bind(fd_can, (struct sockaddr *)&tx_address, sizeof(tx_address));
  99   err_no = errno;
 100   if (ret < 0) {
 101     CAN_MNG_API_LOGE("Socket Bind Error.");
 102     CAN_MNG_API_LOGE("(errno[%d]).", err_no);
 103     (void)close(fd_can);
 104     fd_can = -1;
 105     return -1;
 106   }
 107
 108   // Initialize RcvId Map
 109   memset(&RcvId, 0, sizeof(RcvId));
 110
 111   CAN_MNG_API_LOGT("FUNC OUT");
 112
 113   return CAN_CTL_RET_SUCCESS;
 114 }
 115
 116 UINT32  CanCtlApiClose(CanCtlApiObj* pClientObj)
 117 {
 118   /*----------------------------------------------------------------------*/
 119   /*  Clear Object                                                        */
 120   /*----------------------------------------------------------------------*/
 121   if (pClientObj != NULL) {
 122     memset(pClientObj, 0, sizeof(*pClientObj));
 123   }
 124
 125   if (fd_can != -1) {
 126     (void)close(fd_can);
 127   }
 128
 129   // Initialize fd info
 130   fd_can = -1;
 131
 132   CAN_MNG_API_LOGT("FUNC OUT");
 133
 134   return CAN_CTL_RET_SUCCESS;
 135 }
 136
 137 UINT32  CanCtlApiSndCmd(CanCtlApiObj* pClientObj, CanCtlApiCmd* pSndCmd)
 138 {
 139   int i;
 140   printf("%s: call_id=0x%x len=%d\n",
 141     __func__, pClientObj->call_id, pSndCmd->len);
 142   for (i = 0; i < pSndCmd->len; i++) {
 143     printf("[%d]0x%x ", i, pSndCmd->data[i]);
 144     if ((i != 0) && ((i % 7) == 0))
 145       printf("\n");
 146   }
 147   printf("\n");
 148
 149
 150   int ret;
 151   int err_no;
 152
 153   CAN_MNG_API_LOGT("FUNC IN");
 154   /*----------------------------------------------------------------------*/
 155   /*  Check Input Value                                                   */
 156   /*----------------------------------------------------------------------*/
 157   if (NULL == pSndCmd)
 158   {
 159     CAN_MNG_API_LOGE("%s(%p) invalid parameter.", __func__, pSndCmd);
 160     return CAN_CTL_RET_ERR_PARAM;
 161   }
 162
 163   ret = isOpendDriver();
 164   if (ret != CAN_CTL_RET_SUCCESS) {
 165     CAN_MNG_API_LOGE("Not Open Driver.");
 166     return CAN_CTL_RET_ERR_ERR;
 167   }
 168
 169   struct can_frame frame = {0};
 170
 171   frame.can_id = pSndCmd->data[1] & 0x7F;
 172   frame.can_id <<= 4;
 173   frame.can_id |= (pSndCmd->data[2] & 0xF0) >> 4;
 174   frame.can_dlc = pSndCmd->data[3];
 175   frame.data[0] = pSndCmd->data[4];
 176   frame.data[1] = pSndCmd->data[5];
 177   frame.data[2] = pSndCmd->data[6];
 178   frame.data[3] = pSndCmd->data[7];
 179   frame.data[4] = pSndCmd->data[8];
 180   frame.data[5] = pSndCmd->data[9];
 181   frame.data[6] = pSndCmd->data[10];
 182   frame.data[7] = pSndCmd->data[11];
 183
 184   CAN_MNG_API_LOGT("write() CanDataExtSendNotif start size = %d", (int)sizeof(frame));
 185   ret = write(fd_can, &frame, sizeof(frame));
 186   err_no = errno;
 187   CAN_MNG_API_LOGE("write() ret = %d).", ret);
 188
 189   CAN_MNG_API_LOGT("write() CanDataExtSendNotif end");
 190   if (ret == -1) {
 191     CAN_MNG_API_LOGE("write() error(errno[%d]).", err_no);
 192     return CAN_CTL_RET_ERR_ERR;
 193   }
 194
 195   CAN_MNG_API_LOGT("FUNC OUT");
 196
 197   return CAN_CTL_RET_SUCCESS;
 198 }
 199
 200 UINT32  CanCtlApiSetRcvId(CanCtlApiObj* pClientObj, CanCtlRcvId* pRcvId)
 201 {
 202   FILE* fp;
 203   size_t nmemb;
 204
 205   CAN_MNG_API_LOGT("FUNC IN");
 206   /*----------------------------------------------------------------------*/
 207   /*  Check Input Value                                                   */
 208   /*----------------------------------------------------------------------*/
 209   if (NULL == pRcvId)
 210   {
 211     CAN_MNG_API_LOGE("%s(%p) invalid parameter.", __func__, pRcvId);
 212     return CAN_CTL_RET_ERR_PARAM;
 213   }
 214
 215   struct stat st;
 216   int ret = 0;
 217   if (stat(DUMP_DIR, &st) != 0) {
 218     ret = mkdir(DUMP_DIR, 0755);
 219   }
 220   if (ret != 0) {
 221     CAN_MNG_API_LOGE("Can not Created RcvId Dump dir.");
 222     return CAN_CTL_RET_ERR_ERR;
 223   }
 224   CAN_MNG_API_LOGT("fopen(DUMP_RCVID) start");
 225   fp = fopen(DUMP_RCVID, "wb");
 226   CAN_MNG_API_LOGT("fopen(DUMP_RCVID) end");
 227   if (fp == NULL) {
 228     CAN_MNG_API_LOGE("Can not Opened RcvId Dump file.");
 229     return CAN_CTL_RET_ERR_ERR;
 230   }
 231
 232   CAN_MNG_API_LOGT("fwrite(pRcvId) start");
 233   nmemb = fwrite(pRcvId, sizeof(CanCtlRcvId), 1, fp);
 234   CAN_MNG_API_LOGT("fwrite(pRcvId) end");
 235   if (nmemb != 1) {
 236     CAN_MNG_API_LOGE("RcvId Dump file write error.");
 237     (void)fclose(fp);
 238     return CAN_CTL_RET_ERR_ERR;
 239   }
 240
 241   CAN_MNG_API_LOGT("fclose() start");
 242   (void)fclose(fp);
 243   CAN_MNG_API_LOGT("fclose() end");
 244
 245   CAN_MNG_API_LOGT("FUNC OUT");
 246
 247   return CAN_CTL_RET_SUCCESS;
 248 }
 249
 250 UINT32  CanCtlApiRcvCmd(CanCtlApiObj* pClientObj, CanCtlApiCmd* pRcvCmd)
 251 {
 252   int ret;
 253   int enable_ret;
 254
 255   CanCtlRcvId InitialRcvId;
 256   CanCtlRcvId RcvId;
 257   FILE* fp;
 258   size_t nmemb;
 259
 260   CAN_MNG_API_LOGT("FUNC IN");
 261   /*----------------------------------------------------------------------*/
 262   /*  Check Input Value                                                   */
 263   /*----------------------------------------------------------------------*/
 264   if (NULL == pRcvCmd)
 265   {
 266     CAN_MNG_API_LOGE("%s(%p) invalid parameter.", __func__, pRcvCmd);
 267     return CAN_CTL_RET_ERR_PARAM;
 268   }
 269
 270   ret = isOpendDriver();
 271   if (ret != CAN_CTL_RET_SUCCESS) {
 272     CAN_MNG_API_LOGE("Not Open Driver.");
 273     return CAN_CTL_RET_ERR_ERR;
 274   }
 275
 276   // Restore RcvId
 277   CAN_MNG_API_LOGT("fopen(DUMP_RCVID) start");
 278   fp = fopen(DUMP_RCVID, "rb");
 279   CAN_MNG_API_LOGT("fopen(DUMP_RCVID) end");
 280   if (fp == NULL) {
 281     CAN_MNG_API_LOGE("Can not Opened RcvId Dump file.");
 282     return CAN_CTL_RET_ERR_ERR;
 283   }
 284
 285   CAN_MNG_API_LOGT("fread(RcvId) start");
 286   nmemb = fread(&RcvId, sizeof(CanCtlRcvId), 1, fp);
 287   CAN_MNG_API_LOGT("fread(RcvId) end");
 288   if (nmemb != 1) {
 289     CAN_MNG_API_LOGE("RcvId Dump file read error.");
 290     (void)fclose(fp);
 291     return CAN_CTL_RET_ERR_ERR;
 292   }
 293
 294   CAN_MNG_API_LOGT("fclose() start");
 295   (void)fclose(fp);
 296   CAN_MNG_API_LOGT("fclose() end");
 297
 298   memset(&InitialRcvId.id, 0, sizeof(InitialRcvId.id));
 299
 300   if (memcmp(InitialRcvId.id, RcvId.id, sizeof(RcvId.id)) == 0) {
 301     CAN_MNG_API_LOGE("No RecvId Maps.");
 302     return CAN_CTL_RET_ERR_ERR;
 303   }
 304
 305   enable_ret = getData(&RcvId, pRcvCmd);
 306
 307   if (enable_ret != CAN_CTL_RET_SUCCESS) {
 308     CAN_MNG_API_LOGE("No data founds.");
 309     return enable_ret;
 310   }
 311
 312   CAN_MNG_API_LOGT("FUNC OUT");
 313
 314   return CAN_CTL_RET_SUCCESS;
 315 }
 316
 317 static UINT32 getData(CanCtlRcvId* pRcvId, CanCtlApiCmd* pRcvCmd) {
 318   int ret;
 319   int total_cnt = 0;
 320   int copy_cnt = 0;
 321   int i;
 322   unsigned char can_data[CAN_CTL_CMD_LEN_MAX+1];
 323   unsigned char enable_can_data[CAN_CTL_CMD_LEN_MAX];
 324   unsigned char store_can_data[CAN_CTL_CMD_LEN_MAX];
 325   unsigned char* scd;
 326
 327
 328   UINT16 id_map[CAN_CTL_CMD_ID_HI_NUM];
 329
 330   CAN_MNG_API_LOGT("FUNC IN");
 331
 332   memcpy(id_map, pRcvId->id, sizeof(id_map));
 333
 334   memset(store_can_data, 0, sizeof(store_can_data));
 335   scd = store_can_data;
 336
 337   while(1) {
 338     memset(can_data, 0, sizeof(can_data));
 339     memset(enable_can_data, 0, sizeof(enable_can_data));
 340     ret = getCanData(can_data);
 341     if (ret != CAN_CTL_RET_SUCCESS) {
 342       // In case of error, read next data
 343       CAN_MNG_API_LOGE("getCanData error.");
 344       continue;
 345     }
 346
 347     // Extract data of "CAN data extended reception notification 4"
 348     copy_cnt = copyEnableCandata(id_map, can_data, enable_can_data);
 349     if( copy_cnt == 0 ){
 350       continue;
 351     }
 352
 353     // Store in work buffer
 354     for (i = 0; i < copy_cnt; i++) {
 355       if (total_cnt >= CAN_CTL_MAX_RCV_CAN_SIZE) {
 356         CAN_MNG_API_LOGE("buffer over");
 357         break;
 358       }
 359       memcpy(scd, &enable_can_data[CAN_DATA_SIZE * i], CAN_DATA_SIZE);
 360       scd = scd + CAN_DATA_SIZE;
 361       total_cnt++;
 362     }
 363
 364     // Check work buffer overflow
 365     if (total_cnt >= CAN_CTL_MAX_RCV_CAN_SIZE) {
 366       CAN_MNG_API_LOGE("buffer over");
 367       break;
 368     }
 369   }
 370
 371   // If data exists, set and return
 372   if (total_cnt != 0) {
 373     pRcvCmd->len = 1 + (total_cnt * CAN_DATA_SIZE);
 374     pRcvCmd->data[0] = total_cnt;
 375     memcpy( &(pRcvCmd->data[1]), store_can_data, (total_cnt * CAN_DATA_SIZE));
 376   } else {
 377   // If there is no data, return with length 0
 378     pRcvCmd->len = 0;
 379     memset( &(pRcvCmd->data[0]), 0, sizeof(UINT8) * CAN_DAT_LEN_MAX );
 380     CAN_MNG_API_LOGE("data not found");
 381   }
 382
 383   CAN_MNG_API_LOGT("FUNC OUT");
 384
 385   return CAN_CTL_RET_SUCCESS;
 386 }
 387
 388 static UINT32 getCanData(unsigned char *can_data) {
 389   int err_no;
 390   int i;
 391
 392   CAN_MNG_API_LOGT("FUNC IN");
 393
 394   CAN_MNG_API_LOGE("recvfrom start");
 395
 396   int nbytes = 0;
 397   struct can_frame frame = {0};
 398   socklen_t addrlen = sizeof(struct sockaddr_can);
 399   nbytes = recvfrom(fd_can, &frame, sizeof(frame), 0, (struct sockaddr*)&tx_address, &addrlen);
 400   err_no = errno;
 401   CAN_MNG_API_LOGE("recvfrom end ret = %d",nbytes);
 402   if (nbytes == -1) {
 403     CAN_MNG_API_LOGE("Not Read CAN Driver(errno[%d]).", err_no);
 404     return CAN_CTL_RET_ERR_ERR;
 405   } else if (nbytes != CAN_MTU) {
 406     CAN_MNG_API_LOGE("Receive Error size: %d.", nbytes);
 407     return CAN_CTL_RET_ERR_ERR;
 408   }
 409
 410   CAN_MNG_API_LOGT("Recvfrom CAN Data(start)");
 411   can_data[0] = (frame.can_id & 0x7F0) >> 4;
 412   can_data[1] = (frame.can_id & 0x0F) << 4;
 413   can_data[2] = frame.can_dlc;
 414   for (i = 0; i < frame.can_dlc; i++) {
 415     can_data[3+i] = frame.data[i];
 416     CAN_MNG_API_LOGE(" 0x%02x", frame.data[i]);
 417   }
 418   CAN_MNG_API_LOGT("");
 419   CAN_MNG_API_LOGT("Recvfrom Data(end)");
 420
 421   CAN_MNG_API_LOGT("FUNC OUT");
 422   return CAN_CTL_RET_SUCCESS;
 423 }
 424
 425 static UINT32 copyEnableCandata(UINT16* id_map, unsigned char* can_data, unsigned char* enable_can_data) {
 426   int id;
 427   UINT16 val;
 428   unsigned char* ecd = enable_can_data;
 429   UINT32 count = 0;
 430
 431   CAN_MNG_API_LOGT("FUNC IN");
 432   // Search data of "CAN data extended reception notification 4"
 433   id  = can_data[0];
 434   val = can_data[1] >> 4;  // Shift right 4 bits and determine search position
 435
 436   CAN_MNG_API_LOGE("data id =%x", id);
 437   CAN_MNG_API_LOGE("data val=%x", val);
 438   CAN_MNG_API_LOGE("file val=%x", id_map[id]);
 439
 440   // If the target data is found (If the bit is on ?)
 441   if ((id_map[id] & (0x01 << val)) != 0) {
 442     CAN_MNG_API_LOGT("matched.");
 443     CAN_MNG_API_LOGE("id_map[%u]", id_map[id]);
 444
 445     // Store data in work buffer
 446     memcpy(ecd, &can_data[0], CAN_DATA_SIZE);
 447     count++;
 448   }
 449
 450   CAN_MNG_API_LOGT("FUNC OUT");
 451   return count;
 452 }
 453
 454 UINT32  CanCtlApiRcvSpd(CanCtlApiObj* pClientObj, CanCtlApiCmd* pRcvCmd)
 455 {
 456   // Note.
 457   // If vendor needs the special implementation about receiving the vehicle speed,
 458   // it should be implemented by vendor.
 459   return CAN_CTL_RET_SUCCESS;
 460 }
 461
 462 static UINT32 isOpendDriver() {
 463   CAN_MNG_API_LOGT("FUNC IN");
 464
 465   if (fd_can == -1) {
 466       CAN_MNG_API_LOGE("Not Open CAN Driver.");
 467       return CAN_CTL_RET_ERR_ERR;
 468   }
 469
 470   CAN_MNG_API_LOGT("FUNC OUT");
 471   return CAN_CTL_RET_SUCCESS;
 472 }