//---------------------------------------------------------------------------
//
// Function: EplSdoAsnduCb
//
// Description: callback function for SDO ASnd frames
//
//
//
// Parameters: pFrameInfo_p = Frame with SDO payload
//
//
// Returns: tEplKernel = errorcode
//
//
// State:
//
//---------------------------------------------------------------------------
tEplKernel PUBLIC EplSdoAsnduCb(tEplFrameInfo * pFrameInfo_p)
{
tEplKernel Ret = kEplSuccessful;
unsigned int uiCount;
unsigned int* puiConnection;
unsigned int uiNodeId;
unsigned int uiFreeEntry = 0xFFFF;
tEplSdoConHdl SdoConHdl;
tEplFrame* pFrame;
pFrame = pFrameInfo_p->m_pFrame;
uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bSrcNodeId);
// search corresponding entry in control structure
uiCount = 0;
puiConnection = &SdoAsndInstance_g.m_auiSdoAsndConnection[0];
while (uiCount < EPL_SDO_MAX_CONNECTION_ASND)
{
if (uiNodeId == *puiConnection)
{
break;
}
else if ((*puiConnection == 0)
&& (uiFreeEntry == 0xFFFF))
{ // free entry
uiFreeEntry = uiCount;
}
uiCount++;
puiConnection++;
}
if (uiCount == EPL_SDO_MAX_CONNECTION_ASND)
{
if (uiFreeEntry != 0xFFFF)
{
puiConnection = &SdoAsndInstance_g.m_auiSdoAsndConnection[uiFreeEntry];
*puiConnection = uiNodeId;
uiCount = uiFreeEntry;
}
else
{
EPL_DBGLVL_SDO_TRACE0("EplSdoAsnduCb(): no free handle\n");
goto Exit;
}
}
// if (uiNodeId == *puiConnection)
{ // entry found or created
SdoConHdl = (uiCount | EPL_SDO_ASND_HANDLE );
SdoAsndInstance_g.m_fpSdoAsySeqCb(SdoConHdl, &pFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame, (pFrameInfo_p->m_uiFrameSize - 18));
}
Exit:
return Ret;
}
//---------------------------------------------------------------------------
//
// Function: EplSdoUdpThread
//
// Description: thread check socket for new data
//
//
//
// Parameters: lpParameter = pointer to parameter type tEplSdoUdpThreadPara
//
//
// Returns: u32 = errorcode
//
//
// State:
//
//---------------------------------------------------------------------------
static int EplSdoUdpThread(void *pArg_p)
{
tEplSdoUdpInstance *pInstance;
struct sockaddr_in RemoteAddr;
int iError;
int iCount;
int iFreeEntry;
u8 abBuffer[EPL_MAX_SDO_REC_FRAME_SIZE];
unsigned int uiSize;
tEplSdoConHdl SdoConHdl;
pInstance = (tEplSdoUdpInstance *) pArg_p;
daemonize("EplSdoUdpThread");
allow_signal(SIGTERM);
for (; pInstance->m_iTerminateThread == 0;)
{
// wait for data
uiSize = sizeof(struct sockaddr);
iError = recvfrom(pInstance->m_UdpSocket, // Socket
(char *)&abBuffer[0], // buffer for data
sizeof(abBuffer), // size of the buffer
0, // flags
(struct sockaddr *)&RemoteAddr,
(int *)&uiSize);
if (iError == -ERESTARTSYS) {
break;
}
if (iError > 0) {
// get handle for higher layer
iCount = 0;
iFreeEntry = 0xFFFF;
while (iCount < EPL_SDO_MAX_CONNECTION_UDP) {
// check if this connection is already known
if ((pInstance->m_aSdoAbsUdpConnection[iCount].
m_ulIpAddr == RemoteAddr.sin_addr.s_addr)
&& (pInstance->
m_aSdoAbsUdpConnection[iCount].
m_uiPort == RemoteAddr.sin_port)) {
break;
}
if ((pInstance->m_aSdoAbsUdpConnection[iCount].
m_ulIpAddr == 0)
&& (pInstance->
m_aSdoAbsUdpConnection[iCount].
m_uiPort == 0)
&& (iFreeEntry == 0xFFFF))
{
iFreeEntry = iCount;
}
iCount++;
}
if (iCount == EPL_SDO_MAX_CONNECTION_UDP) {
// connection unknown
// see if there is a free handle
if (iFreeEntry != 0xFFFF) {
// save adress infos
pInstance->
m_aSdoAbsUdpConnection[iFreeEntry].
m_ulIpAddr =
RemoteAddr.sin_addr.s_addr;
pInstance->
m_aSdoAbsUdpConnection[iFreeEntry].
m_uiPort = RemoteAddr.sin_port;
// call callback
SdoConHdl = iFreeEntry;
SdoConHdl |= EPL_SDO_UDP_HANDLE;
// offset 4 -> start of SDO Sequence header
pInstance->m_fpSdoAsySeqCb(SdoConHdl,
(tEplAsySdoSeq
*) &
abBuffer[4],
(iError -
4));
} else {
EPL_DBGLVL_SDO_TRACE0
("Error in EplSdoUdpThread() no free handle\n");
}
} else {
// known connection
// call callback with correct handle
SdoConHdl = iCount;
SdoConHdl |= EPL_SDO_UDP_HANDLE;
// offset 4 -> start of SDO Sequence header
pInstance->m_fpSdoAsySeqCb(SdoConHdl,
(tEplAsySdoSeq *) &
abBuffer[4],
(iError - 4));
}
} // end of if(iError!=SOCKET_ERROR)
} // end of for(;;)
complete_and_exit(&SdoUdpInstance_g.m_CompletionUdpThread, 0);
return 0;
}
//---------------------------------------------------------------------------
//
// Function: EplSdoUdpuConfig
//
// Description: reconfigurate socket with new IP-Address
// -> needed for NMT ResetConfiguration
//
// Parameters: ulIpAddr_p = IpAddress in platform byte order
// uiPort_p = port number in platform byte order
//
//
// Returns: tEplKernel = Errorcode
//
//
// State:
//
//---------------------------------------------------------------------------
tEplKernel EplSdoUdpuConfig(unsigned long ulIpAddr_p, unsigned int uiPort_p)
{
tEplKernel Ret;
struct sockaddr_in Addr;
int iError;
Ret = kEplSuccessful;
if (uiPort_p == 0) { // set UDP port to default port number
uiPort_p = EPL_C_SDO_EPL_PORT;
} else if (uiPort_p > 65535) {
Ret = kEplSdoUdpSocketError;
goto Exit;
}
if (SdoUdpInstance_g.m_ThreadHandle != 0) { // listen thread was started
// close old thread
SdoUdpInstance_g.m_iTerminateThread = 1;
/* kill_proc(SdoUdpInstance_g.m_ThreadHandle, SIGTERM, 1 ); */
send_sig(SIGTERM, SdoUdpInstance_g.m_ThreadHandle, 1);
wait_for_completion(&SdoUdpInstance_g.m_CompletionUdpThread);
SdoUdpInstance_g.m_iTerminateThread = 0;
SdoUdpInstance_g.m_ThreadHandle = 0;
}
if (SdoUdpInstance_g.m_UdpSocket != INVALID_SOCKET) {
// close socket
iError = closesocket(SdoUdpInstance_g.m_UdpSocket);
SdoUdpInstance_g.m_UdpSocket = INVALID_SOCKET;
if (iError != 0) {
Ret = kEplSdoUdpSocketError;
goto Exit;
}
}
// create Socket
SdoUdpInstance_g.m_UdpSocket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (SdoUdpInstance_g.m_UdpSocket == INVALID_SOCKET) {
Ret = kEplSdoUdpNoSocket;
EPL_DBGLVL_SDO_TRACE0("EplSdoUdpuConfig: socket() failed\n");
goto Exit;
}
// bind socket
Addr.sin_family = AF_INET;
Addr.sin_port = htons((unsigned short)uiPort_p);
Addr.sin_addr.s_addr = htonl(ulIpAddr_p);
iError =
bind(SdoUdpInstance_g.m_UdpSocket, (struct sockaddr *)&Addr,
sizeof(Addr));
if (iError < 0) {
//iError = WSAGetLastError();
EPL_DBGLVL_SDO_TRACE1
("EplSdoUdpuConfig: bind() finished with %i\n", iError);
Ret = kEplSdoUdpNoSocket;
goto Exit;
}
// create Listen-Thread
SdoUdpInstance_g.m_ThreadHandle =
kernel_thread(EplSdoUdpThread, &SdoUdpInstance_g, CLONE_KERNEL);
if (SdoUdpInstance_g.m_ThreadHandle == 0) {
Ret = kEplSdoUdpThreadError;
goto Exit;
}
Exit:
return Ret;
}
