From f186de7700bf4550ce2420d1cd87d43a5a1bfc39 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Wed, 26 Apr 2023 16:11:54 +0800 Subject: [PATCH] udpate socket can --- core/driver/socketcan/socket_can.cpp | 238 ++++++++++++++++++++++++++++++++--- core/driver/socketcan/socket_can.hpp | 66 ++++++++-- 2 files changed, 277 insertions(+), 27 deletions(-) diff --git a/core/driver/socketcan/socket_can.cpp b/core/driver/socketcan/socket_can.cpp index 2054026..70cd350 100644 --- a/core/driver/socketcan/socket_can.cpp +++ b/core/driver/socketcan/socket_can.cpp @@ -1,4 +1,8 @@ #include "socket_can.hpp" + +#include + +#include "iflytopcpp/core/components/stringutils.hpp" using namespace iflytop; #define DO(exptr) \ do { \ @@ -14,19 +18,57 @@ SocketCan::SocketCanError_t SocketCan::sendFrame(shared_ptr fram return kParamError; } - canfd_frame_t canframe = frame->getCanFrame(); + if (isTxing()) { + logger->error("txing, can't send frame"); + return kDeviceBusy; + } - int ret = write(m_socketCanFd, &canframe, canframe.len + 8); - if (ret != (canframe.len + 8)) { + canfd_frame_t canframe = frame->getCanFrame(); + return writeFrame(canframe); +} +SocketCan::SocketCanError_t SocketCan::writeFrame(const canfd_frame_t &frame) { + setTxStateToTxing(frame); + int ret = write(m_socketCanFd, &frame, frame.len + 8); + if (ret != (frame.len + 8)) { logger->error("write fail,{}", strerror(errno)); + unsetTxStateToTxing(); return kWriteError; } return kSuccess; } + +SocketCan::SocketCanError_t SocketCan::sendFrame(shared_ptr frame, int overtime) { + tp_steady start = tu_steady().now(); + while (tu_steady().elapsedTimeMs(start) < overtime && isTxing()) { + usleep(100); + } + + if (isTxing()) { + logger->error("send frame overtime"); + return kOvertime; + } + return sendFrame(frame); +} + +bool SocketCan::isTxing() { return m_txState.txState == kTxing; } + +bool SocketCan::isTxFrame(const canfd_frame_t &frame) { + lock_guard lock(m_txState.lock); + if (m_txState.txbuf.can_id == frame.can_id && m_txState.txbuf.len == frame.len && memcmp(m_txState.txbuf.data, frame.data, frame.len) == 0) { + return true; + } + return false; +} + SocketCan::SocketCanError_t SocketCan::sendFrame(string framestr) { struct canfd_frame frame = {0}; int required_mtu = 0; + if (isTxing()) { + logger->error("txing, can't send frame"); + return kDeviceBusy; + } + required_mtu = m_socketCanUtils.parse_canframe((char *)framestr.c_str(), &frame); if (!required_mtu) { logger->error("Wrong CAN-frame format!"); @@ -38,12 +80,8 @@ SocketCan::SocketCanError_t SocketCan::sendFrame(string framestr) { return kCANFDNotSupported; } - logger->info("send frame:{} {} {} {} {} {}", required_mtu, frame.can_id, frame.len, frame.flags, frame.__res0, frame.__res1); - if (write(m_socketCanFd, &frame, required_mtu) != required_mtu) { - logger->error("write failed,{}", strerror(errno)); - return kWriteError; - } - return kSuccess; + // logger->info("send frame:{} {} {} {} {} {}", required_mtu, frame.can_id, frame.len, frame.flags, frame.__res0, frame.__res1); + return writeFrame(frame); } void SocketCan::initialize(shared_ptr socketCanConfig) { // @@ -86,7 +124,9 @@ void SocketCan::socketcanInitialize() { struct sockaddr_can addr = {0}; can_filter_t zerofilter = {0}; int recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */ + int localloop = 0; vector filters = m_socketCanConfig->m_canfilters; + can_err_mask_t err_mask = 0; if (!findCan(m_socketCanConfig->m_canName)) { logger->error("can:{} not found", m_socketCanConfig->m_canName); @@ -117,15 +157,34 @@ void SocketCan::socketcanInitialize() { addr.can_ifindex = ifr.ifr_ifindex; DO(bind(m_socketCanFd, (struct sockaddr *)&addr, sizeof(addr))); - // 设置过滤器 - if (filters.empty()) { - filters.push_back(zerofilter); - } + // 设置过滤器,默认不过滤 DO(setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_FILTER, &zerofilter, sizeof(zerofilter))); - recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */ - DO(setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs, sizeof(recv_own_msgs))); + if (m_socketCanConfig->enablLoopback) { + localloop = 1; /* 0 = disabled (default), 1 = enabled */ + DO(setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &localloop, sizeof(localloop))); + recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */ + DO(setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs, sizeof(recv_own_msgs))); + } else { + localloop = 0; /* 0 = disabled (default), 1 = enabled */ + DO(setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &localloop, sizeof(localloop))); + + recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */ + DO(setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs, sizeof(recv_own_msgs))); + } + + err_mask = CAN_ERR_TX_TIMEOUT | // + CAN_ERR_LOSTARB | // + CAN_ERR_CRTL | // + CAN_ERR_PROT | // + CAN_ERR_TRX | // + CAN_ERR_ACK | // + CAN_ERR_BUSOFF | // + CAN_ERR_BUSERROR | // + CAN_ERR_RESTARTED; + + setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask, sizeof(err_mask)); logger->info("socketcan init success"); return; @@ -153,6 +212,142 @@ void SocketCan::dosystem(string cmd) { logger->info("do cmd:{}", cmd); system(cmd.c_str()); } +bool SocketCan::tryUpdateTxState(canfd_frame_t &rxloopmsg) { + lock_guard lock(m_txState.lock); + if (m_txState.txState == kTxing && isTxFrame(rxloopmsg)) { + unsetTxStateToTxing(); + return true; + } + return false; +} + +void SocketCan::setTxStateToTxing(const canfd_frame_t &txpacket) { + lock_guard lock(m_txState.lock); + m_txState.txState = kTxing; + m_txState.txbuf = txpacket; +} +void SocketCan::unsetTxStateToTxing() { + lock_guard lock(m_txState.lock); + m_txState.txState = kTxIdle; +} + +#define TEST_CANDRIVER_DO(exptr) \ + do { \ + if ((exptr) != 0) { \ + logger->error("do {} failed,{}", #exptr, strerror(errno)); \ + if (socketId > 0) { \ + close(socketId); \ + socketId = -1; \ + return -1; \ + } \ + } \ + } while (0) + +int SocketCan::dumpCanDriverInfo(string canDevice, int baudrate) { + int socketId = -1; + struct ifreq ifr = {0}; + struct sockaddr_can addr = {0}; + can_filter_t zerofilter = {0}; + int recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */ + int localloop = 0; + size_t optlen; + can_err_mask_t err_mask = 0; + int ret = 0; + fd_set readfds, readFileDescriptors; + timeval waitTime; + int loopTime = 0; + logger_t logger = GET_LOGGER(SocketCan); + + if (!findCan(canDevice)) { + logger->error("can:{} not found", canDevice); + return -1; + } + + // set bitrate + system(fmt::format("ip link set {} down", canDevice).c_str()); + system(fmt::format("ip link set {} type can bitrate {}", canDevice, baudrate).c_str()); + system(fmt::format("ip link set {} up", canDevice).c_str()); + + socketId = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (socketId < 0) { + logger->error("socket failed,{}", strerror(errno)); + return -1; + } + + // 设置依靠名字绑定CAN设备 + strcpy(ifr.ifr_name, canDevice.c_str()); + TEST_CANDRIVER_DO(ioctl(socketId, SIOCGIFINDEX, &ifr)); + + // BIND + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + TEST_CANDRIVER_DO(bind(socketId, (struct sockaddr *)&addr, sizeof(addr))); + + // 设置过滤器 + TEST_CANDRIVER_DO(setsockopt(socketId, SOL_CAN_RAW, CAN_RAW_FILTER, &zerofilter, sizeof(zerofilter))); + + // 打开回环 + localloop = 1; /* 0 = disabled (default), 1 = enabled */ + TEST_CANDRIVER_DO(setsockopt(socketId, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &localloop, sizeof(localloop))); + + recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */ + TEST_CANDRIVER_DO(setsockopt(socketId, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs, sizeof(recv_own_msgs))); + + // 发送一条消息 + canfd_frame_t txframe = {0}; + canfd_frame_t rxcanframe = {0}; + + int rxloopcount = 0; + txframe.can_id = 0x12; + txframe.len = 8; + txframe.data[0] = 0x01; + + logger->info("write one frame"); + ret = write(socketId, &txframe, txframe.len + 8); + if (ret < 0) { + logger->error("write failed,{}", strerror(errno)); + close(socketId); + return -1; + } + + FD_ZERO(&readFileDescriptors); + FD_SET(socketId, &readFileDescriptors); + while (loopTime < 10) { + loopTime++; + waitTime.tv_sec = 0; + waitTime.tv_usec = 100000; // 100ms + readfds = readFileDescriptors; + + int n = select(socketId + 1, &readfds, 0, 0, &waitTime); + if (n <= 0) continue; + + int readsize = read(socketId, &rxcanframe, sizeof(rxcanframe)); + if (readsize < 0) { + logger->error("read failed,{}", strerror(errno)); + close(socketId); + return -1; + } + + if (rxcanframe.can_id == txframe.can_id && // + rxcanframe.data[0] == txframe.data[0] && // + rxcanframe.len == txframe.len) { + rxloopcount++; + logger->info("rx loop frame {}", rxloopcount); + } + } + + logger->info("Test Can Driver OK!"); + logger->info("=================Can Driver Info ================="); + logger->info("canDevice :{}", canDevice); + logger->info("baudrate :{}", baudrate); + logger->info("loop count :{}", rxloopcount); + logger->info("shouldEnableLoop:{}", rxloopcount == 1); + logger->info("=================================================="); + + close(socketId); + + return 0; +} void SocketCan::socketCanReadThreadLoop() { ThisThread thisThread; @@ -180,8 +375,15 @@ void SocketCan::socketCanReadThreadLoop() { continue; } - shared_ptr socketCanFrame = SocketCanFrame::createFrame(canframe); - logger->debug("RX:{}", socketCanFrame->toString()); - onSocketCanFrame(socketCanFrame); + if (canframe.can_id & CAN_ERR_FLAG) { + logger->error("error frame {:x},{}", canframe.can_id, StringUtils().bytesToString(canframe.data, canframe.len)); + } else { + // logger->info("TX:{} {} {}", canframe.can_id, canframe.__res0, canframe.__res1); + if (!tryUpdateTxState(canframe)) { + shared_ptr socketCanFrame = SocketCanFrame::createFrame(canframe); + logger->debug("RX:{}", socketCanFrame->toString()); + onSocketCanFrame(socketCanFrame); + } + } } } diff --git a/core/driver/socketcan/socket_can.hpp b/core/driver/socketcan/socket_can.hpp index 762fd27..8d9587a 100644 --- a/core/driver/socketcan/socket_can.hpp +++ b/core/driver/socketcan/socket_can.hpp @@ -8,11 +8,15 @@ #include #include #include +#include +#include #include #include #include #include +#include "iflytopcpp/core/basic/concurrentqueue/concurrentqueue.h" +#include "iflytopcpp/core/components/timeutils.hpp" #include "iflytopcpp/core/spdlogfactory/logger.hpp" #include "iflytopcpp/core/thread/thread.hpp" #include "socket_can_frame.hpp" @@ -33,21 +37,43 @@ namespace iflytop { using namespace std; using namespace core; +using namespace moodycamel; class SocketCanConfig { public: - string m_canName; - int m_canBaudrate; - vector m_canfilters; -}; + string m_canName; + int m_canBaudrate; -class SocketCanDeviceState { - public: + /** + * @brief + * SocketCan服务依赖于loopback功能，一般来说是需要使能的。 + * 但有些can驱动自动开启了这个功能，这时候就需要禁用，否则就会收到两个相同的帧。 + * 通过testCanDriver可以测试是否需要禁用loopback功能。 + */ + bool enablLoopback = false; + + vector m_canfilters; // 应用层过滤，这里不要配置 }; class SocketCan : public enable_shared_from_this { ENABLE_LOGGER(SocketCan); + public: + typedef enum { + kTxIdle, + kTxing, + } TxState_t; + + class TxState { + public: + canfd_frame_t txbuf; + TxState_t txState = kTxIdle; + int txAckCount = 0; + zsteady_tp txLoopMessageRxTicket; + recursive_mutex lock; + }; + + private: unique_ptr m_thread; unique_ptr m_monitorThread; int m_socketCanFd = -1; @@ -56,6 +82,8 @@ class SocketCan : public enable_shared_from_this { shared_ptr m_socketCanConfig; + TxState m_txState; + public: typedef enum { kSuccess, @@ -65,6 +93,8 @@ class SocketCan : public enable_shared_from_this { kCANFDNotSupported, kWriteError, kParamError, + kDeviceBusy, + kOvertime, } SocketCanError_t; public: @@ -76,6 +106,7 @@ class SocketCan : public enable_shared_from_this { void startListen(); + SocketCanError_t sendFrame(shared_ptr frame, int overtime); SocketCanError_t sendFrame(shared_ptr frame); /** * @brief 发送can帧 @@ -88,6 +119,15 @@ class SocketCan : public enable_shared_from_this { * @return int */ SocketCanError_t sendFrame(string frame); + /** + * @brief 使用这个方法测试目标设备CAN驱动的行为，需要CAN总线上，除了目标设备外，至少有一个正常工作的节点。 + * + * @param canDevice + * @param baudrate + * @return int + */ + static int dumpCanDriverInfo(string canDevice, int baudrate); + bool isTxing(); private: void socketcanInitialize(); @@ -99,11 +139,19 @@ class SocketCan : public enable_shared_from_this { void socketCanReadThreadLoop(); private: - void dosystem(string cmd); - void setCanBitrate(string canName, int bitrate); - bool findCan(string canName); + void dosystem(string cmd); + void setCanBitrate(string canName, int bitrate); + static bool findCan(string canName); + + SocketCanError_t writeFrame(const canfd_frame_t &frame); private: void canReadThread(); + + bool tryUpdateTxState(canfd_frame_t &rxloopmsg); + void setTxStateToTxing(const canfd_frame_t &txpacket); + void unsetTxStateToTxing(); + + bool isTxFrame(const canfd_frame_t &frame); }; } // namespace iflytop \ No newline at end of file