service/peripheral/communication/server/src/CAN/Delivery/CAN_Delivery.cpp

   1 /*
   2  * @copyright Copyright (c) 2016-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 #include "CAN_Delivery.h"
  18
  19 #include <can_hal.h>
  20 #include <native_service/frameworkunified_framework_if.h>
  21 #include <peripheral_service/Canif_API.h>
  22 #include <can_hal.h>
  23 #include <utility>
  24 #include <map>
  25 #include <string>
  26 #include "CAN_TxMsg.h"
  27 #include "CAN_CommWatch.h"
  28 #include "Canif_API_Local.h"
  29 #include "API_Local_Common.h"
  30 #include "Thread_Common.h"
  31
  32 static CAN_DeliveryEntryList g_map_delivery_list_can;
  33 // Member of g_msg_rx_msg exist as long as communication exists.
  34 static std::map<CANID, CanMessage *> g_can_rx_msg;
  35
  36 void CANDeliveryInit(void) {
  37   g_map_delivery_list_can.clear();
  38   return;
  39 }
  40
  41 static CanMessage *CANMsgFind(CANID k) {
  42   std::map<CANID, CanMessage *>::iterator it;
  43
  44   it = g_can_rx_msg.find(k);
  45   if (it == g_can_rx_msg.end())
  46     return NULL;
  47   return it->second;
  48 }
  49
  50 static void CANMsgInsert(CANID k, CanMessage *msg) {
  51   CanMessage *latest = CANMsgFind(k);
  52   if (!latest) {
  53     latest = new CanMessage();
  54   }
  55   g_can_rx_msg[k] = latest;
  56   memcpy(latest, msg, sizeof(CanMessage));
  57 }
  58
  59 static CAN_DeliveryEntryListIt CANDeliveryFind(CANID canid, std::string s) {
  60   std::pair<CAN_DeliveryEntryListIt, CAN_DeliveryEntryListIt> range;
  61   CAN_DeliveryEntryListPair p = std::make_pair(canid, s);
  62   CAN_DeliveryEntryListIt it;
  63   bool found = false;
  64   range = g_map_delivery_list_can.equal_range(canid);
  65   for (it = range.first; it != range.second; ++it) {
  66     if (*it == p) {
  67       found = true;
  68       break;
  69     }
  70   }
  71
  72   if (!found)
  73     it = g_map_delivery_list_can.end();
  74
  75   return it;
  76 }
  77
  78 bool CANDeliveryInsert(CANID canid, std::string s) {
  79   CAN_DeliveryEntryListIt it = CANDeliveryFind(canid, s);
  80   CAN_DeliveryEntryListPair p = std::make_pair(canid, s);
  81   bool inserted = false;
  82
  83   if (it == g_map_delivery_list_can.end()) {
  84     g_map_delivery_list_can.insert(p);
  85     inserted = true;
  86   }
  87
  88   return inserted;
  89 }
  90
  91 static EFrameworkunifiedStatus CANDeliverySndCmdSingle(HANDLE h_app, CANID cmd,
  92                                           CanMessage *msg, std::string dest) {
  93   CAN_MSG_CANCMD st_delivery_msg;
  94   EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail;
  95   HANDLE h_client;
  96   uint16_t len = 0;
  97
  98   // Create delivery data
  99   memset(&st_delivery_msg, 0, sizeof(st_delivery_msg));
 100   switch (cmd) {
 101   case CAN_CMDID_FUELCALC_RST_REQ_RX:
 102     len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_FUELCALC_RST_SIZE;
 103     break;
 104   case CAN_CMDID_STARTUP_FIN_RESP_RX:
 105     len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_STARTUP_FIN_SIZE;
 106     break;
 107   case CAN_CMDID_MRST_INFO_RESP_RX:
 108     len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_MRST_INFO_SIZE;
 109     break;
 110   case CAN_CMDID_VERSION_RESP_RX:
 111     len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_VERSION_SIZE;
 112     break;
 113   case CAN_CMDID_CONNECTION_NODE_RESP_RX:
 114     len = (uint16_t)(CAN_TX_CMD_DELIVERY_SIZE + msg->data[0] + 1);
 115     break;
 116   default:
 117     return e_status;
 118   }
 119   st_delivery_msg.hdr.hdr.rid = 0;
 120   st_delivery_msg.hdr.hdr.cid = (uint16_t)CID_CAN_CMD_DELIVERY;
 121   st_delivery_msg.hdr.hdr.msgbodysize = len;
 122   st_delivery_msg.data.cmd_id = (uint8_t)cmd;
 123   memcpy(&st_delivery_msg.data.data, msg->data,
 124                                      (size_t)(len - CAN_TX_CMD_DELIVERY_SIZE));
 125
 126   h_client = CommonFindSender(h_app, dest);
 127   if (!h_client) {
 128     return e_status;
 129   }
 130
 131   FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "cid=%x msgbodysize=%x cmd_id=%x",
 132          st_delivery_msg.hdr.hdr.cid, st_delivery_msg.hdr.hdr.msgbodysize,
 133          st_delivery_msg.data.cmd_id);
 134   FRAMEWORKUNIFIEDLOG(ZONE_DEBUG, __func__, "msg_data=%s",
 135          MessageDataOutputLog(st_delivery_msg.data.data, len).c_str());
 136
 137   e_status = FrameworkunifiedSendMsg(h_client, CID_CAN_CMD_DELIVERY,
 138                                    sizeof(st_delivery_msg), &st_delivery_msg);
 139   if (e_status != eFrameworkunifiedStatusOK) {
 140     FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "FrameworkunifiedSendMsg Error(e_status:%d to:%s)",
 141                                e_status, dest.c_str());
 142   } else {
 143     FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__,
 144            "CAN Delivery : dst = %s, CMD = %x, DLC = %x",
 145            dest.c_str(), cmd, msg->dlc);
 146   }
 147   return e_status;
 148 }
 149
 150 static EFrameworkunifiedStatus CANDeliverySndMsgSingle(HANDLE h_app, CANID ul_canid,
 151         uint8_t uc_dlc, const uint8_t *puc_data,
 152         enum CanIfEchoBackFlags flag, std::string dest) {
 153   CAN_MSG_CANDATA st_delivery_msg;
 154   EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail;
 155   HANDLE h_client;
 156   uint8_t uc_size = uc_dlc;
 157
 158   // Create delivery data
 159   memset(&st_delivery_msg, 0, sizeof(st_delivery_msg));
 160   st_delivery_msg.hdr.hdr.cid = CID_CAN_DATA_DELIVERY;
 161   st_delivery_msg.hdr.hdr.msgbodysize = (uint16_t)(CAN_TRX_CANID_SIZE +
 162                                                  CAN_TRX_DLC_SIZE +
 163                                                  (uint32_t)uc_size);
 164   st_delivery_msg.hdr.hdr.rid = 0;
 165   st_delivery_msg.data.can_id = ul_canid;
 166   st_delivery_msg.data.dlc = uc_size;
 167   st_delivery_msg.echoback = flag;
 168   if (uc_size > (uint8_t)CAN_DATA_SIZE)
 169     uc_size = CAN_DATA_SIZE;
 170   memcpy(st_delivery_msg.data.data, puc_data, (size_t)uc_size);
 171
 172   h_client = CommonFindSender(h_app, dest);
 173   if (!h_client) {
 174     return e_status;
 175   }
 176
 177   FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "can_id=%x dlc=%x echoback=%d",
 178          st_delivery_msg.data.can_id, st_delivery_msg.data.dlc,
 179          st_delivery_msg.echoback);
 180   FRAMEWORKUNIFIEDLOG(ZONE_DEBUG, __func__, "msg_data=%s",
 181          MessageDataOutputLog(st_delivery_msg.data.data, uc_size).c_str());
 182
 183   e_status = FrameworkunifiedSendMsg(h_client, CID_CAN_DATA_DELIVERY,
 184                                    sizeof(st_delivery_msg), &st_delivery_msg);
 185   if (e_status != eFrameworkunifiedStatusOK) {
 186     FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "FrameworkunifiedSendMsg Error(e_status:%d to:%s)",
 187                                e_status, dest.c_str());
 188   } else {
 189     FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__,
 190            "CAN Delivery : dst = %s, CANID = %x, DLC = %x",
 191            dest.c_str(), ul_canid, uc_dlc);
 192   }
 193   return e_status;
 194 }
 195
 196 EFrameworkunifiedStatus CANDeliveryEntry(HANDLE h_app) {
 197   FRAMEWORKUNIFIEDLOG(ZONE_FUNC, __func__, "@@@ Start communication CanRecv");
 198
 199   uint16_t i;
 200   EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusOK;
 201   CAN_DELIVERY_ENTRY rcv_msg;
 202   CAN_DeliveryEntryListIt it;
 203   bool fail = false;
 204
 205   memset(&rcv_msg, 0, sizeof(rcv_msg));
 206   e_status = FrameworkunifiedGetMsgDataOfSize(h_app, &rcv_msg,
 207                                               sizeof(rcv_msg), eSMRRelease);
 208   if (e_status != eFrameworkunifiedStatusOK) {
 209     FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__,
 210           "CAN DeliveryEntry FrameworkunifiedGetMsgDataOfSize Error(%d)", e_status);
 211     if (e_status == eFrameworkunifiedStatusInvldBufSize) {
 212       FrameworkunifiedClearMsgData(h_app);
 213     }
 214     goto cleanup;
 215   }
 216
 217   for (i = 0; i < rcv_msg.usCanNum; i++) {
 218     CANID canid = rcv_msg.ulCanid[i];
 219     CanMessage *latest = CANMsgFind(canid);
 220
 221     if (CANDeliveryInsert(canid, rcv_msg.notifyName)) {
 222       FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "(cnd:%lu) : CANID=0x%x, dst=%s",
 223         g_map_delivery_list_can.size(), canid, rcv_msg.notifyName);
 224     }
 225
 226     if (latest) {
 227       switch (canid) {
 228       case CAN_CMDID_FUELCALC_RST_REQ_RX:
 229       case CAN_CMDID_STARTUP_FIN_RESP_RX:
 230       case CAN_CMDID_MRST_INFO_RESP_RX:
 231       case CAN_CMDID_VERSION_RESP_RX:
 232       case CAN_CMDID_CONNECTION_NODE_RESP_RX:
 233         if (eFrameworkunifiedStatusOK != CANDeliverySndCmdSingle(h_app, canid,
 234                                                  latest, rcv_msg.notifyName)) {
 235           fail = true;
 236         }
 237         break;
 238       default:
 239         if (eFrameworkunifiedStatusOK != CANDeliverySndMsgSingle(h_app, canid,
 240             static_cast<uint8_t>(latest->dlc),
 241             latest->data, CANIF_PURERECV, rcv_msg.notifyName)) {
 242           fail = true;
 243         }
 244         break;
 245       }
 246     }
 247   }
 248
 249   if (fail)
 250     e_status = eFrameworkunifiedStatusFail;
 251
 252 cleanup:
 253   return e_status;
 254 }
 255
 256 static EFrameworkunifiedStatus CANDeliverySndMsgToClient(HANDLE h_app, CANID ul_canid,
 257                         void *data, size_t size, PS_CommunicationProtocol cid) {
 258   EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail;
 259   HANDLE h_client = NULL;
 260   CAN_DeliveryEntryListIt it;
 261   std::pair<CAN_DeliveryEntryListIt, CAN_DeliveryEntryListIt> range;
 262   bool fail = false;
 263
 264   it = g_map_delivery_list_can.find(ul_canid);
 265   range = g_map_delivery_list_can.equal_range(ul_canid);
 266   for (it = range.first; it != range.second; ++it) {
 267     h_client = CommonFindSender(h_app, it->second);
 268     if (!h_client) {
 269       continue;
 270     }
 271
 272     e_status = FrameworkunifiedSendMsg(h_client, cid, (UI_32)size, data);
 273     if (e_status != eFrameworkunifiedStatusOK) {
 274       FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "FrameworkunifiedSendMsg Error(e_status:%d to:%s)",
 275                                               e_status, it->second.c_str());
 276       fail = true;
 277     }
 278   }
 279   return fail ? eFrameworkunifiedStatusFail : eFrameworkunifiedStatusOK;
 280 }
 281
 282 EFrameworkunifiedStatus CANDeliverySndMsg(HANDLE h_app, CANID ul_canid, uint8_t n_ta,
 283         uint8_t uc_dlc, const uint8_t *puc_data,
 284         PS_CommunicationProtocol cid, enum CanIfEchoBackFlags flag) {
 285   CAN_MSG_CANDATA st_delivery_msg;
 286   uint8_t uc_size = uc_dlc;
 287
 288   // Create delivery data
 289   memset(&st_delivery_msg, 0, sizeof(st_delivery_msg));
 290   st_delivery_msg.hdr.hdr.cid = cid;
 291   st_delivery_msg.hdr.hdr.msgbodysize = (uint16_t)(CAN_TRX_CANID_SIZE +
 292                                                  CAN_TRX_DLC_SIZE +
 293                                                  (uint32_t)uc_size);
 294   st_delivery_msg.hdr.hdr.rid = 0;
 295   st_delivery_msg.data.can_id = ul_canid;
 296   st_delivery_msg.data.dlc = uc_size;
 297   st_delivery_msg.echoback = flag;
 298
 299   FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "can_id=%x dlc=%x echoback=%d",
 300          st_delivery_msg.data.can_id, st_delivery_msg.data.dlc,
 301          st_delivery_msg.echoback);
 302   FRAMEWORKUNIFIEDLOG(ZONE_DEBUG, __func__, "msg_data=%s",
 303          MessageDataOutputLog(st_delivery_msg.data.data, uc_size).c_str());
 304
 305
 306   if (uc_size > (uint8_t)CAN_DATA_SIZE)
 307     uc_size = CAN_DATA_SIZE;
 308   memcpy(st_delivery_msg.data.data, puc_data, (size_t)uc_size);
 309
 310   return CANDeliverySndMsgToClient(h_app, ul_canid, &st_delivery_msg,
 311                                    sizeof(st_delivery_msg), cid);
 312 }
 313
 314 EFrameworkunifiedStatus CANCommandDeliveryRcvProcess(HANDLE h_app,
 315                                                  CanMessage *msg, uint8_t cmd) {
 316   EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail;
 317   CAN_MSG_CANCMD st_delivery_msg;
 318   uint16_t len = 0;
 319
 320   memset(&st_delivery_msg, 0, sizeof(st_delivery_msg));
 321   switch (cmd) {
 322   case CAN_CMDID_FUELCALC_RST_REQ_RX:
 323     len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_FUELCALC_RST_SIZE;
 324     break;
 325   case CAN_CMDID_STARTUP_FIN_RESP_RX:
 326     len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_STARTUP_FIN_SIZE;
 327     break;
 328   case CAN_CMDID_MRST_INFO_RESP_RX:
 329     len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_MRST_INFO_SIZE;
 330     break;
 331   case CAN_CMDID_VERSION_RESP_RX:
 332     len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_VERSION_SIZE;
 333     break;
 334   case CAN_CMDID_CONNECTION_NODE_RESP_RX:
 335     len = (uint16_t)(CAN_TX_CMD_DELIVERY_SIZE + msg->data[0] + 1);
 336     break;
 337   default:
 338     return e_status;
 339   }
 340   st_delivery_msg.hdr.hdr.rid = 0;
 341   st_delivery_msg.hdr.hdr.cid = (uint16_t)CID_CAN_CMD_DELIVERY;
 342   st_delivery_msg.hdr.hdr.msgbodysize = len;
 343   st_delivery_msg.data.cmd_id = cmd;
 344   memcpy(&st_delivery_msg.data.data, msg->data,
 345                                      (size_t)(len - CAN_TX_CMD_DELIVERY_SIZE));
 346   CANMsgInsert(cmd, msg);
 347
 348   FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "cid=%x msgbodysize=%x cmd_id=%x",
 349          st_delivery_msg.hdr.hdr.cid, st_delivery_msg.hdr.hdr.msgbodysize,
 350          st_delivery_msg.data.cmd_id);
 351   FRAMEWORKUNIFIEDLOG(ZONE_DEBUG, __func__, "msg_data=%s",
 352          MessageDataOutputLog(st_delivery_msg.data.data, len).c_str());
 353
 354   return CANDeliverySndMsgToClient(h_app, msg->can_id, &st_delivery_msg,
 355                                  sizeof(st_delivery_msg), CID_CAN_CMD_DELIVERY);
 356 }
 357
 358 EFrameworkunifiedStatus CANDeliveryRcvProcess(HANDLE h_app, CanMessage *msg) {
 359   if (!Canif_CheckCanID(msg->can_id)) {
 360     FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "CANID is invalid (%x)", msg->can_id);
 361     return eFrameworkunifiedStatusFail;
 362   }
 363
 364   CANCommWatchClear(h_app, msg->can_id);
 365   CANMsgInsert(msg->can_id, msg);
 366   if (eFrameworkunifiedStatusOK != CANDeliverySndMsg(h_app, msg->can_id, CAN_NTA_INVALID,
 367                        (uint8_t)msg->dlc, msg->data, CID_CAN_DATA_DELIVERY)) {
 368     FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "CAN Delivery Error");
 369     return eFrameworkunifiedStatusFail;
 370   }
 371
 372   return eFrameworkunifiedStatusOK;
 373 }
 374
 375 EFrameworkunifiedStatus CANClearEntry(HANDLE h_app) {
 376   char notify_name[CANIF_NOTIFY_NAME_MAX_SIZE + 1] = {0};
 377   CAN_DeliveryEntryListIt it;
 378   EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusOK;
 379
 380   e_status = FrameworkunifiedGetMsgDataOfSize(h_app, &notify_name,
 381                                  CANIF_NOTIFY_NAME_MAX_SIZE, eSMRRelease);
 382   if (e_status != eFrameworkunifiedStatusOK) {
 383     FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__,
 384                 "#CAN thread# FrameworkunifiedGetMsgDataOfSize Error(%d)", e_status);
 385     if (e_status == eFrameworkunifiedStatusInvldBufSize) {
 386       FrameworkunifiedClearMsgData(h_app);
 387     }
 388     return e_status;
 389   }
 390
 391   FRAMEWORKUNIFIEDLOG(ZONE_DEBUG, __func__, "notify_name=%s", notify_name);
 392
 393   // To avoid destruction of iterator.
 394   it = g_map_delivery_list_can.begin();
 395   while (it != g_map_delivery_list_can.end()) {
 396     size_t n = strnlen(it->second.c_str(), CANIF_NOTIFY_NAME_MAX_SIZE);
 397     if (!strncmp(it->second.c_str(), notify_name, n)) {
 398       g_map_delivery_list_can.erase(it++);
 399     } else {
 400       ++it;
 401     }
 402   }
 403
 404   return eFrameworkunifiedStatusOK;
 405 }