//--------------------------------------------------------------------------- // // 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; }