mirror of
https://github.com/eclipse-openvehicle-api/openvehicle-api.git
synced 2026-04-20 11:18:16 +00:00
tompzf
339
sdv_services/can_communication_socket_can/can_com_sockets.cpp
Normal file
339
sdv_services/can_communication_socket_can/can_com_sockets.cpp
Normal file
@@ -0,0 +1,339 @@
|
||||
#include "can_com_sockets.h"
|
||||
|
||||
void CCANSockets::Initialize(const sdv::u8string& rssObjectConfig)
|
||||
{
|
||||
std::deque<std::string> vecConfigInterfaces;
|
||||
try
|
||||
{
|
||||
sdv::toml::CTOMLParser config(rssObjectConfig.c_str());
|
||||
sdv::toml::CNodeCollection nodeSource = config.GetDirect("canSockets");
|
||||
if (nodeSource.GetType() == sdv::toml::ENodeType::node_array)
|
||||
{
|
||||
for (size_t nIndex = 0; nIndex < nodeSource.GetCount(); nIndex++)
|
||||
{
|
||||
sdv::toml::CNode nodeInterface = nodeSource[nIndex];
|
||||
if (nodeInterface.GetType() == sdv::toml::ENodeType::node_string)
|
||||
{
|
||||
std::string ssIfcName = nodeInterface.GetValue();
|
||||
if (!ssIfcName.empty())
|
||||
{
|
||||
vecConfigInterfaces.push_back(ssIfcName);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (vecConfigInterfaces.size() == 0)
|
||||
{
|
||||
auto node = config.GetDirect("canSockets");
|
||||
if (node.GetType() == sdv::toml::ENodeType::node_string)
|
||||
{
|
||||
vecConfigInterfaces.push_back(node.GetValue());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
catch (const sdv::toml::XTOMLParseException& e)
|
||||
{
|
||||
SDV_LOG_ERROR("Configuration could not be read: ", e.what());
|
||||
m_eStatus = sdv::EObjectStatus::initialization_failure;
|
||||
return;
|
||||
}
|
||||
|
||||
if (vecConfigInterfaces.size() == 0)
|
||||
{
|
||||
SDV_LOG_WARNING("Configuration failure, no 'canSockets' param found");
|
||||
}
|
||||
|
||||
if (!SetupCANSockets(vecConfigInterfaces))
|
||||
{
|
||||
m_eStatus = sdv::EObjectStatus::initialization_failure;
|
||||
return;
|
||||
}
|
||||
|
||||
m_threadReceive = std::thread(&CCANSockets::ReceiveThreadFunc, this);
|
||||
|
||||
m_eStatus = sdv::EObjectStatus::initialized;
|
||||
}
|
||||
|
||||
bool CCANSockets::SetupCANSockets(const std::deque<std::string>& vecConfigInterfaces)
|
||||
{
|
||||
// Retrieve the list of available interfaces
|
||||
std::set<std::string> availableInterfaces;
|
||||
struct ifaddrs *ifaddr, *ifa;
|
||||
if (getifaddrs(&ifaddr) != -1)
|
||||
{
|
||||
for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next)
|
||||
{
|
||||
availableInterfaces.insert(ifa->ifa_name);
|
||||
}
|
||||
}
|
||||
freeifaddrs(ifaddr);
|
||||
|
||||
CreateAndBindSockets(vecConfigInterfaces, availableInterfaces);
|
||||
|
||||
bool isSocketCreated = false;
|
||||
std::unique_lock<std::mutex> lock(m_mtxSockets);
|
||||
for (auto socket : m_vecSockets)
|
||||
{
|
||||
if ((socket.localSocket > 0) && (socket.networkInterface > 0))
|
||||
{
|
||||
isSocketCreated = true;
|
||||
SDV_LOG_INFO("Enabling CAN interface: ", socket.name);
|
||||
}
|
||||
}
|
||||
|
||||
if (isSocketCreated)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
SDV_LOG_ERROR(" Error: Socket list is empty.");
|
||||
return false;
|
||||
}
|
||||
|
||||
void CCANSockets::CreateAndBindSockets(const std::deque<std::string>& vecConfigInterfaces,
|
||||
const std::set<std::string>& availableInterfaces)
|
||||
{
|
||||
for (const auto& configInterface : vecConfigInterfaces)
|
||||
{
|
||||
SSocketDefinition socketDef;
|
||||
socketDef.name = "";
|
||||
socketDef.networkInterface = 0;
|
||||
socketDef.localSocket = -1;
|
||||
|
||||
// Create a socket
|
||||
int localSocket = socket(PF_CAN, SOCK_RAW, CAN_RAW);
|
||||
if (localSocket == -1)
|
||||
{
|
||||
SDV_LOG_ERROR("Error creating Socket");
|
||||
return;
|
||||
}
|
||||
|
||||
if(availableInterfaces.find(configInterface) == availableInterfaces.end())
|
||||
{
|
||||
SDV_LOG_WARNING("Interface not found: ", configInterface);
|
||||
std::unique_lock<std::mutex> lock(m_mtxSockets);
|
||||
m_vecSockets.push_back(socketDef);
|
||||
continue;
|
||||
}
|
||||
|
||||
auto ifIndex = if_nametoindex(configInterface.c_str());
|
||||
socketDef.networkInterface = ifIndex;
|
||||
|
||||
// Bind the socket to the specified CAN interface
|
||||
sockaddr_can sAddr = {};
|
||||
sAddr.can_family = AF_CAN;
|
||||
sAddr.can_ifindex = ifIndex;
|
||||
if (bind(localSocket, reinterpret_cast<sockaddr*>(&sAddr), sizeof(sAddr)) == -1)
|
||||
{
|
||||
SDV_LOG_ERROR("Error binding socket to interface ", configInterface);
|
||||
std::unique_lock<std::mutex> lock(m_mtxSockets);
|
||||
m_vecSockets.push_back(socketDef);
|
||||
close(localSocket);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Set socket to be non-blocking
|
||||
if (fcntl(localSocket, F_SETFL, O_NONBLOCK) == -1)
|
||||
{
|
||||
perror("fcntl - F_SETFL");
|
||||
SDV_LOG_ERROR("Error setting socket to be non-blocking:", configInterface);
|
||||
close(localSocket);
|
||||
std::unique_lock<std::mutex> lock(m_mtxSockets);
|
||||
m_vecSockets.push_back(socketDef);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Store the successfully configured socket
|
||||
socketDef.name = configInterface;
|
||||
socketDef.localSocket = localSocket;
|
||||
std::unique_lock<std::mutex> lock(m_mtxSockets);
|
||||
m_vecSockets.push_back(socketDef);
|
||||
}
|
||||
}
|
||||
|
||||
sdv::EObjectStatus CCANSockets::GetStatus() const
|
||||
{
|
||||
return m_eStatus;
|
||||
}
|
||||
|
||||
void CCANSockets::SetOperationMode(sdv::EOperationMode eMode)
|
||||
{
|
||||
switch (eMode)
|
||||
{
|
||||
case sdv::EOperationMode::configuring:
|
||||
if (m_eStatus == sdv::EObjectStatus::running || m_eStatus == sdv::EObjectStatus::initialized)
|
||||
m_eStatus = sdv::EObjectStatus::configuring;
|
||||
break;
|
||||
case sdv::EOperationMode::running:
|
||||
if (m_eStatus == sdv::EObjectStatus::configuring || m_eStatus == sdv::EObjectStatus::initialized)
|
||||
m_eStatus = sdv::EObjectStatus::running;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void CCANSockets::Shutdown()
|
||||
{
|
||||
m_eStatus = sdv::EObjectStatus::shutdown_in_progress;
|
||||
|
||||
// Wait until the receiving thread is finished.
|
||||
if (m_threadReceive.joinable())
|
||||
m_threadReceive.join();
|
||||
|
||||
std::unique_lock<std::mutex> lock(m_mtxSockets);
|
||||
for (const auto& socket : m_vecSockets)
|
||||
{
|
||||
if (socket.localSocket > 0)
|
||||
{
|
||||
close(socket.localSocket);
|
||||
}
|
||||
}
|
||||
|
||||
m_eStatus = sdv::EObjectStatus::destruction_pending;
|
||||
}
|
||||
|
||||
void CCANSockets::RegisterReceiver(/*in*/ sdv::can::IReceive* pReceiver)
|
||||
{
|
||||
if (m_eStatus != sdv::EObjectStatus::configuring) return;
|
||||
if (!pReceiver) return;
|
||||
|
||||
SDV_LOG_INFO("Registering VAPI CAN communication receiver...");
|
||||
|
||||
std::unique_lock<std::mutex> lock(m_mtxReceivers);
|
||||
m_setReceivers.insert(pReceiver);
|
||||
}
|
||||
|
||||
void CCANSockets::UnregisterReceiver(/*in*/ sdv::can::IReceive* pReceiver)
|
||||
{
|
||||
// NOTE: Normally the remove function should be called in the configuration mode. Since it doesn't give
|
||||
// feedback and the associated caller might delete any receiving function, allow the removal to take place even
|
||||
// when running.
|
||||
|
||||
if (!pReceiver) return;
|
||||
|
||||
SDV_LOG_INFO("Unregistering VAPI CAN communication receiver...");
|
||||
|
||||
std::unique_lock<std::mutex> lock(m_mtxReceivers);
|
||||
m_setReceivers.erase(pReceiver);
|
||||
}
|
||||
|
||||
sdv::sequence<sdv::u8string> CCANSockets::GetInterfaces() const
|
||||
{
|
||||
sdv::sequence<sdv::u8string> seqIfcNames;
|
||||
if (m_eStatus != sdv::EObjectStatus::running)
|
||||
{
|
||||
return seqIfcNames;
|
||||
}
|
||||
|
||||
std::unique_lock<std::mutex> lock(m_mtxSockets);
|
||||
for (const auto& rprInterface : m_vecSockets)
|
||||
{
|
||||
if (!rprInterface.name.empty())
|
||||
{
|
||||
seqIfcNames.push_back(rprInterface.name);
|
||||
}
|
||||
}
|
||||
|
||||
return seqIfcNames;
|
||||
}
|
||||
|
||||
void CCANSockets::Send(const sdv::can::SMessage& sMsg, uint32_t uiConfigIndex)
|
||||
{
|
||||
if (m_eStatus != sdv::EObjectStatus::running) return;
|
||||
if (sMsg.bCanFd) return; // CAN-FD not supported
|
||||
if (sMsg.seqData.size() > 8) return; // Invalid message length.
|
||||
|
||||
sockaddr_can sAddr{};
|
||||
can_frame sFrame{};
|
||||
|
||||
// Convert the message structure from VAPI SMessage to SocketCAN can_frame
|
||||
memset(&sFrame,0, sizeof(sFrame));
|
||||
sFrame.can_dlc = sMsg.seqData.size();
|
||||
if (sFrame.can_dlc > 8)
|
||||
return;
|
||||
|
||||
sFrame.can_id = sMsg.uiID;
|
||||
if (sMsg.bExtended)
|
||||
{
|
||||
sFrame.can_id |= CAN_EFF_FLAG;
|
||||
}
|
||||
|
||||
for (uint32_t index = 0; index < sFrame.can_dlc; ++index)
|
||||
{
|
||||
sFrame.data[index] = sMsg.seqData[index];
|
||||
}
|
||||
|
||||
std::unique_lock<std::mutex> lock(m_mtxSockets);
|
||||
|
||||
auto it = m_vecSockets.begin();
|
||||
std::advance(it, uiConfigIndex);
|
||||
if (it != m_vecSockets.end())
|
||||
{
|
||||
if ((it->localSocket > 0) && (it->networkInterface > 0))
|
||||
{
|
||||
sAddr.can_ifindex = it->networkInterface;
|
||||
sAddr.can_family = AF_CAN;
|
||||
sendto(it->localSocket, &sFrame, sizeof(can_frame), 0, reinterpret_cast<sockaddr*>(&sAddr), sizeof(sAddr));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CCANSockets::ReceiveThreadFunc()
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
enum {retry, cont, exit} eNextStep = exit;
|
||||
switch (m_eStatus)
|
||||
{
|
||||
case sdv::EObjectStatus::configuring:
|
||||
case sdv::EObjectStatus::initialization_pending:
|
||||
case sdv::EObjectStatus::initialized:
|
||||
case sdv::EObjectStatus::initializing:
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
eNextStep = retry;
|
||||
break;
|
||||
case sdv::EObjectStatus::running:
|
||||
eNextStep = cont;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
if (eNextStep == exit) break;
|
||||
if (eNextStep == retry) continue;
|
||||
|
||||
sockaddr_can sAddr{};
|
||||
can_frame sFrame{};
|
||||
socklen_t nAddrLen = sizeof(sAddr);
|
||||
|
||||
for (const auto& socket : m_vecSockets)
|
||||
{
|
||||
if ((socket.localSocket > 0) && (socket.networkInterface > 0))
|
||||
{
|
||||
//auto socketIndex = socket.localSocket;
|
||||
ssize_t nBytes = recvfrom(socket.localSocket, &sFrame, sizeof(can_frame), 0, reinterpret_cast<sockaddr*>(&sAddr), &nAddrLen);
|
||||
if (nBytes < 0)
|
||||
{
|
||||
// No data received... wait for 1 ms and try again
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
continue;
|
||||
}
|
||||
|
||||
sdv::can::SMessage sMsg{};
|
||||
sMsg.uiID = sFrame.can_id;
|
||||
for (size_t nDataIndex = 0; nDataIndex < static_cast<size_t>(sFrame.can_dlc); nDataIndex++)
|
||||
{
|
||||
sMsg.seqData.push_back(sFrame.data[nDataIndex]);
|
||||
}
|
||||
|
||||
// Broadcast the message to the receivers
|
||||
std::unique_lock<std::mutex> lockReceivers(m_mtxReceivers);
|
||||
for (sdv::can::IReceive* pReceiver : m_setReceivers)
|
||||
{
|
||||
pReceiver->Receive(sMsg, socket.networkInterface);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user