From b86466ed44e7aa37ef7957b24a7afec76a996e51 Mon Sep 17 00:00:00 2001 From: Adrian Del Grosso <10929341+ad3154@users.noreply.github.com> Date: Sun, 3 Mar 2024 17:41:47 -0700 Subject: [PATCH] [HW]: Add Peak Ethernet Gateway DR support Added a CAN plugin for the PEAK ethernet gateway. --- hardware_integration/CMakeLists.txt | 13 +- .../available_can_drivers.hpp | 4 + .../peak_ethernet_gateway_plugin.hpp | 79 ++++ .../src/peak_ethernet_gateway_plugin.cpp | 386 ++++++++++++++++++ 4 files changed, 481 insertions(+), 1 deletion(-) create mode 100644 hardware_integration/include/isobus/hardware_integration/peak_ethernet_gateway_plugin.hpp create mode 100644 hardware_integration/src/peak_ethernet_gateway_plugin.cpp diff --git a/hardware_integration/CMakeLists.txt b/hardware_integration/CMakeLists.txt index 87839eb3..ef94acaa 100644 --- a/hardware_integration/CMakeLists.txt +++ b/hardware_integration/CMakeLists.txt @@ -53,6 +53,16 @@ if("WindowsPCANBasic" IN_LIST CAN_DRIVER) list(APPEND HARDWARE_INTEGRATION_SRC "pcan_basic_windows_plugin.cpp") list(APPEND HARDWARE_INTEGRATION_INCLUDE "pcan_basic_windows_plugin.hpp") endif() +if("PeakEthernetGateway" IN_LIST CAN_DRIVER) + list(APPEND HARDWARE_INTEGRATION_SRC "peak_ethernet_gateway_plugin.cpp") + list(APPEND HARDWARE_INTEGRATION_INCLUDE "peak_ethernet_gateway_plugin.hpp") + if (WIN32) + find_library(SOCKET_LIB ws2_32) + if (SOCKET_LIB) + message(STATUS "Linking Winsock2 due to PEAK Ethernet Gateway") + endif() + endif() +endif() if("VirtualCAN" IN_LIST CAN_DRIVER) list(APPEND HARDWARE_INTEGRATION_SRC "virtual_can_plugin.cpp") list(APPEND HARDWARE_INTEGRATION_INCLUDE "virtual_can_plugin.hpp") @@ -128,7 +138,8 @@ add_library(${PROJECT_NAME}::HardwareIntegration ALIAS HardwareIntegration) target_compile_features(HardwareIntegration PUBLIC cxx_std_11) set_target_properties(HardwareIntegration PROPERTIES CXX_EXTENSIONS OFF) target_link_libraries(HardwareIntegration PRIVATE ${PROJECT_NAME}::Utility - ${PROJECT_NAME}::Isobus) + ${PROJECT_NAME}::Isobus + ${SOCKET_LIB}) if("WindowsPCANBasic" IN_LIST CAN_DRIVER) if(MSVC) diff --git a/hardware_integration/include/isobus/hardware_integration/available_can_drivers.hpp b/hardware_integration/include/isobus/hardware_integration/available_can_drivers.hpp index 8a9042dd..56d2a4f6 100644 --- a/hardware_integration/include/isobus/hardware_integration/available_can_drivers.hpp +++ b/hardware_integration/include/isobus/hardware_integration/available_can_drivers.hpp @@ -18,6 +18,10 @@ #include "isobus/hardware_integration/pcan_basic_windows_plugin.hpp" #endif +#ifdef ISOBUS_PEAKETHERNETGATEWAY_AVAILABLE +#include "isobus/hardware_integration/peak_ethernet_gateway_plugin.hpp" +#endif + #ifdef ISOBUS_VIRTUALCAN_AVAILABLE #include "isobus/hardware_integration/virtual_can_plugin.hpp" #endif diff --git a/hardware_integration/include/isobus/hardware_integration/peak_ethernet_gateway_plugin.hpp b/hardware_integration/include/isobus/hardware_integration/peak_ethernet_gateway_plugin.hpp new file mode 100644 index 00000000..2f8a1524 --- /dev/null +++ b/hardware_integration/include/isobus/hardware_integration/peak_ethernet_gateway_plugin.hpp @@ -0,0 +1,79 @@ +//================================================================================================ +/// @file peak_ethernet_gateway_plugin.hpp +/// +/// @brief An interface for using a PEAK Ethernet Gateway DR. +/// @author Adrian Del Grosso +/// +/// @copyright 2024 The Open-Agriculture Developers +//================================================================================================ +#ifndef PEAK_ETHERNET_GATEWAY_PLUGIN_HPP +#define PEAK_ETHERNET_GATEWAY_PLUGIN_HPP + +#include +#include +#include + +#ifdef WIN32 +#include +#else +#include +#endif + +#include "isobus/hardware_integration/can_hardware_plugin.hpp" +#include "isobus/isobus/can_hardware_abstraction.hpp" +#include "isobus/isobus/can_message_frame.hpp" + +namespace isobus +{ + /// @brief Peak Ethernet Gateway CAN hardware plugin + class PeakEthernetGatewayPlugin : public CANHardwarePlugin + { + public: + /// @brief Constructor for a PeakEthernetGatewayPlugin + explicit PeakEthernetGatewayPlugin(std::string targetIPAddress, + std::uint16_t txPort, + std::uint16_t rxPort, + std::uint8_t CANChannelOnGateway, + bool useUDP = true); + + /// @brief The destructor for PCANBasicWindowsPlugin + virtual ~PeakEthernetGatewayPlugin(); + + /// @brief Returns if the connection with the hardware is valid + /// @returns `true` if connected, `false` if not connected + bool get_is_valid() const override; + + /// @brief Closes the connection to the hardware + void close() override; + + /// @brief Connects to the hardware you specified in the constructor's channel argument + void open() override; + + /// @brief Returns a frame from the hardware (synchronous), or `false` if no frame can be read. + /// @param[in, out] canFrame The CAN frame that was read + /// @returns `true` if a CAN frame was read, otherwise `false` + bool read_frame(isobus::CANMessageFrame &canFrame) override; + + /// @brief Writes a frame to the bus (synchronous) + /// @param[in] canFrame The frame to write to the bus + /// @returns `true` if the frame was written, otherwise `false` + bool write_frame(const isobus::CANMessageFrame &canFrame) override; + + private: + bool openRxSocket(); + bool openTxSocket(); + + static constexpr std::uint32_t RX_BUFFER_SIZE_BYTES = 2048; + + std::unique_ptr rxBuffer; + std::deque rxQueue; + std::string ipAddress; + int rxSocket = 0; + int txSocket = 0; + std::uint16_t sendPort; + std::uint16_t receivePort; + bool isOpen = false; + bool udp; + }; +} +#endif // PEAK_ETHERNET_GATEWAY_PLUGIN_HPP diff --git a/hardware_integration/src/peak_ethernet_gateway_plugin.cpp b/hardware_integration/src/peak_ethernet_gateway_plugin.cpp new file mode 100644 index 00000000..024b5eee --- /dev/null +++ b/hardware_integration/src/peak_ethernet_gateway_plugin.cpp @@ -0,0 +1,386 @@ +#include "isobus/hardware_integration/peak_ethernet_gateway_plugin.hpp" + +#include "isobus/isobus/can_stack_logger.hpp" + +#include + +#ifndef _MSC_VER +#include +#include +#include +#endif + +namespace isobus +{ +#ifdef __GNUC__ +#define PACK(__Declaration__) __Declaration__ __attribute__((__packed__)) +#endif + +#ifdef _MSC_VER +#define PACK(__Declaration__) __pragma(pack(push, 1)) __Declaration__ __pragma(pack(pop)) +#endif + + typedef PACK(struct _CAN2IP { + std::uint16_t sz; + std::uint16_t type; + std::uint64_t tag; + std::uint64_t timestamp; + std::uint8_t channel; + std::uint8_t dlc; + std::uint16_t flag; + std::uint32_t id; + std::uint8_t data[8]; + }) S_CAN2IP; + + PeakEthernetGatewayPlugin::PeakEthernetGatewayPlugin(std::string targetIPAddress, + std::uint16_t txPort, + std::uint16_t rxPort, + bool useUDP) : + ipAddress(targetIPAddress), + sendPort(txPort), + receivePort(rxPort), + udp(useUDP) + { + } + + PeakEthernetGatewayPlugin::~PeakEthernetGatewayPlugin() + { + PeakEthernetGatewayPlugin::close(); +#ifdef WIN32 + WSACleanup(); +#endif + } + + bool PeakEthernetGatewayPlugin::get_is_valid() const + { + return isOpen; + } + + void PeakEthernetGatewayPlugin::close() + { + if (0 != rxSocket) + { +#ifdef WIN32 + closesocket(rxSocket); +#else + ::close(rxSocket); +#endif + } + if (0 != txSocket) + { +#ifdef WIN32 + closesocket(txSocket); +#else + ::close(txSocket); +#endif + } + } + + void PeakEthernetGatewayPlugin::open() + { + if (!isOpen) + { + bool rxOpened = openRxSocket(); + + if (0 != rxSocket) + { + if (rxOpened) + { + bool txOpened = openTxSocket(); + + if (txOpened) + { + rxBuffer.reset(new std::uint8_t[RX_BUFFER_SIZE_BYTES]()); + isOpen = true; + } + else if (0 != txSocket) + { + close(); + } + } + else + { + close(); + } + } + } + } + + bool PeakEthernetGatewayPlugin::read_frame(isobus::CANMessageFrame &canFrame) + { + bool retVal = false; + + if (!rxQueue.empty()) + { + canFrame = rxQueue.front(); + rxQueue.pop_front(); + retVal = true; + } + else if (nullptr != rxBuffer) + { + memset(rxBuffer.get(), 0, RX_BUFFER_SIZE_BYTES); + int bytesRead = recv(rxSocket, (char *)rxBuffer.get(), RX_BUFFER_SIZE_BYTES, 0); + + if (bytesRead > 0) + { + std::uint32_t numberOfFrames = bytesRead / sizeof(S_CAN2IP); + + for (std::uint32_t i = 0; i < numberOfFrames; i++) + { + // I know this is all very cursed, but I copied it from PEAK's own example code + auto size = ntohs(*((std::uint16_t *)&rxBuffer.get()[0 + (i * sizeof(S_CAN2IP))])); + + if (sizeof(S_CAN2IP) == size) + { + // Currently unused in the PEAK driver? I hope peak adds support for this eventually so that > 1 channel will work. + // std::uint8_t channel = rxBuffer.get()[20]; + + canFrame.timestamp_us = ntohl(*((std::uint64_t *)&rxBuffer.get()[12])); + canFrame.dataLength = rxBuffer.get()[21]; + canFrame.isExtendedFrame = (0x02 == ntohs(*((std::uint16_t *)&rxBuffer.get()[22]))); + canFrame.identifier = ntohl(*((std::uint32_t *)&rxBuffer.get()[24])); + memset(&(canFrame.data[0]), 0, 8); + memcpy(&(canFrame.data[0]), &rxBuffer.get()[28], canFrame.dataLength > 8 ? 8 : canFrame.dataLength); + rxQueue.push_back(canFrame); + } + else + { + // Bad frame? + } + } + + if (!rxQueue.empty()) + { + canFrame = rxQueue.front(); + rxQueue.pop_front(); + retVal = true; + } + } + else + { + LOG_ERROR("[PEAK]: Error reading from rx socket. Remote disconnected?"); + } + } + return retVal; + } + + bool PeakEthernetGatewayPlugin::write_frame(const isobus::CANMessageFrame &canFrame) + { + S_CAN2IP frame = { + htons(0x24), + htons(0x80), + 0, + 0, + 0, + canFrame.dataLength, + htons(0x02), + htonl(canFrame.identifier), + { canFrame.data[0], + canFrame.data[1], + canFrame.data[2], + canFrame.data[3], + canFrame.data[4], + canFrame.data[5], + canFrame.data[6], + canFrame.data[7] } + }; + return send(txSocket, (char *)&frame, sizeof(frame), 0); + } + + bool PeakEthernetGatewayPlugin::openRxSocket() + { + struct sockaddr_in sockname; +#ifdef WIN32 + char optval = 1; + SOCKET sockID = 0; +#else + std::int32_t optval = 1; + std::int32_t sockID = 0; +#endif + int setsockoptRetVal = 0; + int socketType = udp ? SOCK_DGRAM : SOCK_STREAM; + +#ifdef WIN32 + WSAData data; + WSAStartup(MAKEWORD(2, 2), &data); +#endif + + sockID = socket(AF_INET, socketType, udp ? IPPROTO_UDP : IPPROTO_TCP); + + if (sockID < 0) + { + LOG_ERROR("[PEAK]: Unable to open rx socket"); + return false; + } + + setsockoptRetVal = setsockopt(sockID, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval)); + if (setsockoptRetVal < 0) + { + LOG_ERROR("[PEAK]: Error using rx setsockopt (SOL_SOCKET, SO_REUSEADDR)"); + return false; + } + + // Enable keepalive packets + if (!udp) + { + setsockoptRetVal = setsockopt(sockID, SOL_SOCKET, SO_KEEPALIVE, &optval, sizeof(optval)); + if (setsockoptRetVal < 0) + { + LOG_ERROR("[PEAK]: Error using rx setsockopt (SOL_SOCKET, SO_KEEPALIVE)"); + return false; + } + } + +#ifdef __linux__ + setsockoptRetVal = setsockopt(sockID, SOL_IP, IP_RECVERR, &optval, sizeof(int)); + if (setsockoptRetVal < 0) + { + LOG_ERROR("[PEAK]: Error using rx setsockopt (SOL_IP, IP_RECVERR)"); + return false; + } +#endif + + //Config Socket + memset((char *)&sockname, 0, sizeof(struct sockaddr_in)); + sockname.sin_family = AF_INET; + sockname.sin_port = htons(receivePort); + sockname.sin_addr.s_addr = htonl(INADDR_ANY); + + if (bind(sockID, (const struct sockaddr *)&sockname, sizeof(sockname)) < 0) + { + LOG_ERROR("[PEAK]: Unable to bind rx socket"); + return false; + } + + if (!udp) + { + struct sockaddr peer_addr; +#if defined __linux__ || defined __APPLE__ + socklen_t addr_len; +#else + int addr_len; +#endif + + if (listen(sockID, 10) < 0) // Hardcoded queue length of 10, is that enough? + { + LOG_ERROR("[PEAK]: Unable to listen to clients"); + return false; + } + + sockID = accept(sockID, &peer_addr, &addr_len); + rxSocket = sockID; + LOG_DEBUG("[PEAK]: Accepted TCP connection from client. Ready to receive TCP CAN frames"); + } + else + { + LOG_DEBUG("[PEAK]: Ready to receive UDP CAN frames"); + rxSocket = sockID; + } + return (0 != sockID); + } + + bool PeakEthernetGatewayPlugin::openTxSocket() + { + struct sockaddr_in sockname; +#ifdef WIN32 + char optval = 1; +#else + std::int32_t optval = 1; +#endif + int sockID = 0; + int setsockoptRetVal = 0; + int socketType = udp ? SOCK_DGRAM : SOCK_STREAM; + + if ((ipAddress.length() < 7) && ("*" != ipAddress)) + { + LOG_ERROR("[PEAK]: Invalid IP address"); + return false; + } + + //New Socket Instance + if (socketType == SOCK_STREAM) + { + if ((sockID = socket(PF_INET, SOCK_STREAM, 0)) < 0) + { + LOG_ERROR("[PEAK]: Unable to open tx socket"); + return false; + } + setsockoptRetVal = setsockopt(sockID, IPPROTO_TCP, TCP_NODELAY, &optval, sizeof(int)); + if (setsockoptRetVal < 0) + { + LOG_ERROR("[PEAK]: Error using tx setsockopt"); + return false; + } + } + else if (socketType == SOCK_DGRAM) + { + if ((sockID = socket(PF_INET, SOCK_DGRAM, 0)) < 0) + { + LOG_ERROR("[PEAK]: Unable to open tx socket"); + return false; + } + } + + setsockoptRetVal = setsockopt(sockID, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(int)); + if (setsockoptRetVal < 0) + { + LOG_ERROR("[PEAK]: Error using tx setsockopt (SOL_SOCKET, SO_REUSEADDR)"); + return false; + } + + if (!udp) + { + setsockoptRetVal = setsockopt(sockID, SOL_SOCKET, SO_KEEPALIVE, &optval, sizeof(int)); + if (setsockoptRetVal < 0) + { + LOG_ERROR("[PEAK]: Error using tx setsockopt (SOL_SOCKET, SO_KEEPALIVE)"); + return false; + } + } + +#ifdef __linux__ + setsockoptRetVal = setsockopt(sockID, SOL_IP, IP_RECVERR, &optval, sizeof(int)); + if (setsockoptRetVal < 0) + { + LOG_ERROR("[PEAK]: Error using tx setsockopt (SOL_IP, IP_RECVERR)"); + return false; + } +#endif + + //Config Socket + memset((char *)&sockname, 0, sizeof(struct sockaddr_in)); + sockname.sin_family = AF_INET; + sockname.sin_port = htons(sendPort); + if ("*" != ipAddress) + { + sockname.sin_addr.s_addr = inet_addr(ipAddress.c_str()); + if (sockname.sin_addr.s_addr == INADDR_NONE) + { + LOG_ERROR("[PEAK]: tx socket invalid IP address"); + return false; + } + } + else + { + sockname.sin_addr.s_addr = htonl(INADDR_ANY); /* wildcard */ + } + + int connectResult = connect(sockID, (struct sockaddr *)&sockname, sizeof(struct sockaddr_in)); + if (0 == connectResult) + { + txSocket = sockID; + LOG_DEBUG("[PEAK]: Transmit socket connected"); + return true; + } + else + { + LOG_ERROR("[PEAK]: tx connect failed (%s)\n", strerror(errno)); +#ifdef WIN32 + closesocket(sockID); +#else + ::close(sockID); +#endif + return false; + } + } +}