drivers/staging/epl/EplSdoUdpu.c

   1 /****************************************************************************
   2
   3   (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
   4       www.systec-electronic.com
   5
   6   Project:      openPOWERLINK
   7
   8   Description:  source file for SDO/UDP-Protocolabstractionlayer module
   9
  10   License:
  11
  12     Redistribution and use in source and binary forms, with or without
  13     modification, are permitted provided that the following conditions
  14     are met:
  15
  16     1. Redistributions of source code must retain the above copyright
  17        notice, this list of conditions and the following disclaimer.
  18
  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.
  22
  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.
  27
  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.
  40
  41     Severability Clause:
  42
  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.
  49
  50   -------------------------------------------------------------------------
  51
  52                 $RCSfile: EplSdoUdpu.c,v $
  53
  54                 $Author: D.Krueger $
  55
  56                 $Revision: 1.8 $  $Date: 2008/10/17 15:32:32 $
  57
  58                 $State: Exp $
  59
  60                 Build Environment:
  61                     GCC V3.4
  62
  63   -------------------------------------------------------------------------
  64
  65   Revision History:
  66
  67   2006/06/26 k.t.:   start of the implementation
  68
  69 ****************************************************************************/
  70
  71
  72 #include "user/EplSdoUdpu.h"
  73
  74 #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
  75
  76 #if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
  77 #include "SocketLinuxKernel.h"
  78 #include <linux/completion.h>
  79 #include <linux/sched.h>
  80 #endif
  81
  82
  83 /***************************************************************************/
  84 /*                                                                         */
  85 /*                                                                         */
  86 /*          G L O B A L   D E F I N I T I O N S                            */
  87 /*                                                                         */
  88 /*                                                                         */
  89 /***************************************************************************/
  90
  91 //---------------------------------------------------------------------------
  92 // const defines
  93 //---------------------------------------------------------------------------
  94
  95 #ifndef EPL_SDO_MAX_CONNECTION_UDP
  96 #define EPL_SDO_MAX_CONNECTION_UDP  5
  97 #endif
  98
  99 //---------------------------------------------------------------------------
 100 // local types
 101 //---------------------------------------------------------------------------
 102
 103 typedef struct
 104 {
 105     unsigned long   m_ulIpAddr;     // in network byte order
 106     unsigned int    m_uiPort;       // in network byte order
 107
 108 } tEplSdoUdpCon;
 109
 110 // instance table
 111 typedef struct
 112 {
 113     tEplSdoUdpCon           m_aSdoAbsUdpConnection[EPL_SDO_MAX_CONNECTION_UDP];
 114     tEplSequLayerReceiveCb  m_fpSdoAsySeqCb;
 115     SOCKET                  m_UdpSocket;
 116
 117 #if (TARGET_SYSTEM == _WIN32_)
 118     HANDLE                  m_ThreadHandle;
 119     LPCRITICAL_SECTION      m_pCriticalSection;
 120     CRITICAL_SECTION        m_CriticalSection;
 121
 122 #elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
 123     struct completion       m_CompletionUdpThread;
 124     int                     m_ThreadHandle;
 125     int                     m_iTerminateThread;
 126 #endif
 127
 128 } tEplSdoUdpInstance;
 129
 130 //---------------------------------------------------------------------------
 131 // modul globale vars
 132 //---------------------------------------------------------------------------
 133
 134 static tEplSdoUdpInstance  SdoUdpInstance_g;
 135
 136 //---------------------------------------------------------------------------
 137 // local function prototypes
 138 //---------------------------------------------------------------------------
 139
 140 #if (TARGET_SYSTEM == _WIN32_)
 141 static DWORD PUBLIC EplSdoUdpThread(LPVOID lpParameter);
 142
 143 #elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
 144 static int EplSdoUdpThread(void * pArg_p);
 145 #endif
 146
 147 /***************************************************************************/
 148 /*                                                                         */
 149 /*                                                                         */
 150 /*          C L A S S  <EPL-SDO-UDP-Layer>                                 */
 151 /*                                                                         */
 152 /*                                                                         */
 153 /***************************************************************************/
 154 //
 155 // Description: Protocolabstraction layer for UDP
 156 //
 157 //
 158 /***************************************************************************/
 159
 160
 161
 162 //=========================================================================//
 163 //                                                                         //
 164 //          P U B L I C   F U N C T I O N S                                //
 165 //                                                                         //
 166 //=========================================================================//
 167
 168 //---------------------------------------------------------------------------
 169 //
 170 // Function:    EplSdoUdpuInit
 171 //
 172 // Description: init first instance of the module
 173 //
 174 //
 175 //
 176 // Parameters:  pReceiveCb_p    =   functionpointer to Sdo-Sequence layer
 177 //                                  callback-function
 178 //
 179 //
 180 // Returns:     tEplKernel  = Errorcode
 181 //
 182 //
 183 // State:
 184 //
 185 //---------------------------------------------------------------------------
 186 tEplKernel PUBLIC EplSdoUdpuInit(tEplSequLayerReceiveCb fpReceiveCb_p)
 187 {
 188 tEplKernel  Ret;
 189
 190
 191     Ret = EplSdoUdpuAddInstance(fpReceiveCb_p);
 192
 193 return Ret;
 194 }
 195
 196 //---------------------------------------------------------------------------
 197 //
 198 // Function:    EplSdoUdpuAddInstance
 199 //
 200 // Description: init additional instance of the module
 201 //              înit socket and start Listen-Thread
 202 //
 203 //
 204 //
 205 // Parameters:  pReceiveCb_p    =   functionpointer to Sdo-Sequence layer
 206 //                                  callback-function
 207 //
 208 //
 209 // Returns:     tEplKernel  = Errorcode
 210 //
 211 //
 212 // State:
 213 //
 214 //---------------------------------------------------------------------------
 215 tEplKernel PUBLIC EplSdoUdpuAddInstance(tEplSequLayerReceiveCb fpReceiveCb_p)
 216 {
 217 tEplKernel          Ret;
 218
 219 #if (TARGET_SYSTEM == _WIN32_)
 220 int                 iError;
 221 WSADATA             Wsa;
 222
 223 #endif
 224
 225     // set instance variables to 0
 226     EPL_MEMSET(&SdoUdpInstance_g, 0x00, sizeof(SdoUdpInstance_g));
 227
 228     Ret = kEplSuccessful;
 229
 230     // save pointer to callback-function
 231     if (fpReceiveCb_p != NULL)
 232     {
 233         SdoUdpInstance_g.m_fpSdoAsySeqCb = fpReceiveCb_p;
 234     }
 235     else
 236     {
 237         Ret = kEplSdoUdpMissCb;
 238         goto Exit;
 239     }
 240
 241 #if (TARGET_SYSTEM == _WIN32_)
 242     // start winsock2 for win32
 243     // windows specific start of socket
 244     iError = WSAStartup(MAKEWORD(2,0),&Wsa);
 245     if (iError != 0)
 246     {
 247         Ret = kEplSdoUdpNoSocket;
 248         goto Exit;
 249     }
 250
 251     // create critical section for acccess of instnace variables
 252     SdoUdpInstance_g.m_pCriticalSection = &SdoUdpInstance_g.m_CriticalSection;
 253     InitializeCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
 254
 255 #elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
 256     init_completion(&SdoUdpInstance_g.m_CompletionUdpThread);
 257     SdoUdpInstance_g.m_iTerminateThread = 0;
 258 #endif
 259
 260     SdoUdpInstance_g.m_ThreadHandle = 0;
 261     SdoUdpInstance_g.m_UdpSocket = INVALID_SOCKET;
 262
 263     Ret = EplSdoUdpuConfig(INADDR_ANY, 0);
 264
 265 Exit:
 266     return Ret;
 267 }
 268
 269
 270 //---------------------------------------------------------------------------
 271 //
 272 // Function:    EplSdoUdpuDelInstance
 273 //
 274 // Description: del instance of the module
 275 //              del socket and del Listen-Thread
 276 //
 277 //
 278 //
 279 // Parameters:
 280 //
 281 //
 282 // Returns:     tEplKernel  = Errorcode
 283 //
 284 //
 285 // State:
 286 //
 287 //---------------------------------------------------------------------------
 288 tEplKernel PUBLIC EplSdoUdpuDelInstance()
 289 {
 290 tEplKernel      Ret;
 291
 292 #if (TARGET_SYSTEM == _WIN32_)
 293 BOOL                fTermError;
 294 #endif
 295
 296     Ret = kEplSuccessful;
 297
 298     if (SdoUdpInstance_g.m_ThreadHandle != 0)
 299     {   // listen thread was started
 300         // close thread
 301 #if (TARGET_SYSTEM == _WIN32_)
 302         fTermError = TerminateThread(SdoUdpInstance_g.m_ThreadHandle, 0);
 303         if(fTermError == FALSE)
 304         {
 305             Ret = kEplSdoUdpThreadError;
 306             goto Exit;
 307         }
 308
 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);
 314 #endif
 315
 316         SdoUdpInstance_g.m_ThreadHandle = 0;
 317     }
 318
 319     if (SdoUdpInstance_g.m_UdpSocket != INVALID_SOCKET)
 320     {
 321         // close socket
 322         closesocket(SdoUdpInstance_g.m_UdpSocket);
 323         SdoUdpInstance_g.m_UdpSocket = INVALID_SOCKET;
 324     }
 325
 326 #if (TARGET_SYSTEM == _WIN32_)
 327     // delete critical section
 328     DeleteCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
 329 #endif
 330
 331 #if (TARGET_SYSTEM == _WIN32_)
 332     // for win 32
 333     WSACleanup();
 334 #endif
 335
 336 #if (TARGET_SYSTEM == _WIN32_)
 337 Exit:
 338 #endif
 339     return Ret;
 340 }
 341
 342 //---------------------------------------------------------------------------
 343 //
 344 // Function:    EplSdoUdpuConfig
 345 //
 346 // Description: reconfigurate socket with new IP-Address
 347 //              -> needed for NMT ResetConfiguration
 348 //
 349 // Parameters:  ulIpAddr_p      = IpAddress in platform byte order
 350 //              uiPort_p        = port number in platform byte order
 351 //
 352 //
 353 // Returns:     tEplKernel  = Errorcode
 354 //
 355 //
 356 // State:
 357 //
 358 //---------------------------------------------------------------------------
 359 tEplKernel PUBLIC EplSdoUdpuConfig(unsigned long ulIpAddr_p, unsigned int uiPort_p)
 360 {
 361 tEplKernel          Ret;
 362 struct sockaddr_in  Addr;
 363 int                 iError;
 364
 365 #if (TARGET_SYSTEM == _WIN32_)
 366 BOOL                fTermError;
 367 unsigned long       ulThreadId;
 368 #endif
 369
 370     Ret = kEplSuccessful;
 371
 372     if (uiPort_p == 0)
 373     {   // set UDP port to default port number
 374         uiPort_p = EPL_C_SDO_EPL_PORT;
 375     }
 376     else if (uiPort_p > 65535)
 377     {
 378         Ret = kEplSdoUdpSocketError;
 379         goto Exit;
 380     }
 381
 382     if (SdoUdpInstance_g.m_ThreadHandle != 0)
 383     {   // listen thread was started
 384
 385         // close old thread
 386 #if (TARGET_SYSTEM == _WIN32_)
 387         fTermError = TerminateThread(SdoUdpInstance_g.m_ThreadHandle, 0);
 388         if(fTermError == FALSE)
 389         {
 390             Ret = kEplSdoUdpThreadError;
 391             goto Exit;
 392         }
 393
 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;
 400 #endif
 401
 402         SdoUdpInstance_g.m_ThreadHandle = 0;
 403     }
 404
 405     if (SdoUdpInstance_g.m_UdpSocket != INVALID_SOCKET)
 406     {
 407         // close socket
 408         iError = closesocket(SdoUdpInstance_g.m_UdpSocket);
 409         SdoUdpInstance_g.m_UdpSocket = INVALID_SOCKET;
 410         if(iError != 0)
 411         {
 412             Ret = kEplSdoUdpSocketError;
 413             goto Exit;
 414         }
 415     }
 416
 417     // create Socket
 418     SdoUdpInstance_g.m_UdpSocket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
 419     if (SdoUdpInstance_g.m_UdpSocket == INVALID_SOCKET)
 420     {
 421         Ret = kEplSdoUdpNoSocket;
 422         EPL_DBGLVL_SDO_TRACE0("EplSdoUdpuConfig: socket() failed\n");
 423         goto Exit;
 424     }
 425
 426     // bind socket
 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));
 431     if (iError < 0)
 432     {
 433         //iError = WSAGetLastError();
 434         EPL_DBGLVL_SDO_TRACE1("EplSdoUdpuConfig: bind() finished with %i\n", iError);
 435         Ret = kEplSdoUdpNoSocket;
 436         goto Exit;
 437     }
 438
 439     // create Listen-Thread
 440 #if (TARGET_SYSTEM == _WIN32_)
 441     // for win32
 442
 443     // create thread
 444     SdoUdpInstance_g.m_ThreadHandle = CreateThread(NULL,
 445                                                     0,
 446                                                     EplSdoUdpThread,
 447                                                     &SdoUdpInstance_g,
 448                                                     0,
 449                                                     &ulThreadId);
 450     if(SdoUdpInstance_g.m_ThreadHandle == NULL)
 451     {
 452         Ret = kEplSdoUdpThreadError;
 453         goto Exit;
 454     }
 455
 456 #elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
 457
 458     SdoUdpInstance_g.m_ThreadHandle = kernel_thread(EplSdoUdpThread, &SdoUdpInstance_g, CLONE_KERNEL);
 459     if(SdoUdpInstance_g.m_ThreadHandle == 0)
 460     {
 461         Ret = kEplSdoUdpThreadError;
 462         goto Exit;
 463     }
 464 #endif
 465
 466
 467 Exit:
 468     return Ret;
 469
 470 }
 471
 472
 473 //---------------------------------------------------------------------------
 474 //
 475 // Function:    EplSdoUdpuInitCon
 476 //
 477 // Description: init a new connect
 478 //
 479 //
 480 //
 481 // Parameters:  pSdoConHandle_p = pointer for the new connection handle
 482 //              uiTargetNodeId_p = NodeId of the target node
 483 //
 484 //
 485 // Returns:     tEplKernel  = Errorcode
 486 //
 487 //
 488 // State:
 489 //
 490 //---------------------------------------------------------------------------
 491 tEplKernel PUBLIC EplSdoUdpuInitCon(tEplSdoConHdl*  pSdoConHandle_p,
 492                                     unsigned int    uiTargetNodeId_p)
 493 {
 494 tEplKernel          Ret;
 495 unsigned int        uiCount;
 496 unsigned int        uiFreeCon;
 497 tEplSdoUdpCon*      pSdoUdpCon;
 498
 499     Ret = kEplSuccessful;
 500
 501     // get free entry in control structure
 502     uiCount = 0;
 503     uiFreeCon = EPL_SDO_MAX_CONNECTION_UDP;
 504     pSdoUdpCon = &SdoUdpInstance_g.m_aSdoAbsUdpConnection[0];
 505     while (uiCount < EPL_SDO_MAX_CONNECTION_UDP)
 506     {
 507         if ((pSdoUdpCon->m_ulIpAddr & htonl(0xFF)) == htonl(uiTargetNodeId_p))
 508         {   // existing connection to target node found
 509             // set handle
 510             *pSdoConHandle_p = (uiCount | EPL_SDO_UDP_HANDLE);
 511
 512             goto Exit;
 513         }
 514         else if ((pSdoUdpCon->m_ulIpAddr == 0)
 515                 && (pSdoUdpCon->m_uiPort == 0))
 516         {
 517             uiFreeCon = uiCount;
 518         }
 519         uiCount++;
 520         pSdoUdpCon++;
 521     }
 522
 523     if (uiFreeCon == EPL_SDO_MAX_CONNECTION_UDP)
 524     {
 525         // error no free handle
 526         Ret = kEplSdoUdpNoFreeHandle;
 527     }
 528     else
 529     {
 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
 534
 535         // set handle
 536         *pSdoConHandle_p = (uiFreeCon | EPL_SDO_UDP_HANDLE);
 537
 538     }
 539
 540 Exit:
 541     return Ret;
 542
 543 }
 544
 545 //---------------------------------------------------------------------------
 546 //
 547 // Function:    EplSdoUdpuSendData
 548 //
 549 // Description: send data using exisiting connection
 550 //
 551 //
 552 //
 553 // Parameters:  SdoConHandle_p  = connection handle
 554 //              pSrcData_p      = pointer to data
 555 //              dwDataSize_p    = number of databyte
 556 //                                  -> without asend-header!!!
 557 //
 558 // Returns:     tEplKernel  = Errorcode
 559 //
 560 //
 561 // State:
 562 //
 563 //---------------------------------------------------------------------------
 564 tEplKernel PUBLIC EplSdoUdpuSendData(tEplSdoConHdl       SdoConHandle_p,
 565                                     tEplFrame *          pSrcData_p,
 566                                     DWORD                dwDataSize_p)
 567 {
 568 tEplKernel          Ret;
 569 int                 iError;
 570 unsigned int        uiArray;
 571 struct sockaddr_in  Addr;
 572
 573     Ret = kEplSuccessful;
 574
 575     uiArray = (SdoConHandle_p & ~EPL_SDO_ASY_HANDLE_MASK);
 576     if(uiArray >= EPL_SDO_MAX_CONNECTION_UDP)
 577     {
 578         Ret = kEplSdoUdpInvalidHdl;
 579         goto Exit;
 580     }
 581     //set message type
 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);
 587
 588     // calc size
 589     dwDataSize_p += EPL_ASND_HEADER_SIZE;
 590
 591     // call sendto
 592     Addr.sin_family = AF_INET;
 593 #if (TARGET_SYSTEM == _WIN32_)
 594         // enter  critical section for process function
 595     EnterCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
 596 #endif
 597
 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;
 600
 601 #if (TARGET_SYSTEM == _WIN32_)
 602     // leave critical section for process function
 603     LeaveCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
 604 #endif
 605
 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
 609                 0,                                     // flags
 610                 (struct sockaddr*)&Addr,                      // target
 611                 sizeof(struct sockaddr_in));                  // sizeof targetadress
 612     if(iError < 0)
 613     {
 614         EPL_DBGLVL_SDO_TRACE1("EplSdoUdpuSendData: sendto() finished with %i\n", iError);
 615         Ret = kEplSdoUdpSendError;
 616         goto Exit;
 617     }
 618
 619 Exit:
 620     return Ret;
 621
 622 }
 623
 624
 625 //---------------------------------------------------------------------------
 626 //
 627 // Function:    EplSdoUdpuDelCon
 628 //
 629 // Description: delete connection from intern structure
 630 //
 631 //
 632 //
 633 // Parameters:  SdoConHandle_p  = connection handle
 634 //
 635 // Returns:     tEplKernel  = Errorcode
 636 //
 637 //
 638 // State:
 639 //
 640 //---------------------------------------------------------------------------
 641 tEplKernel PUBLIC EplSdoUdpuDelCon(tEplSdoConHdl SdoConHandle_p)
 642 {
 643 tEplKernel      Ret;
 644 unsigned int    uiArray;
 645
 646
 647     uiArray = (SdoConHandle_p & ~EPL_SDO_ASY_HANDLE_MASK);
 648
 649     if(uiArray >= EPL_SDO_MAX_CONNECTION_UDP)
 650     {
 651         Ret = kEplSdoUdpInvalidHdl;
 652         goto Exit;
 653     }
 654     else
 655     {
 656         Ret = kEplSuccessful;
 657     }
 658
 659
 660     // delete connection
 661     SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiArray].m_ulIpAddr = 0;
 662     SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiArray].m_uiPort = 0;
 663
 664 Exit:
 665     return Ret;
 666 }
 667
 668 //=========================================================================//
 669 //                                                                         //
 670 //          P R I V A T E   F U N C T I O N S                              //
 671 //                                                                         //
 672 //=========================================================================//
 673
 674 //---------------------------------------------------------------------------
 675 //
 676 // Function:        EplSdoUdpThread
 677 //
 678 // Description:     thread check socket for new data
 679 //
 680 //
 681 //
 682 // Parameters:      lpParameter = pointer to parameter type tEplSdoUdpThreadPara
 683 //
 684 //
 685 // Returns:         DWORD   =   errorcode
 686 //
 687 //
 688 // State:
 689 //
 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)
 695 #endif
 696 {
 697
 698 tEplSdoUdpInstance* pInstance;
 699 struct sockaddr_in  RemoteAddr;
 700 int                 iError;
 701 int                 iCount;
 702 int                 iFreeEntry;
 703 BYTE                abBuffer[EPL_MAX_SDO_REC_FRAME_SIZE];
 704 unsigned int        uiSize;
 705 tEplSdoConHdl       SdoConHdl;
 706
 707 #if (TARGET_SYSTEM == _WIN32_)
 708     pInstance = (tEplSdoUdpInstance*)lpParameter;
 709
 710     for (;;)
 711
 712 #elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
 713     pInstance = (tEplSdoUdpInstance*)pArg_p;
 714     daemonize("EplSdoUdpThread");
 715     allow_signal( SIGTERM );
 716
 717     for (;pInstance->m_iTerminateThread == 0;)
 718 #endif
 719
 720     {
 721         // wait for data
 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
 726                         0,                          // flags
 727                         (struct sockaddr*)&RemoteAddr,
 728                         (int*)&uiSize);
 729 #if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
 730         if (iError == -ERESTARTSYS)
 731         {
 732             break;
 733         }
 734 #endif
 735         if (iError > 0)
 736         {
 737             // get handle for higher layer
 738             iCount = 0;
 739             iFreeEntry = 0xFFFF;
 740 #if (TARGET_SYSTEM == _WIN32_)
 741         // enter  critical section for process function
 742     EnterCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
 743 #endif
 744             while (iCount < EPL_SDO_MAX_CONNECTION_UDP)
 745             {
 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))
 749                 {
 750                     break;
 751                 }
 752
 753                 if((pInstance->m_aSdoAbsUdpConnection[iCount].m_ulIpAddr == 0)
 754                     && (pInstance->m_aSdoAbsUdpConnection[iCount].m_uiPort == 0)
 755                     && (iFreeEntry == 0xFFFF))
 756
 757                 {
 758                     iFreeEntry  = iCount;
 759                 }
 760
 761                 iCount++;
 762             }
 763
 764             if (iCount == EPL_SDO_MAX_CONNECTION_UDP)
 765             {
 766                 // connection unknown
 767                 // see if there is a free handle
 768                 if (iFreeEntry != 0xFFFF)
 769                 {
 770                     // save adress infos
 771                     pInstance->m_aSdoAbsUdpConnection[iFreeEntry].m_ulIpAddr =
 772                         RemoteAddr.sin_addr.s_addr;
 773                     pInstance->m_aSdoAbsUdpConnection[iFreeEntry].m_uiPort =
 774                         RemoteAddr.sin_port;
 775 #if (TARGET_SYSTEM == _WIN32_)
 776     // leave critical section for process function
 777     LeaveCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
 778 #endif
 779                     // call callback
 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));
 784                 }
 785                 else
 786                 {
 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);
 791 #endif
 792                 }
 793
 794             }
 795             else
 796             {
 797                 // known connection
 798                 // call callback with correct handle
 799                 SdoConHdl = iCount;
 800                 SdoConHdl |= EPL_SDO_UDP_HANDLE;
 801 #if (TARGET_SYSTEM == _WIN32_)
 802     // leave critical section for process function
 803     LeaveCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
 804 #endif
 805                 // offset 4 -> start of SDO Sequence header
 806                 pInstance->m_fpSdoAsySeqCb(SdoConHdl, (tEplAsySdoSeq*)&abBuffer[4], (iError - 4));
 807             }
 808         } // end of  if(iError!=SOCKET_ERROR)
 809     }// end of for(;;)
 810
 811 #if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
 812     complete_and_exit(&SdoUdpInstance_g.m_CompletionUdpThread, 0);
 813 #endif
 814
 815     return 0;
 816 }
 817
 818 #endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
 819
 820 // EOF
 821