//------------------------------------------------------------------------------
tOplkError sdoudp_sendToSocket(const tSdoUdpCon* pSdoUdpCon_p,
const tPlkFrame* pSrcData_p,
size_t dataSize_p)
{
struct sockaddr_in addr;
int error;
// Check parameter validity
ASSERT(pSdoUdpCon_p != NULL);
ASSERT(pSrcData_p != NULL);
addr.sin_family = AF_INET;
addr.sin_port = pSdoUdpCon_p->port;
addr.sin_addr.s_addr = pSdoUdpCon_p->ipAddr;
error = sendto(instance_l.udpSocket,
(const char*)&pSrcData_p->messageType,
dataSize_p,
0,
(struct sockaddr*)&addr,
sizeof(struct sockaddr_in));
if (error < 0)
{
DEBUG_LVL_SDO_TRACE("%s(): sendto() finished with %i\n", __func__, error);
return kErrorSdoUdpSendError;
}
return kErrorOk;
}
//------------------------------------------------------------------------------
static void receiveFromSocket(const tSdoUdpSocketInstance* pInstance_p)
{
struct sockaddr_in remoteAddr;
int error;
UINT8 aBuffer[SDO_MAX_RX_FRAME_SIZE_UDP];
size_t size;
tSdoUdpCon sdoUdpCon;
OPLK_MEMSET(&remoteAddr, 0, sizeof(remoteAddr));
size = sizeof(struct sockaddr);
error = recvfrom(pInstance_p->udpSocket,
(char*)&aBuffer[0],
sizeof(aBuffer),
0,
(struct sockaddr*)&remoteAddr,
(socklen_t*)&size);
if (error > 0)
{
const tAsySdoSeq* pSdoSeqData;
size_t dataSize = (size_t)error - ASND_HEADER_SIZE;
pSdoSeqData = (const tAsySdoSeq*)&aBuffer[ASND_HEADER_SIZE];
sdoUdpCon.ipAddr = remoteAddr.sin_addr.s_addr;
sdoUdpCon.port = remoteAddr.sin_port;
sdoudp_receiveData(&sdoUdpCon, pSdoSeqData, dataSize);
}
else
{
DEBUG_LVL_SDO_TRACE("%s() error=%d\n", __func__, error);
}
}
//------------------------------------------------------------------------------
tOplkError sdoudp_createSocket(tSdoUdpCon* pSdoUdpCon_p)
{
struct sockaddr_in addr;
int error;
// Check parameter validity
ASSERT(pSdoUdpCon_p != NULL);
instance_l.udpSocket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (instance_l.udpSocket == INVALID_SOCKET)
{
DEBUG_LVL_SDO_TRACE("%s(): socket() failed\n", __func__);
return kErrorSdoUdpNoSocket;
}
if (pSdoUdpCon_p->ipAddr == SDOUDP_INADDR_ANY)
pSdoUdpCon_p->ipAddr = INADDR_ANY;
// bind socket
addr.sin_family = AF_INET;
addr.sin_port = htons(pSdoUdpCon_p->port);
addr.sin_addr.s_addr = htonl(pSdoUdpCon_p->ipAddr);
error = bind(instance_l.udpSocket, (struct sockaddr*)&addr, sizeof(addr));
if (error < 0)
{
DEBUG_LVL_SDO_TRACE("%s(): bind() finished with %i\n", __func__, error);
return kErrorSdoUdpNoSocket;
}
// create Listen-Thread
instance_l.fStopThread = FALSE;
if (pthread_create(&instance_l.threadHandle, NULL, sdoUdpThread, &instance_l) != 0)
return kErrorSdoUdpThreadError;
#if (defined(__GLIBC__) && (__GLIBC__ >= 2) && (__GLIBC_MINOR__ >= 12))
pthread_setname_np(instance_l.threadHandle, "oplk-sdoudp");
#endif
return kErrorOk;
}
//------------------------------------------------------------------------------
static tOplkError sdoAsndCb(const tFrameInfo* pFrameInfo_p)
{
tOplkError ret = kErrorOk;
UINT count;
UINT* pConnection;
UINT nodeId;
UINT freeEntry = 0xFFFF;
tSdoConHdl sdoConHdl;
tPlkFrame* pFrame;
pFrame = pFrameInfo_p->frame.pBuffer;
nodeId = ami_getUint8Le(&pFrame->srcNodeId);
// search corresponding entry in control structure
count = 0;
pConnection = &sdoAsndInstance_l.aSdoAsndConnection[0];
while (count < CONFIG_SDO_MAX_CONNECTION_ASND)
{
if (nodeId == *pConnection)
break;
else if ((*pConnection == 0) && (freeEntry == 0xFFFF))
{ // free entry
freeEntry = count;
}
count++;
pConnection++;
}
if (count == CONFIG_SDO_MAX_CONNECTION_ASND)
{
if (freeEntry != 0xFFFF)
{
pConnection = &sdoAsndInstance_l.aSdoAsndConnection[freeEntry];
*pConnection = nodeId;
count = freeEntry;
}
else
{
DEBUG_LVL_SDO_TRACE("%s(): no free handle\n", __func__);
return ret;
}
}
sdoConHdl = (tSdoConHdl)(count | SDO_ASND_HANDLE);
sdoAsndInstance_l.pfnSdoAsySeqCb(sdoConHdl,
&pFrame->data.asnd.payload.sdoSequenceFrame,
(pFrameInfo_p->frameSize - 18));
return ret;
}
//------------------------------------------------------------------------------
static tThreadResult sdoUdpThread(tThreadArg pArg_p)
{
const tSdoUdpSocketInstance* pInstance;
fd_set readFds;
int result;
struct timeval timeout;
pInstance = (const tSdoUdpSocketInstance*)pArg_p;
while (!pInstance->fStopThread)
{
timeout.tv_sec = 0;
timeout.tv_usec = 400000;
FD_ZERO(&readFds);
FD_SET(pInstance->udpSocket, &readFds);
result = select(pInstance->udpSocket + 1,
&readFds,
NULL,
NULL,
&timeout);
switch (result)
{
case 0: // timeout
break;
case -1: // error
DEBUG_LVL_SDO_TRACE("select error: %s\n", strerror(errno));
break;
default: // data available
receiveFromSocket(pInstance);
break;
}
}
pthread_exit(NULL);
return 0;
}