1 files changed, 405 insertions, 0 deletions
diff --git a/peripheralservice/communication/server/src/CAN/Delivery/CAN_Delivery.cpp b/peripheralservice/communication/server/src/CAN/Delivery/CAN_Delivery.cpp
new file mode 100644
index 00000000..b9914313
--- /dev/null
+++ b/peripheralservice/communication/server/src/CAN/Delivery/CAN_Delivery.cpp
@@ -0,0 +1,405 @@
+/*
+ * @copyright Copyright (c) 2016-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 "CAN_Delivery.h"
+
+#include <can_hal.h>
+#include <native_service/frameworkunified_framework_if.h>
+#include <peripheral_service/Canif_API.h>
+#include <can_hal.h>
+#include <utility>
+#include <map>
+#include <string>
+#include "CAN_TxMsg.h"
+#include "CAN_CommWatch.h"
+#include "Canif_API_Local.h"
+#include "API_Local_Common.h"
+#include "Thread_Common.h"
+
+static CAN_DeliveryEntryList g_map_delivery_list_can;
+// Member of g_msg_rx_msg exist as long as communication exists.
+static std::map<CANID, CanMessage *> g_can_rx_msg;
+
+void CANDeliveryInit(void) {
+  g_map_delivery_list_can.clear();
+  return;
+}
+
+static CanMessage *CANMsgFind(CANID k) {
+  std::map<CANID, CanMessage *>::iterator it;
+
+  it = g_can_rx_msg.find(k);
+  if (it == g_can_rx_msg.end())
+    return NULL;
+  return it->second;
+}
+
+static void CANMsgInsert(CANID k, CanMessage *msg) {
+  CanMessage *latest = CANMsgFind(k);
+  if (!latest) {
+    latest = new CanMessage();
+  }
+  g_can_rx_msg[k] = latest;
+  memcpy(latest, msg, sizeof(CanMessage));
+}
+
+static CAN_DeliveryEntryListIt CANDeliveryFind(CANID canid, std::string s) {
+  std::pair<CAN_DeliveryEntryListIt, CAN_DeliveryEntryListIt> range;
+  CAN_DeliveryEntryListPair p = std::make_pair(canid, s);
+  CAN_DeliveryEntryListIt it;
+  bool found = false;
+  range = g_map_delivery_list_can.equal_range(canid);
+  for (it = range.first; it != range.second; ++it) {
+    if (*it == p) {
+      found = true;
+      break;
+    }
+  } 
+
+  if (!found)
+    it = g_map_delivery_list_can.end();
+
+  return it;
+}
+
+bool CANDeliveryInsert(CANID canid, std::string s) {
+  CAN_DeliveryEntryListIt it = CANDeliveryFind(canid, s);
+  CAN_DeliveryEntryListPair p = std::make_pair(canid, s);
+  bool inserted = false;
+
+  if (it == g_map_delivery_list_can.end()) {
+    g_map_delivery_list_can.insert(p);
+    inserted = true;
+  }
+
+  return inserted;
+}
+
+static EFrameworkunifiedStatus CANDeliverySndCmdSingle(HANDLE h_app, CANID cmd,
+                                          CanMessage *msg, std::string dest) {
+  CAN_MSG_CANCMD st_delivery_msg;
+  EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail;
+  HANDLE h_client;
+  uint16_t len = 0;
+
+  // Create delivery data
+  memset(&st_delivery_msg, 0, sizeof(st_delivery_msg));
+  switch (cmd) {
+  case CAN_CMDID_FUELCALC_RST_REQ_RX:
+    len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_FUELCALC_RST_SIZE;
+    break;
+  case CAN_CMDID_STARTUP_FIN_RESP_RX:
+    len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_STARTUP_FIN_SIZE;
+    break;
+  case CAN_CMDID_MRST_INFO_RESP_RX:
+    len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_MRST_INFO_SIZE;
+    break;
+  case CAN_CMDID_VERSION_RESP_RX:
+    len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_VERSION_SIZE;
+    break;
+  case CAN_CMDID_CONNECTION_NODE_RESP_RX:
+    len = (uint16_t)(CAN_TX_CMD_DELIVERY_SIZE + msg->data[0] + 1);
+    break;
+  default:
+    return e_status;
+  }
+  st_delivery_msg.hdr.hdr.rid = 0;
+  st_delivery_msg.hdr.hdr.cid = (uint16_t)CID_CAN_CMD_DELIVERY;
+  st_delivery_msg.hdr.hdr.msgbodysize = len;
+  st_delivery_msg.data.cmd_id = (uint8_t)cmd;
+  memcpy(&st_delivery_msg.data.data, msg->data,
+                                     (size_t)(len - CAN_TX_CMD_DELIVERY_SIZE));
+
+  h_client = CommonFindSender(h_app, dest);
+  if (!h_client) {
+    return e_status;
+  }
+
+  FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "cid=%x msgbodysize=%x cmd_id=%x",
+         st_delivery_msg.hdr.hdr.cid, st_delivery_msg.hdr.hdr.msgbodysize,
+         st_delivery_msg.data.cmd_id);
+  FRAMEWORKUNIFIEDLOG(ZONE_DEBUG, __func__, "msg_data=%s",
+         MessageDataOutputLog(st_delivery_msg.data.data, len).c_str());
+
+  e_status = FrameworkunifiedSendMsg(h_client, CID_CAN_CMD_DELIVERY,
+                                   sizeof(st_delivery_msg), &st_delivery_msg);
+  if (e_status != eFrameworkunifiedStatusOK) {
+    FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "FrameworkunifiedSendMsg Error(e_status:%d to:%s)",
+                               e_status, dest.c_str());
+  } else {
+    FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__,
+           "CAN Delivery : dst = %s, CMD = %x, DLC = %x",
+           dest.c_str(), cmd, msg->dlc);
+  }
+  return e_status;
+}
+
+static EFrameworkunifiedStatus CANDeliverySndMsgSingle(HANDLE h_app, CANID ul_canid,
+        uint8_t uc_dlc, const uint8_t *puc_data,
+        enum CanIfEchoBackFlags flag, std::string dest) {
+  CAN_MSG_CANDATA st_delivery_msg;
+  EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail;
+  HANDLE h_client;
+  uint8_t uc_size = uc_dlc;
+
+  // Create delivery data
+  memset(&st_delivery_msg, 0, sizeof(st_delivery_msg));
+  st_delivery_msg.hdr.hdr.cid = CID_CAN_DATA_DELIVERY;
+  st_delivery_msg.hdr.hdr.msgbodysize = (uint16_t)(CAN_TRX_CANID_SIZE + 
+                                                 CAN_TRX_DLC_SIZE + 
+                                                 (uint32_t)uc_size);
+  st_delivery_msg.hdr.hdr.rid = 0;
+  st_delivery_msg.data.can_id = ul_canid;
+  st_delivery_msg.data.dlc = uc_size;
+  st_delivery_msg.echoback = flag;
+  if (uc_size > (uint8_t)CAN_DATA_SIZE)
+    uc_size = CAN_DATA_SIZE;
+  memcpy(st_delivery_msg.data.data, puc_data, (size_t)uc_size);
+
+  h_client = CommonFindSender(h_app, dest);
+  if (!h_client) {
+    return e_status;
+  }
+
+  FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "can_id=%x dlc=%x echoback=%d",
+         st_delivery_msg.data.can_id, st_delivery_msg.data.dlc,
+         st_delivery_msg.echoback);
+  FRAMEWORKUNIFIEDLOG(ZONE_DEBUG, __func__, "msg_data=%s",
+         MessageDataOutputLog(st_delivery_msg.data.data, uc_size).c_str());
+
+  e_status = FrameworkunifiedSendMsg(h_client, CID_CAN_DATA_DELIVERY,
+                                   sizeof(st_delivery_msg), &st_delivery_msg);
+  if (e_status != eFrameworkunifiedStatusOK) {
+    FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "FrameworkunifiedSendMsg Error(e_status:%d to:%s)",
+                               e_status, dest.c_str());
+  } else {
+    FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__,
+           "CAN Delivery : dst = %s, CANID = %x, DLC = %x",
+           dest.c_str(), ul_canid, uc_dlc);
+  }
+  return e_status;
+}
+
+EFrameworkunifiedStatus CANDeliveryEntry(HANDLE h_app) {
+  FRAMEWORKUNIFIEDLOG(ZONE_FUNC, __func__, "@@@ Start communication CanRecv");
+
+  uint16_t i;
+  EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusOK;
+  CAN_DELIVERY_ENTRY rcv_msg;
+  CAN_DeliveryEntryListIt it;
+  bool fail = false;
+
+  memset(&rcv_msg, 0, sizeof(rcv_msg));
+  e_status = FrameworkunifiedGetMsgDataOfSize(h_app, &rcv_msg,
+                                              sizeof(rcv_msg), eSMRRelease);
+  if (e_status != eFrameworkunifiedStatusOK) {
+    FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__,
+          "CAN DeliveryEntry FrameworkunifiedGetMsgDataOfSize Error(%d)", e_status);
+    if (e_status == eFrameworkunifiedStatusInvldBufSize) {
+      FrameworkunifiedClearMsgData(h_app);
+    }
+    goto cleanup;
+  }
+
+  for (i = 0; i < rcv_msg.usCanNum; i++) {
+    CANID canid = rcv_msg.ulCanid[i];
+    CanMessage *latest = CANMsgFind(canid);
+
+    if (CANDeliveryInsert(canid, rcv_msg.notifyName)) {
+      FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "(cnd:%lu) : CANID=0x%x, dst=%s",
+        g_map_delivery_list_can.size(), canid, rcv_msg.notifyName);
+    }
+
+    if (latest) {
+      switch (canid) {
+      case CAN_CMDID_FUELCALC_RST_REQ_RX:
+      case CAN_CMDID_STARTUP_FIN_RESP_RX:
+      case CAN_CMDID_MRST_INFO_RESP_RX:
+      case CAN_CMDID_VERSION_RESP_RX:
+      case CAN_CMDID_CONNECTION_NODE_RESP_RX:
+        if (eFrameworkunifiedStatusOK != CANDeliverySndCmdSingle(h_app, canid,
+                                                 latest, rcv_msg.notifyName)) {
+          fail = true;
+        }
+        break;
+      default:
+        if (eFrameworkunifiedStatusOK != CANDeliverySndMsgSingle(h_app, canid,
+            static_cast<uint8_t>(latest->dlc), 
+            latest->data, CANIF_PURERECV, rcv_msg.notifyName)) {
+          fail = true;
+        }
+        break;
+      }
+    }
+  }
+
+  if (fail)
+    e_status = eFrameworkunifiedStatusFail;
+
+cleanup:
+  return e_status;
+}
+
+static EFrameworkunifiedStatus CANDeliverySndMsgToClient(HANDLE h_app, CANID ul_canid, 
+                        void *data, size_t size, PS_CommunicationProtocol cid) {
+  EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail;
+  HANDLE h_client = NULL;
+  CAN_DeliveryEntryListIt it;
+  std::pair<CAN_DeliveryEntryListIt, CAN_DeliveryEntryListIt> range;
+  bool fail = false;
+
+  it = g_map_delivery_list_can.find(ul_canid);
+  range = g_map_delivery_list_can.equal_range(ul_canid);
+  for (it = range.first; it != range.second; ++it) {
+    h_client = CommonFindSender(h_app, it->second);
+    if (!h_client) {
+      continue;
+    }
+
+    e_status = FrameworkunifiedSendMsg(h_client, cid, (UI_32)size, data);
+    if (e_status != eFrameworkunifiedStatusOK) {
+      FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "FrameworkunifiedSendMsg Error(e_status:%d to:%s)",
+                                              e_status, it->second.c_str());
+      fail = true;
+    }
+  }
+  return fail ? eFrameworkunifiedStatusFail : eFrameworkunifiedStatusOK;
+}
+
+EFrameworkunifiedStatus CANDeliverySndMsg(HANDLE h_app, CANID ul_canid, uint8_t n_ta,
+        uint8_t uc_dlc, const uint8_t *puc_data,
+        PS_CommunicationProtocol cid, enum CanIfEchoBackFlags flag) {
+  CAN_MSG_CANDATA st_delivery_msg;
+  uint8_t uc_size = uc_dlc;
+
+  // Create delivery data
+  memset(&st_delivery_msg, 0, sizeof(st_delivery_msg));
+  st_delivery_msg.hdr.hdr.cid = cid;
+  st_delivery_msg.hdr.hdr.msgbodysize = (uint16_t)(CAN_TRX_CANID_SIZE + 
+                                                 CAN_TRX_DLC_SIZE + 
+                                                 (uint32_t)uc_size);
+  st_delivery_msg.hdr.hdr.rid = 0;
+  st_delivery_msg.data.can_id = ul_canid;
+  st_delivery_msg.data.dlc = uc_size;
+  st_delivery_msg.echoback = flag;
+ 
+  FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "can_id=%x dlc=%x echoback=%d",
+         st_delivery_msg.data.can_id, st_delivery_msg.data.dlc,
+         st_delivery_msg.echoback);
+  FRAMEWORKUNIFIEDLOG(ZONE_DEBUG, __func__, "msg_data=%s",
+         MessageDataOutputLog(st_delivery_msg.data.data, uc_size).c_str());
+
+ 
+  if (uc_size > (uint8_t)CAN_DATA_SIZE)
+    uc_size = CAN_DATA_SIZE;
+  memcpy(st_delivery_msg.data.data, puc_data, (size_t)uc_size);
+
+  return CANDeliverySndMsgToClient(h_app, ul_canid, &st_delivery_msg,
+                                   sizeof(st_delivery_msg), cid);
+}
+
+EFrameworkunifiedStatus CANCommandDeliveryRcvProcess(HANDLE h_app,
+                                                 CanMessage *msg, uint8_t cmd) {
+  EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail;
+  CAN_MSG_CANCMD st_delivery_msg;
+  uint16_t len = 0;
+
+  memset(&st_delivery_msg, 0, sizeof(st_delivery_msg));
+  switch (cmd) {
+  case CAN_CMDID_FUELCALC_RST_REQ_RX:
+    len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_FUELCALC_RST_SIZE;
+    break;
+  case CAN_CMDID_STARTUP_FIN_RESP_RX:
+    len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_STARTUP_FIN_SIZE;
+    break;
+  case CAN_CMDID_MRST_INFO_RESP_RX:
+    len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_MRST_INFO_SIZE;
+    break;
+  case CAN_CMDID_VERSION_RESP_RX:
+    len = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_VERSION_SIZE;
+    break;
+  case CAN_CMDID_CONNECTION_NODE_RESP_RX:
+    len = (uint16_t)(CAN_TX_CMD_DELIVERY_SIZE + msg->data[0] + 1);
+    break;
+  default:
+    return e_status;
+  }
+  st_delivery_msg.hdr.hdr.rid = 0;
+  st_delivery_msg.hdr.hdr.cid = (uint16_t)CID_CAN_CMD_DELIVERY;
+  st_delivery_msg.hdr.hdr.msgbodysize = len;
+  st_delivery_msg.data.cmd_id = cmd;
+  memcpy(&st_delivery_msg.data.data, msg->data,
+                                     (size_t)(len - CAN_TX_CMD_DELIVERY_SIZE));
+  CANMsgInsert(cmd, msg);
+
+  FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "cid=%x msgbodysize=%x cmd_id=%x",
+         st_delivery_msg.hdr.hdr.cid, st_delivery_msg.hdr.hdr.msgbodysize,
+         st_delivery_msg.data.cmd_id);
+  FRAMEWORKUNIFIEDLOG(ZONE_DEBUG, __func__, "msg_data=%s",
+         MessageDataOutputLog(st_delivery_msg.data.data, len).c_str());
+
+  return CANDeliverySndMsgToClient(h_app, msg->can_id, &st_delivery_msg,
+                                 sizeof(st_delivery_msg), CID_CAN_CMD_DELIVERY);
+}
+
+EFrameworkunifiedStatus CANDeliveryRcvProcess(HANDLE h_app, CanMessage *msg) {
+  if (!Canif_CheckCanID(msg->can_id)) {
+    FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "CANID is invalid (%x)", msg->can_id);
+    return eFrameworkunifiedStatusFail;
+  }
+
+  CANCommWatchClear(h_app, msg->can_id);
+  CANMsgInsert(msg->can_id, msg);
+  if (eFrameworkunifiedStatusOK != CANDeliverySndMsg(h_app, msg->can_id, CAN_NTA_INVALID,
+                       (uint8_t)msg->dlc, msg->data, CID_CAN_DATA_DELIVERY)) {
+    FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "CAN Delivery Error");
+    return eFrameworkunifiedStatusFail;
+  }
+
+  return eFrameworkunifiedStatusOK;
+}
+
+EFrameworkunifiedStatus CANClearEntry(HANDLE h_app) {
+  char notify_name[CANIF_NOTIFY_NAME_MAX_SIZE + 1] = {0};
+  CAN_DeliveryEntryListIt it;
+  EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusOK;
+
+  e_status = FrameworkunifiedGetMsgDataOfSize(h_app, &notify_name,
+                                 CANIF_NOTIFY_NAME_MAX_SIZE, eSMRRelease);
+  if (e_status != eFrameworkunifiedStatusOK) {
+    FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__,
+                "#CAN thread# FrameworkunifiedGetMsgDataOfSize Error(%d)", e_status);
+    if (e_status == eFrameworkunifiedStatusInvldBufSize) {
+      FrameworkunifiedClearMsgData(h_app);
+    }
+    return e_status;
+  }
+
+  FRAMEWORKUNIFIEDLOG(ZONE_DEBUG, __func__, "notify_name=%s", notify_name);
+
+  // To avoid destruction of iterator.
+  it = g_map_delivery_list_can.begin();
+  while (it != g_map_delivery_list_can.end()) {
+    size_t n = strnlen(it->second.c_str(), CANIF_NOTIFY_NAME_MAX_SIZE);
+    if (!strncmp(it->second.c_str(), notify_name, n)) {
+      g_map_delivery_list_can.erase(it++);
+    } else {
+      ++it;
+    }
+  }
+
+  return eFrameworkunifiedStatusOK;
+}