1 /****************************************************************************
3 (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
4 www.systec-electronic.com
8 Description: source file for SDO/UDP-Protocolabstractionlayer module
12 Redistribution and use in source and binary forms, with or without
13 modification, are permitted provided that the following conditions
16 1. Redistributions of source code must retain the above copyright
17 notice, this list of conditions and the following disclaimer.
19 2. Redistributions in binary form must reproduce the above copyright
20 notice, this list of conditions and the following disclaimer in the
21 documentation and/or other materials provided with the distribution.
23 3. Neither the name of SYSTEC electronic GmbH nor the names of its
24 contributors may be used to endorse or promote products derived
25 from this software without prior written permission. For written
26 permission, please contact info@systec-electronic.com.
28 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
29 "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
30 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
31 FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
32 COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
33 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
34 BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
35 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
36 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
37 LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
38 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
39 POSSIBILITY OF SUCH DAMAGE.
43 If a provision of this License is or becomes illegal, invalid or
44 unenforceable in any jurisdiction, that shall not affect:
45 1. the validity or enforceability in that jurisdiction of any other
46 provision of this License; or
47 2. the validity or enforceability in other jurisdictions of that or
48 any other provision of this License.
50 -------------------------------------------------------------------------
52 $RCSfile: EplSdoUdpu.c,v $
56 $Revision: 1.8 $ $Date: 2008/10/17 15:32:32 $
63 -------------------------------------------------------------------------
67 2006/06/26 k.t.: start of the implementation
69 ****************************************************************************/
72 #include "user/EplSdoUdpu.h"
74 #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
76 #if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
77 #include "SocketLinuxKernel.h"
78 #include <linux/completion.h>
79 #include <linux/sched.h>
83 /***************************************************************************/
86 /* G L O B A L D E F I N I T I O N S */
89 /***************************************************************************/
91 //---------------------------------------------------------------------------
93 //---------------------------------------------------------------------------
95 #ifndef EPL_SDO_MAX_CONNECTION_UDP
96 #define EPL_SDO_MAX_CONNECTION_UDP 5
99 //---------------------------------------------------------------------------
101 //---------------------------------------------------------------------------
105 unsigned long m_ulIpAddr; // in network byte order
106 unsigned int m_uiPort; // in network byte order
113 tEplSdoUdpCon m_aSdoAbsUdpConnection[EPL_SDO_MAX_CONNECTION_UDP];
114 tEplSequLayerReceiveCb m_fpSdoAsySeqCb;
117 #if (TARGET_SYSTEM == _WIN32_)
118 HANDLE m_ThreadHandle;
119 LPCRITICAL_SECTION m_pCriticalSection;
120 CRITICAL_SECTION m_CriticalSection;
122 #elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
123 struct completion m_CompletionUdpThread;
125 int m_iTerminateThread;
128 } tEplSdoUdpInstance;
130 //---------------------------------------------------------------------------
131 // modul globale vars
132 //---------------------------------------------------------------------------
134 static tEplSdoUdpInstance SdoUdpInstance_g;
136 //---------------------------------------------------------------------------
137 // local function prototypes
138 //---------------------------------------------------------------------------
140 #if (TARGET_SYSTEM == _WIN32_)
141 static DWORD PUBLIC EplSdoUdpThread(LPVOID lpParameter);
143 #elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
144 static int EplSdoUdpThread(void * pArg_p);
147 /***************************************************************************/
150 /* C L A S S <EPL-SDO-UDP-Layer> */
153 /***************************************************************************/
155 // Description: Protocolabstraction layer for UDP
158 /***************************************************************************/
162 //=========================================================================//
164 // P U B L I C F U N C T I O N S //
166 //=========================================================================//
168 //---------------------------------------------------------------------------
170 // Function: EplSdoUdpuInit
172 // Description: init first instance of the module
176 // Parameters: pReceiveCb_p = functionpointer to Sdo-Sequence layer
180 // Returns: tEplKernel = Errorcode
185 //---------------------------------------------------------------------------
186 tEplKernel PUBLIC EplSdoUdpuInit(tEplSequLayerReceiveCb fpReceiveCb_p)
191 Ret = EplSdoUdpuAddInstance(fpReceiveCb_p);
196 //---------------------------------------------------------------------------
198 // Function: EplSdoUdpuAddInstance
200 // Description: init additional instance of the module
201 // înit socket and start Listen-Thread
205 // Parameters: pReceiveCb_p = functionpointer to Sdo-Sequence layer
209 // Returns: tEplKernel = Errorcode
214 //---------------------------------------------------------------------------
215 tEplKernel PUBLIC EplSdoUdpuAddInstance(tEplSequLayerReceiveCb fpReceiveCb_p)
219 #if (TARGET_SYSTEM == _WIN32_)
225 // set instance variables to 0
226 EPL_MEMSET(&SdoUdpInstance_g, 0x00, sizeof(SdoUdpInstance_g));
228 Ret = kEplSuccessful;
230 // save pointer to callback-function
231 if (fpReceiveCb_p != NULL)
233 SdoUdpInstance_g.m_fpSdoAsySeqCb = fpReceiveCb_p;
237 Ret = kEplSdoUdpMissCb;
241 #if (TARGET_SYSTEM == _WIN32_)
242 // start winsock2 for win32
243 // windows specific start of socket
244 iError = WSAStartup(MAKEWORD(2,0),&Wsa);
247 Ret = kEplSdoUdpNoSocket;
251 // create critical section for acccess of instnace variables
252 SdoUdpInstance_g.m_pCriticalSection = &SdoUdpInstance_g.m_CriticalSection;
253 InitializeCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
255 #elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
256 init_completion(&SdoUdpInstance_g.m_CompletionUdpThread);
257 SdoUdpInstance_g.m_iTerminateThread = 0;
260 SdoUdpInstance_g.m_ThreadHandle = 0;
261 SdoUdpInstance_g.m_UdpSocket = INVALID_SOCKET;
263 Ret = EplSdoUdpuConfig(INADDR_ANY, 0);
270 //---------------------------------------------------------------------------
272 // Function: EplSdoUdpuDelInstance
274 // Description: del instance of the module
275 // del socket and del Listen-Thread
282 // Returns: tEplKernel = Errorcode
287 //---------------------------------------------------------------------------
288 tEplKernel PUBLIC EplSdoUdpuDelInstance()
292 #if (TARGET_SYSTEM == _WIN32_)
296 Ret = kEplSuccessful;
298 if (SdoUdpInstance_g.m_ThreadHandle != 0)
299 { // listen thread was started
301 #if (TARGET_SYSTEM == _WIN32_)
302 fTermError = TerminateThread(SdoUdpInstance_g.m_ThreadHandle, 0);
303 if(fTermError == FALSE)
305 Ret = kEplSdoUdpThreadError;
309 #elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
310 SdoUdpInstance_g.m_iTerminateThread = 1;
311 /* kill_proc(SdoUdpInstance_g.m_ThreadHandle, SIGTERM, 1 ); */
312 send_sig(SIGTERM, SdoUdpInstance_g.m_ThreadHandle, 1);
313 wait_for_completion(&SdoUdpInstance_g.m_CompletionUdpThread);
316 SdoUdpInstance_g.m_ThreadHandle = 0;
319 if (SdoUdpInstance_g.m_UdpSocket != INVALID_SOCKET)
322 closesocket(SdoUdpInstance_g.m_UdpSocket);
323 SdoUdpInstance_g.m_UdpSocket = INVALID_SOCKET;
326 #if (TARGET_SYSTEM == _WIN32_)
327 // delete critical section
328 DeleteCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
331 #if (TARGET_SYSTEM == _WIN32_)
336 #if (TARGET_SYSTEM == _WIN32_)
342 //---------------------------------------------------------------------------
344 // Function: EplSdoUdpuConfig
346 // Description: reconfigurate socket with new IP-Address
347 // -> needed for NMT ResetConfiguration
349 // Parameters: ulIpAddr_p = IpAddress in platform byte order
350 // uiPort_p = port number in platform byte order
353 // Returns: tEplKernel = Errorcode
358 //---------------------------------------------------------------------------
359 tEplKernel PUBLIC EplSdoUdpuConfig(unsigned long ulIpAddr_p, unsigned int uiPort_p)
362 struct sockaddr_in Addr;
365 #if (TARGET_SYSTEM == _WIN32_)
367 unsigned long ulThreadId;
370 Ret = kEplSuccessful;
373 { // set UDP port to default port number
374 uiPort_p = EPL_C_SDO_EPL_PORT;
376 else if (uiPort_p > 65535)
378 Ret = kEplSdoUdpSocketError;
382 if (SdoUdpInstance_g.m_ThreadHandle != 0)
383 { // listen thread was started
386 #if (TARGET_SYSTEM == _WIN32_)
387 fTermError = TerminateThread(SdoUdpInstance_g.m_ThreadHandle, 0);
388 if(fTermError == FALSE)
390 Ret = kEplSdoUdpThreadError;
394 #elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
395 SdoUdpInstance_g.m_iTerminateThread = 1;
396 /* kill_proc(SdoUdpInstance_g.m_ThreadHandle, SIGTERM, 1 ); */
397 send_sig(SIGTERM, SdoUdpInstance_g.m_ThreadHandle, 1);
398 wait_for_completion(&SdoUdpInstance_g.m_CompletionUdpThread);
399 SdoUdpInstance_g.m_iTerminateThread = 0;
402 SdoUdpInstance_g.m_ThreadHandle = 0;
405 if (SdoUdpInstance_g.m_UdpSocket != INVALID_SOCKET)
408 iError = closesocket(SdoUdpInstance_g.m_UdpSocket);
409 SdoUdpInstance_g.m_UdpSocket = INVALID_SOCKET;
412 Ret = kEplSdoUdpSocketError;
418 SdoUdpInstance_g.m_UdpSocket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
419 if (SdoUdpInstance_g.m_UdpSocket == INVALID_SOCKET)
421 Ret = kEplSdoUdpNoSocket;
422 EPL_DBGLVL_SDO_TRACE0("EplSdoUdpuConfig: socket() failed\n");
427 Addr.sin_family = AF_INET;
428 Addr.sin_port = htons((unsigned short) uiPort_p);
429 Addr.sin_addr.s_addr = htonl(ulIpAddr_p);
430 iError = bind(SdoUdpInstance_g.m_UdpSocket, (struct sockaddr*)&Addr, sizeof (Addr));
433 //iError = WSAGetLastError();
434 EPL_DBGLVL_SDO_TRACE1("EplSdoUdpuConfig: bind() finished with %i\n", iError);
435 Ret = kEplSdoUdpNoSocket;
439 // create Listen-Thread
440 #if (TARGET_SYSTEM == _WIN32_)
444 SdoUdpInstance_g.m_ThreadHandle = CreateThread(NULL,
450 if(SdoUdpInstance_g.m_ThreadHandle == NULL)
452 Ret = kEplSdoUdpThreadError;
456 #elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
458 SdoUdpInstance_g.m_ThreadHandle = kernel_thread(EplSdoUdpThread, &SdoUdpInstance_g, CLONE_KERNEL);
459 if(SdoUdpInstance_g.m_ThreadHandle == 0)
461 Ret = kEplSdoUdpThreadError;
473 //---------------------------------------------------------------------------
475 // Function: EplSdoUdpuInitCon
477 // Description: init a new connect
481 // Parameters: pSdoConHandle_p = pointer for the new connection handle
482 // uiTargetNodeId_p = NodeId of the target node
485 // Returns: tEplKernel = Errorcode
490 //---------------------------------------------------------------------------
491 tEplKernel PUBLIC EplSdoUdpuInitCon(tEplSdoConHdl* pSdoConHandle_p,
492 unsigned int uiTargetNodeId_p)
495 unsigned int uiCount;
496 unsigned int uiFreeCon;
497 tEplSdoUdpCon* pSdoUdpCon;
499 Ret = kEplSuccessful;
501 // get free entry in control structure
503 uiFreeCon = EPL_SDO_MAX_CONNECTION_UDP;
504 pSdoUdpCon = &SdoUdpInstance_g.m_aSdoAbsUdpConnection[0];
505 while (uiCount < EPL_SDO_MAX_CONNECTION_UDP)
507 if ((pSdoUdpCon->m_ulIpAddr & htonl(0xFF)) == htonl(uiTargetNodeId_p))
508 { // existing connection to target node found
510 *pSdoConHandle_p = (uiCount | EPL_SDO_UDP_HANDLE);
514 else if ((pSdoUdpCon->m_ulIpAddr == 0)
515 && (pSdoUdpCon->m_uiPort == 0))
523 if (uiFreeCon == EPL_SDO_MAX_CONNECTION_UDP)
525 // error no free handle
526 Ret = kEplSdoUdpNoFreeHandle;
530 pSdoUdpCon = &SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiFreeCon];
531 // save infos for connection
532 pSdoUdpCon->m_uiPort = htons(EPL_C_SDO_EPL_PORT);
533 pSdoUdpCon->m_ulIpAddr = htonl(0xC0A86400 | uiTargetNodeId_p); // 192.168.100.uiTargetNodeId_p
536 *pSdoConHandle_p = (uiFreeCon | EPL_SDO_UDP_HANDLE);
545 //---------------------------------------------------------------------------
547 // Function: EplSdoUdpuSendData
549 // Description: send data using exisiting connection
553 // Parameters: SdoConHandle_p = connection handle
554 // pSrcData_p = pointer to data
555 // dwDataSize_p = number of databyte
556 // -> without asend-header!!!
558 // Returns: tEplKernel = Errorcode
563 //---------------------------------------------------------------------------
564 tEplKernel PUBLIC EplSdoUdpuSendData(tEplSdoConHdl SdoConHandle_p,
565 tEplFrame * pSrcData_p,
570 unsigned int uiArray;
571 struct sockaddr_in Addr;
573 Ret = kEplSuccessful;
575 uiArray = (SdoConHandle_p & ~EPL_SDO_ASY_HANDLE_MASK);
576 if(uiArray >= EPL_SDO_MAX_CONNECTION_UDP)
578 Ret = kEplSdoUdpInvalidHdl;
582 AmiSetByteToLe(&pSrcData_p->m_le_bMessageType, 0x06); // SDO
583 // target node id (for Udp = 0)
584 AmiSetByteToLe(&pSrcData_p->m_le_bDstNodeId, 0x00);
585 // set source-nodeid (for Udp = 0)
586 AmiSetByteToLe(&pSrcData_p->m_le_bSrcNodeId, 0x00);
589 dwDataSize_p += EPL_ASND_HEADER_SIZE;
592 Addr.sin_family = AF_INET;
593 #if (TARGET_SYSTEM == _WIN32_)
594 // enter critical section for process function
595 EnterCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
598 Addr.sin_port = (unsigned short) SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiArray].m_uiPort;
599 Addr.sin_addr.s_addr = SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiArray].m_ulIpAddr;
601 #if (TARGET_SYSTEM == _WIN32_)
602 // leave critical section for process function
603 LeaveCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
606 iError = sendto (SdoUdpInstance_g.m_UdpSocket, // sockethandle
607 (const char*) &pSrcData_p->m_le_bMessageType, // data to send
608 dwDataSize_p, // number of bytes to send
610 (struct sockaddr*)&Addr, // target
611 sizeof(struct sockaddr_in)); // sizeof targetadress
614 EPL_DBGLVL_SDO_TRACE1("EplSdoUdpuSendData: sendto() finished with %i\n", iError);
615 Ret = kEplSdoUdpSendError;
625 //---------------------------------------------------------------------------
627 // Function: EplSdoUdpuDelCon
629 // Description: delete connection from intern structure
633 // Parameters: SdoConHandle_p = connection handle
635 // Returns: tEplKernel = Errorcode
640 //---------------------------------------------------------------------------
641 tEplKernel PUBLIC EplSdoUdpuDelCon(tEplSdoConHdl SdoConHandle_p)
644 unsigned int uiArray;
647 uiArray = (SdoConHandle_p & ~EPL_SDO_ASY_HANDLE_MASK);
649 if(uiArray >= EPL_SDO_MAX_CONNECTION_UDP)
651 Ret = kEplSdoUdpInvalidHdl;
656 Ret = kEplSuccessful;
661 SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiArray].m_ulIpAddr = 0;
662 SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiArray].m_uiPort = 0;
668 //=========================================================================//
670 // P R I V A T E F U N C T I O N S //
672 //=========================================================================//
674 //---------------------------------------------------------------------------
676 // Function: EplSdoUdpThread
678 // Description: thread check socket for new data
682 // Parameters: lpParameter = pointer to parameter type tEplSdoUdpThreadPara
685 // Returns: DWORD = errorcode
690 //---------------------------------------------------------------------------
691 #if (TARGET_SYSTEM == _WIN32_)
692 static DWORD PUBLIC EplSdoUdpThread(LPVOID lpParameter)
693 #elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
694 static int EplSdoUdpThread(void * pArg_p)
698 tEplSdoUdpInstance* pInstance;
699 struct sockaddr_in RemoteAddr;
703 BYTE abBuffer[EPL_MAX_SDO_REC_FRAME_SIZE];
705 tEplSdoConHdl SdoConHdl;
707 #if (TARGET_SYSTEM == _WIN32_)
708 pInstance = (tEplSdoUdpInstance*)lpParameter;
712 #elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
713 pInstance = (tEplSdoUdpInstance*)pArg_p;
714 daemonize("EplSdoUdpThread");
715 allow_signal( SIGTERM );
717 for (;pInstance->m_iTerminateThread == 0;)
722 uiSize = sizeof(struct sockaddr);
723 iError = recvfrom(pInstance->m_UdpSocket, // Socket
724 (char *)&abBuffer[0], // buffer for data
725 sizeof(abBuffer), // size of the buffer
727 (struct sockaddr*)&RemoteAddr,
729 #if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
730 if (iError == -ERESTARTSYS)
737 // get handle for higher layer
740 #if (TARGET_SYSTEM == _WIN32_)
741 // enter critical section for process function
742 EnterCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
744 while (iCount < EPL_SDO_MAX_CONNECTION_UDP)
746 // check if this connection is already known
747 if((pInstance->m_aSdoAbsUdpConnection[iCount].m_ulIpAddr == RemoteAddr.sin_addr.s_addr)
748 && (pInstance->m_aSdoAbsUdpConnection[iCount].m_uiPort == RemoteAddr.sin_port))
753 if((pInstance->m_aSdoAbsUdpConnection[iCount].m_ulIpAddr == 0)
754 && (pInstance->m_aSdoAbsUdpConnection[iCount].m_uiPort == 0)
755 && (iFreeEntry == 0xFFFF))
764 if (iCount == EPL_SDO_MAX_CONNECTION_UDP)
766 // connection unknown
767 // see if there is a free handle
768 if (iFreeEntry != 0xFFFF)
771 pInstance->m_aSdoAbsUdpConnection[iFreeEntry].m_ulIpAddr =
772 RemoteAddr.sin_addr.s_addr;
773 pInstance->m_aSdoAbsUdpConnection[iFreeEntry].m_uiPort =
775 #if (TARGET_SYSTEM == _WIN32_)
776 // leave critical section for process function
777 LeaveCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
780 SdoConHdl = iFreeEntry;
781 SdoConHdl |= EPL_SDO_UDP_HANDLE;
782 // offset 4 -> start of SDO Sequence header
783 pInstance->m_fpSdoAsySeqCb(SdoConHdl, (tEplAsySdoSeq*)&abBuffer[4], (iError - 4));
787 EPL_DBGLVL_SDO_TRACE0("Error in EplSdoUdpThread() no free handle\n");
788 #if (TARGET_SYSTEM == _WIN32_)
789 // leave critical section for process function
790 LeaveCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
798 // call callback with correct handle
800 SdoConHdl |= EPL_SDO_UDP_HANDLE;
801 #if (TARGET_SYSTEM == _WIN32_)
802 // leave critical section for process function
803 LeaveCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
805 // offset 4 -> start of SDO Sequence header
806 pInstance->m_fpSdoAsySeqCb(SdoConHdl, (tEplAsySdoSeq*)&abBuffer[4], (iError - 4));
808 } // end of if(iError!=SOCKET_ERROR)
811 #if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
812 complete_and_exit(&SdoUdpInstance_g.m_CompletionUdpThread, 0);
818 #endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)