diff options
Diffstat (limited to 'peripheralservice/communication/server/src/CAN')
3 files changed, 1061 insertions, 0 deletions
diff --git a/peripheralservice/communication/server/src/CAN/CommWatch/CAN_CommWatch.cpp b/peripheralservice/communication/server/src/CAN/CommWatch/CAN_CommWatch.cpp new file mode 100644 index 00000000..2ece1072 --- /dev/null +++ b/peripheralservice/communication/server/src/CAN/CommWatch/CAN_CommWatch.cpp @@ -0,0 +1,240 @@ +/* + * @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_CommWatch.h" + +#include <native_service/frameworkunified_types.h> +#include <native_service/frameworkunified_framework_if.h> +#include <peripheral_service/Canif_API.h> +#include <utility> +#include <map> +#include <string> +#include "CAN_Thread.h" +#include "Thread_Common.h" + +static CAN_CommWatchTable g_map_comm_watch_list_can; + +void CANCommWatchInit(void) { + g_map_comm_watch_list_can.clear(); + return; +} + +static CAN_CommWatchTableIt CANCommWatchFind(CANID k, CAN_COMM_WATCH_VAL *val) { + std::pair<CAN_CommWatchTableIt, CAN_CommWatchTableIt> range; + CAN_CommWatchTableIt it; + bool found = false; + + range = g_map_comm_watch_list_can.equal_range(k); + for (it = range.first; it != range.second; it++) { + if (!memcmp(&(it->second.notify_name), + val->notify_name, sizeof(val->notify_name))) { + found = true; + break; + } + } + + if (!found) + it = g_map_comm_watch_list_can.end(); + + return it; +} + +static bool CANCommWatchInsert(CANID k, CAN_COMM_WATCH_VAL *val) { + CAN_CommWatchTableIt it = CANCommWatchFind(k, val); + CAN_CommWatchTablePair p = std::make_pair((const CANID)k, *val); + bool inserted = false; + + if (it == g_map_comm_watch_list_can.end()) { + g_map_comm_watch_list_can.insert(p); + inserted = true; + } + return inserted; +} + +static void CANCommWatchUpdate(HANDLE h_app, CANID key, + CAN_COMM_WATCH_VAL *val) { + if (CANCommWatchInsert(key, val)) { + FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "Can CommWatch : " + "Entry (CANID=%x, DataID=%x, dst=%s, time=%d, cnt=%lu)", + key, val->data_id, + val->notify_name, val->set_time, + g_map_comm_watch_list_can.size()); + } else { + CAN_CommWatchTableIt it = CANCommWatchFind(key, val); + FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "Can CommWatch : Update the time " + "(CANID=%x, DataID=%x, dst=%s, time:%d -> %d, cnt =%lu)", + key, val->data_id, val->notify_name, + it->second.set_time, val->set_time, + g_map_comm_watch_list_can.size()); + it->second.comm_watch_flag = val->comm_watch_flag; + it->second.set_time = val->set_time; + it->second.timer_cnt = val->timer_cnt; + it->second.data_id = val->data_id; + } + + return; +} + +static void CANCommWatchStop(HANDLE h_app, CANID key, + CAN_COMM_WATCH_VAL *val) { + CAN_CommWatchTableIt it = CANCommWatchFind(key, val); + if (it != g_map_comm_watch_list_can.end()) { + g_map_comm_watch_list_can.erase(it); + FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "Can CommWatch : Stop the commwatch" + "(CANID=%x, dst=%s, cnt=%lu)", + key, val->notify_name, g_map_comm_watch_list_can.size()); + } + + return; +} + +EFrameworkunifiedStatus CANCommWatch(HANDLE h_app) { + FRAMEWORKUNIFIEDLOG(ZONE_FUNC, __func__, "@@@ Start communication CanCommWatch"); + + EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail; + CAN_COMM_WATCHEXT_MSG_DAT rcv_msg; + CANID key; + CAN_COMM_WATCH_VAL val; + CAN_CommWatchTableIt it; + + memset(&rcv_msg, 0, sizeof(rcv_msg)); + memset(&val, 0, sizeof(val)); + e_status = FrameworkunifiedGetMsgDataOfSize(h_app, &rcv_msg, sizeof(rcv_msg), eSMRRelease); + if (e_status != eFrameworkunifiedStatusOK) { + FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "FrameworkunifiedGetMsgDataOfSize Error(%d)", e_status); + if (e_status == eFrameworkunifiedStatusInvldBufSize) { + FrameworkunifiedClearMsgData(h_app); + } + goto cleanup; + } + + // Create search data + key = rcv_msg.ulCanid; + snprintf(val.notify_name, sizeof(val.notify_name), "%s", rcv_msg.notifyName); + val.data_id = rcv_msg.ulDid; + val.comm_watch_flag = CAN_COMM_NORMAL; + val.set_time = rcv_msg.usWatchTime; + val.timer_cnt = rcv_msg.usWatchTime; + FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, + "CAN CommWatch : dst=%s, CANID=%x, dataID=%x, time=%d", + rcv_msg.notifyName, + rcv_msg.ulCanid, + rcv_msg.ulDid, rcv_msg.usWatchTime); + + if (0 == rcv_msg.usWatchTime) { + CANCommWatchStop(h_app, key, &val); + } else { + CANCommWatchUpdate(h_app, key, &val); + } + e_status = eFrameworkunifiedStatusOK; +cleanup: + return e_status; +} + +static EFrameworkunifiedStatus CANCommWatchSndMsg(HANDLE h_app, + CAN_COMM_WATCH_VAL *v, uint32_t cid) { + EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail; + CAN_MSG_COMM_WATCHSTS msg; + HANDLE h_client = NULL; + + memset(&msg, 0, sizeof(msg)); + msg.hdr.hdr.cid = (uint16_t)cid; + msg.hdr.hdr.msgbodysize = sizeof(CAN_MSG_COMM_WATCHSTS_DAT); + msg.hdr.hdr.rid = 0; + msg.data.ulDid = v->data_id; + + FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "cid=%x msgbodysize=%x ulDid=%x", + msg.hdr.hdr.cid, msg.hdr.hdr.msgbodysize, msg.data.ulDid); + + h_client = CommonFindSender(h_app, v->notify_name); + if (!h_client){ + FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "CommonFindSender failed"); + goto cleanup; + } + + e_status = FrameworkunifiedSendMsg(h_client, cid, sizeof(msg), &msg); + if (e_status != eFrameworkunifiedStatusOK) { + FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, + "FrameworkunifiedSendMSg Error(e_status:%d to %s)", + e_status, v->notify_name); + goto cleanup; + } + + e_status = eFrameworkunifiedStatusOK; +cleanup: + return e_status; +} + +static void CANCommWatchTimeoutCore(HANDLE h_app, CAN_COMM_WATCH_VAL *v) { + EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail; + + if (CAN_COMM_NORMAL != v->comm_watch_flag) + return; + + if (v->timer_cnt != 0) + v->timer_cnt--; + + if (v->timer_cnt != 0) + return; + + e_status = CANCommWatchSndMsg(h_app, v, CID_CAN_COMM_STOP); + if (eFrameworkunifiedStatusOK == e_status) { + v->comm_watch_flag = CAN_COMM_STOP; + } else { + FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "CANCommWatchSndMsg failed"); + // retry at next timeout. + } +} + +EFrameworkunifiedStatus CANCommWatchTimeout(HANDLE h_app) { + CAN_CommWatchTableIt it; + + for (it = g_map_comm_watch_list_can.begin(); + it != g_map_comm_watch_list_can.end(); it++) { + CANCommWatchTimeoutCore(h_app, &(it->second)); + } + + return eFrameworkunifiedStatusOK; +} + +static void CANCommWatchClearCore(HANDLE h_app, CANID id, + CAN_COMM_WATCH_VAL *v) { + EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail; + + v->timer_cnt = v->set_time; + + if (CAN_COMM_STOP != v->comm_watch_flag) + return; + + e_status = CANCommWatchSndMsg(h_app, v, CID_CAN_COMM_RESTORE); + if (eFrameworkunifiedStatusOK == e_status) { + v->comm_watch_flag = CAN_COMM_NORMAL; + FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "Can CommWatch Clear : CANID=%x", id); + } else { + FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "CANCommWatchSndMsg failed"); + // retry at next timeout. + } +} + +void CANCommWatchClear(HANDLE h_app, CANID id) { + std::pair<CAN_CommWatchTableIt, CAN_CommWatchTableIt> range; + CAN_CommWatchTableIt it; + + range = g_map_comm_watch_list_can.equal_range(id); + for (it = range.first; it != range.second; it++) { + CANCommWatchClearCore(h_app, id, &(it->second)); + } +} 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, ¬ify_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; +} diff --git a/peripheralservice/communication/server/src/CAN/TxMsg/CAN_TxMsg.cpp b/peripheralservice/communication/server/src/CAN/TxMsg/CAN_TxMsg.cpp new file mode 100644 index 00000000..fa5d61d6 --- /dev/null +++ b/peripheralservice/communication/server/src/CAN/TxMsg/CAN_TxMsg.cpp @@ -0,0 +1,416 @@ +/* + * @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_TxMsg.h" +#include "CAN_Delivery.h" +#include <string.h> +#include <native_service/frameworkunified_types.h> +#include <native_service/frameworkunified_framework_if.h> +#include <peripheral_service/Canif_API.h> +#include <can_hal.h> +#include <map> +#include <string> +#include "CAN_Thread.h" +#include "Thread_Common.h" +#include "Canif_API_Local.h" +#include "CAN_TxMsg.h" +#include "Canif_TransmissionData.h" + +static std::map<CANID, CAN_SEND_STATUS_DAT> g_m_send_sts; +static bool can_cmd_rst_rcv = false; +static std::map<CANID, CAN_TRANS_START_TABLE_VAL *> g_map_trans_data; + +void CANTxMsgInit(void) { + g_m_send_sts.clear(); + uint32_t i; + + for (i = 0; i < _countof(Can_TransInitData_21PF); i++) { + CANID canid = Can_TransInitData_21PF[i].canid; + CAN_TRANS_START_TABLE_VAL *val = &(Can_TransInitData_21PF[i].val); + g_map_trans_data[canid] = val; + } + + return; +} + +static bool CANCommandFuelCalcRstReqCheck(void) { + return can_cmd_rst_rcv; +} + +void CANCommandSetFuelCalcRstReq(void) { + can_cmd_rst_rcv = true; +} + +static void CANTxMsgErrorLog(const char *reason, const char *func, + CANID id, uint16_t len, HANDLE h_app) { + FRAMEWORKUNIFIEDLOG(ZONE_ERR, func, "%s Frame(0x%x) length=%hu from %s is ignored.", + reason, id, len, FrameworkunifiedGetMsgSrc(h_app)); +} + +static EFrameworkunifiedStatus CANCommandResponse(HANDLE h_app, CAN_MSG_CANCMD *msg, + size_t len, char *notifyName) { + HANDLE h_client = NULL; + EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail; + + h_client = CommonFindSender(h_app, notifyName); + if (!h_client) { + FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "Cannot open sender handle."); + return e_status; + } + + msg->hdr.hdr.cid = (uint16_t)CID_CAN_CMD_DELIVERY; + e_status = FrameworkunifiedSendMsg(h_client, CID_CAN_CMD_DELIVERY, (uint32_t)len, msg); + if (e_status != eFrameworkunifiedStatusOK) + FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "FrameworkunifiedSendMsg is failed."); + + return e_status; +} + +static EFrameworkunifiedStatus CANCommandVersionReq(HANDLE h_app, char *notifyName) { + EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail; + CANHAL_RET_API ret = CANHAL_RET_NORMAL; + std::string version_info; + CAN_MSG_CANCMD msg; + + memset(&msg, 0, sizeof(msg)); + + FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "@@@ Call CanGetVersion for can_hal"); + ret = CanGetVersion(h_app, &version_info); + if (ret != CANHAL_RET_NORMAL) { + FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "Cannot get version from can_hal."); + return e_status; + } + + if (CAN_TX_CMD_VERSION_SIZE != version_info.length()) { + FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "Invalid length of version."); + return e_status; + } + + msg.hdr.hdr.rid = 0; + msg.hdr.hdr.msgbodysize = CAN_TX_CMD_DELIVERY_SIZE + CAN_TX_CMD_VERSION_SIZE; + msg.data.cmd_id = CAN_CMDID_VERSION_RESP_RX; + strncpy((char *)msg.data.data, version_info.c_str(), CAN_TX_CMD_VERSION_SIZE); + return CANCommandResponse(h_app, &msg, sizeof(msg), notifyName); +} + +static EFrameworkunifiedStatus CANCommandFuelcalcRstReq(HANDLE h_app, char *notifyName) { + EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail; + if (CANCommandFuelCalcRstReqCheck()) { + // CAN_CMDID_FUELCALC_RST_REQ_RX is already received from can_hal. + // so response it in direct. + CAN_MSG_CANCMD msg; + memset(&msg, 0, sizeof(msg)); + msg.hdr.hdr.rid = 0; + msg.hdr.hdr.msgbodysize = CAN_TX_CMD_DELIVERY_SIZE + + CAN_TX_CMD_FUELCALC_RST_SIZE; + msg.data.cmd_id = CAN_CMDID_FUELCALC_RST_REQ_RX; + e_status = CANCommandResponse(h_app, &msg, sizeof(msg), notifyName); + } else { + CANDeliveryInsert(CAN_CMDID_FUELCALC_RST_REQ_RX, notifyName); + e_status = eFrameworkunifiedStatusOK; + } + return e_status; +} + +static EFrameworkunifiedStatus CANCommandOther(HANDLE h_app, char *notifyName, + uint8_t cmd, uint8_t rid) { + CanMessage msg; + CANID wait = 0; + CANID can_id = (CANID)cmd; + + memset(&msg, 0, sizeof(msg)); + switch (cmd) { + case CAN_CMDID_STARTUP_FIN_REQ_TX: + wait = CAN_CMDID_STARTUP_FIN_RESP_RX; + break; + case CAN_CMDID_MRST_INFO_REQ_TX: + wait = CAN_CMDID_MRST_INFO_RESP_RX; + break; + case CAN_CMDID_CONNECTION_NODE_REQ_TX: + wait = CAN_CMDID_CONNECTION_NODE_RESP_RX; + break; + case CAN_CMDID_FUELCALC_REQ_TX: + wait = CAN_CMDID_FUELCALC_RST_REQ_RX; + break; + default: + CANTxMsgErrorLog("Unsupported cmd requested", __func__, cmd, 0, h_app); + return eFrameworkunifiedStatusFail; + } + + if (cmd != CAN_CMDID_FUELCALC_REQ_TX) { + CANDeliveryInsert(wait, notifyName); + } + + if (rid != CAN_RID_NOTUSE_CODE) { + // cmd can be assumed same as can_id. + if (0 < g_m_send_sts.count(can_id)) { + if (g_m_send_sts[can_id].notify_name[0] != 0x00) { + FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, + "Not yet deliver SendSts but updated(SendID=%x, CAN ID=%x, Nane=%s)", + g_m_send_sts[can_id].rid, can_id, g_m_send_sts[can_id].notify_name); + } + } + g_m_send_sts[can_id].rid = rid; + memcpy(g_m_send_sts[can_id].notify_name, notifyName, + sizeof(g_m_send_sts[can_id].notify_name)); + } + + msg.can_id = (CANID)cmd; + msg.rid = rid; + msg.dlc = (uint8_t)CAN_CMDSND_DATA_SIZE; + + FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "@@@ Call CanSend for can_hal"); + if (CANHAL_RET_NORMAL != CanSend(h_app, &msg, CAN_HAL_TYPE_CAN)) { + CANTxMsgErrorLog("CAN CanSend failed", __func__, msg.can_id, + (uint16_t)msg.dlc, h_app); + return eFrameworkunifiedStatusFail; + } + + return eFrameworkunifiedStatusOK; +} + +EFrameworkunifiedStatus CANTxMsgCommand(HANDLE h_app) { + FRAMEWORKUNIFIEDLOG(ZONE_FUNC, __func__, "@@@ Start communication CanCommandCtl"); + + EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail; + CAN_CMD_CTRL_MSG_DAT rcv_msg; + + 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__, "FrameworkunifiedGetMsgDataOfSize Error(%d)", e_status); + if (e_status == eFrameworkunifiedStatusInvldBufSize) + FrameworkunifiedClearMsgData(h_app); + return e_status; + } + + FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "notify_name=%s ulEvtId=%x ucRid=%x ucCmdid=%x", + rcv_msg.notifyName, rcv_msg.ulEvtId, rcv_msg.ucRid, rcv_msg.ucCmdid); + + switch (rcv_msg.ucCmdid) { + case CAN_CMDID_VERSION_REQ_TX: + e_status = CANCommandVersionReq(h_app, rcv_msg.notifyName); + break; + case CAN_CMDID_FUELCALC_RST_REQ_DELIVERY: + e_status = CANCommandFuelcalcRstReq(h_app, rcv_msg.notifyName); + break; + default: + e_status = CANCommandOther(h_app, + rcv_msg.notifyName, rcv_msg.ucCmdid, rcv_msg.ucRid); + break; + } + return e_status; +} + +static CAN_TRANS_START_TABLE_VAL *CANTxMsgFindMap(CAN_TRANS_START_MSG_DAT *msg) { + std::map<CANID, CAN_TRANS_START_TABLE_VAL *>::iterator it; + it = g_map_trans_data.find(msg->id); + if (it == g_map_trans_data.end()) + return NULL; + return it->second; +} + +static CAN_TRANS_START_TABLE_VAL *CanSendFind(CAN_TRANS_START_MSG_DAT *msg) { + return CANTxMsgFindMap(msg); +} + +EFrameworkunifiedStatus CANTxMsgBit(HANDLE h_app) { + FRAMEWORKUNIFIEDLOG(ZONE_FUNC, __func__, "@@@ Start communication CanSend(bit)"); + + EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail; + CAN_TRANS_START_MSG_DAT rcv_msg; + CanMessage snd_msg; + CAN_TRANS_START_TABLE_VAL *store; + int32_t i = 0; + uint8_t *store_p; + uint8_t *data_p; + uint8_t *mask_p; + + memset(&rcv_msg, 0, sizeof(rcv_msg)); + memset(&snd_msg, 0, sizeof(snd_msg)); + + e_status = FrameworkunifiedGetMsgDataOfSize(h_app, &rcv_msg, sizeof(rcv_msg), eSMRRelease); + if (e_status != eFrameworkunifiedStatusOK) { + FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "FrameworkunifiedGetMsgDataOfSize Error(%d)", e_status); + if (e_status == eFrameworkunifiedStatusInvldBufSize) + FrameworkunifiedClearMsgData(h_app); + goto cleanup; + } + + e_status = eFrameworkunifiedStatusFail; + + if (TRUE != CommGetAvailabilityCurrent(CAN_AVAILABILITY)) { + CANTxMsgErrorLog("CAN is not ready", __func__, 0, 0, h_app); + goto cleanup; + } + + store = CanSendFind(&rcv_msg); + if (!store) { + CANTxMsgErrorLog("No initial value", __func__, + rcv_msg.id, sizeof(rcv_msg.dat), h_app); + goto cleanup; + } + + store_p = store->dat.dat; + data_p = rcv_msg.dat.dat; + mask_p = rcv_msg.mask.dat; + for (i = 0; i < store->dlc; i++) { + *store_p &= (uint8_t)(~(*mask_p)); + *store_p |= ((*mask_p) & (*data_p)); + ++store_p; + ++data_p; + ++mask_p; + } + + snd_msg.can_id = rcv_msg.id; + snd_msg.dlc = store->dlc; + snd_msg.rid = CAN_RID_NOTUSE_CODE; + memcpy(snd_msg.data, store->dat.dat, snd_msg.dlc); + + FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, + "@@@ Call CanSend for can_hal : " + "can_id=%x dlc=%d rid=%x", snd_msg.can_id, snd_msg.dlc, snd_msg.rid); + FRAMEWORKUNIFIEDLOG(ZONE_DEBUG, __func__, "msg_data=%s", + MessageDataOutputLog(snd_msg.data, snd_msg.dlc).c_str()); + + if (CANHAL_RET_NORMAL != CanSend(h_app, &snd_msg, CAN_HAL_TYPE_CAN)) { + CANTxMsgErrorLog("CAN CanSend failed", __func__, snd_msg.can_id, + (uint16_t)snd_msg.dlc, h_app); + goto cleanup; + } + + // echoback + e_status = CANDeliverySndMsg(h_app, snd_msg.can_id, 0, (uint8_t)snd_msg.dlc, + (const uint8_t *)snd_msg.data, + CID_CAN_DATA_DELIVERY, CANIF_ECHOBACK); +cleanup: + return e_status; +} + +EFrameworkunifiedStatus CANTxMsg(HANDLE h_app) { + FRAMEWORKUNIFIEDLOG(ZONE_FUNC, __func__, "@@@ Start communication CanSend"); + + EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusFail; + CAN_TRANSMISSION_START_MSG_DAT rcv_msg; + CanMessage snd_msg; + memset(&rcv_msg, 0, sizeof(rcv_msg)); + memset(&snd_msg, 0, sizeof(snd_msg)); + + e_status = FrameworkunifiedGetMsgDataOfSize(h_app, &rcv_msg, sizeof(rcv_msg), eSMRRelease); + if (e_status != eFrameworkunifiedStatusOK) { + FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "FrameworkunifiedGetMsgDataOfSize Error(%d)", e_status); + if (e_status == eFrameworkunifiedStatusInvldBufSize) + FrameworkunifiedClearMsgData(h_app); + goto cleanup; + } + + e_status = eFrameworkunifiedStatusFail; + + if (TRUE != CommGetAvailabilityCurrent(CAN_AVAILABILITY)) { + CANTxMsgErrorLog("CAN is not ready", __func__, 0, 0, h_app); + goto cleanup; + } + + snd_msg.can_id = rcv_msg.stCandata.can_id; + snd_msg.dlc = rcv_msg.stCandata.dlc; + snd_msg.rid = rcv_msg.ucRid; + memcpy(snd_msg.data, rcv_msg.stCandata.data, snd_msg.dlc); + + FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "@@@ Call CanSend for can_hal : " + "can_id=%x dlc=%d rid=%x", snd_msg.can_id, snd_msg.dlc, snd_msg.rid); + FRAMEWORKUNIFIEDLOG(ZONE_DEBUG, __func__, "msg_data=%s", + MessageDataOutputLog(snd_msg.data, snd_msg.dlc).c_str()); + + if (CANHAL_RET_NORMAL != CanSend(h_app, &snd_msg, CAN_HAL_TYPE_CAN)) { + CANTxMsgErrorLog("CAN CanSend failed", __func__, snd_msg.can_id, + (uint16_t)snd_msg.dlc, h_app); + goto cleanup; + } + + // echoback + e_status = CANDeliverySndMsg(h_app, snd_msg.can_id, 0, (uint8_t)snd_msg.dlc, + (const uint8_t *)snd_msg.data, + CID_CAN_DATA_DELIVERY, CANIF_ECHOBACK); + if (rcv_msg.ucRid == (uint8_t)CAN_RID_NOTUSE_CODE) + goto cleanup; + + if (0 < g_m_send_sts.count(snd_msg.can_id)) { + if (g_m_send_sts[snd_msg.can_id].notify_name[0] != 0x00) { + FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, + "Not yet deliver SendSts but updated(SendID=%x, CAN ID=%x, Nane=%s)", + g_m_send_sts[snd_msg.can_id].rid, snd_msg.can_id, + g_m_send_sts[snd_msg.can_id].notify_name); + } + } + + g_m_send_sts[snd_msg.can_id].rid = rcv_msg.ucRid; + memcpy(g_m_send_sts[snd_msg.can_id].notify_name, + rcv_msg.notifyName, + sizeof(g_m_send_sts[snd_msg.can_id].notify_name)); +cleanup: + return e_status; +} + +EFrameworkunifiedStatus CANSndStsProcess(HANDLE h_app, CanSendResult *rcv_msg, + PS_CommunicationProtocol cid) { + EFrameworkunifiedStatus e_status = eFrameworkunifiedStatusOK; + CAN_MSG_SENDSTS snd_msg; + CANID can_id = rcv_msg->can_id; + HANDLE sender = NULL; + memset(&snd_msg, 0, sizeof(snd_msg)); + + if (0 == g_m_send_sts.count(can_id)) + goto cleanup; + + if (0x00 == (g_m_send_sts[can_id].notify_name[0])) + goto cleanup; + + if (rcv_msg->rid != g_m_send_sts[can_id].rid) + goto cleanup; + + snd_msg.hdr.hdr.cid = (uint16_t)cid; + snd_msg.hdr.hdr.msgbodysize = (uint16_t)(sizeof(snd_msg.data)); + snd_msg.hdr.hdr.rid = rcv_msg->rid; + snd_msg.data.ulCanid = can_id; + snd_msg.data.ucStatus = + (CAN_SEND_RESULT_SUCCESS == rcv_msg->result) ? CAN_SUCCESS : CAN_RETRYOUT; + + FRAMEWORKUNIFIEDLOG(ZONE_INFO, __func__, "cid=%x msgbodysize=%d rid=%x can_id=%x ucStatus=%x", + snd_msg.hdr.hdr.cid, snd_msg.hdr.hdr.msgbodysize, snd_msg.hdr.hdr.rid, + snd_msg.data.ulCanid, snd_msg.data.ucStatus); + + sender = CommonFindSender(h_app, g_m_send_sts[can_id].notify_name); + if (!sender) { + CANTxMsgErrorLog("Could not open sender.", __func__, can_id, + sizeof(snd_msg), h_app); + goto cleanup; + } + + e_status = FrameworkunifiedSendMsg(sender, cid, sizeof(snd_msg), &snd_msg); + if (eFrameworkunifiedStatusOK != e_status) { + FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__, "FrameworkunifiedSendMsg Error(e_status:%d to:%s)", + e_status, g_m_send_sts[can_id].notify_name); + g_m_send_sts[can_id].notify_name[0] = 0x00; + g_m_send_sts[can_id].rid = 0x00; + goto cleanup; + } + g_m_send_sts[can_id].notify_name[0] = 0x00; + g_m_send_sts[can_id].rid = 0x00; + + e_status = eFrameworkunifiedStatusOK; +cleanup: + return e_status; +} |