?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
added 2.6.29.6 aldebaran kernel
[nao-ulib.git] / kernel / 2.6.29.6-aldebaran-rt / drivers / staging / epl / EplSdoAsySequ.c
blob6b6a9975d78b17f9119dd3d4bbcb9ce540c073af
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 asychronous SDO Sequence Layer 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: EplSdoAsySequ.c,v $
53
54 $Author: D.Krueger $
55
56 $Revision: 1.10 $ $Date: 2008/11/13 17:13:09 $
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 #include "user/EplSdoAsySequ.h"
72
73 #if ((((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) == 0) &&\
74 (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) == 0) )
75
76 #error 'ERROR: At least UDP or Asnd module needed!'
77
78 #endif
79 /***************************************************************************/
80 /* */
81 /* */
82 /* G L O B A L D E F I N I T I O N S */
83 /* */
84 /* */
85 /***************************************************************************/
86
87 //---------------------------------------------------------------------------
88 // const defines
89 //---------------------------------------------------------------------------
90
91 #define EPL_SDO_HISTORY_SIZE 5
92
93 #ifndef EPL_MAX_SDO_SEQ_CON
94 #define EPL_MAX_SDO_SEQ_CON 10
95 #endif
96
97 #define EPL_SEQ_DEFAULT_TIMEOUT 5000 // in [ms] => 5 sec
98
99 #define EPL_SEQ_RETRY_COUNT 5 // => max. Timeout 30 sec
100
101 #define EPL_SEQ_NUM_THRESHOLD 100 // threshold which distinguishes between old and new sequence numbers
102
103 // define frame with size of Asnd-Header-, SDO Sequenze Header size, SDO Command header
104 // and Ethernet-Header size
105 #define EPL_SEQ_FRAME_SIZE 24
106 // size of the header of the asynchronus SDO Sequence layer
107 #define EPL_SEQ_HEADER_SIZE 4
108
109 // buffersize for one frame in history
110 #define EPL_SEQ_HISTROY_FRAME_SIZE EPL_MAX_SDO_FRAME_SIZE
111
112 // mask to get scon and rcon
113 #define EPL_ASY_SDO_CON_MASK 0x03
114
115 //---------------------------------------------------------------------------
116 // local types
117 //---------------------------------------------------------------------------
118
119 // events for processfunction
120 typedef enum {
121 kAsySdoSeqEventNoEvent = 0x00, // no Event
122 kAsySdoSeqEventInitCon = 0x01, // init connection
123 kAsySdoSeqEventFrameRec = 0x02, // frame received
124 kAsySdoSeqEventFrameSend = 0x03, // frame to send
125 kAsySdoSeqEventTimeout = 0x04, // Timeout for connection
126 kAsySdoSeqEventCloseCon = 0x05 // higher layer close connection
127 } tEplAsySdoSeqEvent;
128
129 // structure for History-Buffer
130 typedef struct {
131 BYTE m_bFreeEntries;
132 BYTE m_bWrite; // index of the next free buffer entry
133 BYTE m_bAck; // index of the next message which should become acknowledged
134 BYTE m_bRead; // index between m_bAck and m_bWrite to the next message for retransmission
135 BYTE m_aabHistoryFrame[EPL_SDO_HISTORY_SIZE]
136 [EPL_SEQ_HISTROY_FRAME_SIZE];
137 unsigned int m_auiFrameSize[EPL_SDO_HISTORY_SIZE];
138
139 } tEplAsySdoConHistory;
140
141 // state of the statemaschine
142 typedef enum {
143 kEplAsySdoStateIdle = 0x00,
144 kEplAsySdoStateInit1 = 0x01,
145 kEplAsySdoStateInit2 = 0x02,
146 kEplAsySdoStateInit3 = 0x03,
147 kEplAsySdoStateConnected = 0x04,
148 kEplAsySdoStateWaitAck = 0x05
149 } tEplAsySdoState;
150
151 // connection control structure
152 typedef struct {
153 tEplSdoConHdl m_ConHandle;
154 tEplAsySdoState m_SdoState;
155 BYTE m_bRecSeqNum; // name from view of the communication partner
156 BYTE m_bSendSeqNum; // name from view of the communication partner
157 tEplAsySdoConHistory m_SdoConHistory;
158 tEplTimerHdl m_EplTimerHdl;
159 unsigned int m_uiRetryCount; // retry counter
160 unsigned int m_uiUseCount; // one sequence layer connection may be used by
161 // multiple command layer connections
162
163 } tEplAsySdoSeqCon;
164
165 // instance structure
166 typedef struct {
167 tEplAsySdoSeqCon m_AsySdoConnection[EPL_MAX_SDO_SEQ_CON];
168 tEplSdoComReceiveCb m_fpSdoComReceiveCb;
169 tEplSdoComConCb m_fpSdoComConCb;
170
171 #if defined(WIN32) || defined(_WIN32)
172 LPCRITICAL_SECTION m_pCriticalSection;
173 CRITICAL_SECTION m_CriticalSection;
174
175 LPCRITICAL_SECTION m_pCriticalSectionReceive;
176 CRITICAL_SECTION m_CriticalSectionReceive;
177 #endif
178
179 } tEplAsySdoSequInstance;
180
181 //---------------------------------------------------------------------------
182 // modul globale vars
183 //---------------------------------------------------------------------------
184
185 static tEplAsySdoSequInstance AsySdoSequInstance_g;
186
187 //---------------------------------------------------------------------------
188 // local function prototypes
189 //---------------------------------------------------------------------------
190
191 static tEplKernel EplSdoAsySeqProcess(unsigned int uiHandle_p,
192 unsigned int uiDataSize_p,
193 tEplFrame * pData_p,
194 tEplAsySdoSeq * pRecFrame_p,
195 tEplAsySdoSeqEvent Event_p);
196
197 static tEplKernel EplSdoAsySeqSendIntern(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
198 unsigned int uiDataSize_p,
199 tEplFrame * pData_p,
200 BOOL fFrameInHistory);
201
202 static tEplKernel EplSdoAsySeqSendLowerLayer(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
203 unsigned int uiDataSize_p,
204 tEplFrame * pEplFrame_p);
205
206 tEplKernel PUBLIC EplSdoAsyReceiveCb(tEplSdoConHdl ConHdl_p,
207 tEplAsySdoSeq * pSdoSeqData_p,
208 unsigned int uiDataSize_p);
209
210 static tEplKernel EplSdoAsyInitHistory(void);
211
212 static tEplKernel EplSdoAsyAddFrameToHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
213 tEplFrame * pFrame_p,
214 unsigned int uiSize_p);
215
216 static tEplKernel EplSdoAsyAckFrameToHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
217 BYTE bRecSeqNumber_p);
218
219 static tEplKernel EplSdoAsyReadFromHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
220 tEplFrame ** ppFrame_p,
221 unsigned int *puiSize_p,
222 BOOL fInitRead);
223
224 static unsigned int EplSdoAsyGetFreeEntriesFromHistory(tEplAsySdoSeqCon *
225 pAsySdoSeqCon_p);
226
227 static tEplKernel EplSdoAsySeqSetTimer(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
228 unsigned long ulTimeout);
229
230 /***************************************************************************/
231 /* */
232 /* */
233 /* C L A S S <EPL asychronus SDO Sequence layer> */
234 /* */
235 /* */
236 /***************************************************************************/
237 //
238 // Description: this module contains the asynchronus SDO Sequence Layer for
239 // the EPL SDO service
240 //
241 //
242 /***************************************************************************/
243
244 //=========================================================================//
245 // //
246 // P U B L I C F U N C T I O N S //
247 // //
248 //=========================================================================//
249
250 //---------------------------------------------------------------------------
251 //
252 // Function: EplSdoAsySeqInit
253 //
254 // Description: init first instance
255 //
256 //
257 //
258 // Parameters: fpSdoComCb_p = callback function to inform Command layer
259 // about new frames
260 // fpSdoComConCb_p = callback function to inform command layer
261 // about connection state
262 //
263 //
264 // Returns: tEplKernel = errorcode
265 //
266 //
267 // State:
268 //
269 //---------------------------------------------------------------------------
270 tEplKernel PUBLIC EplSdoAsySeqInit(tEplSdoComReceiveCb fpSdoComCb_p,
271 tEplSdoComConCb fpSdoComConCb_p)
272 {
273 tEplKernel Ret;
274
275 Ret = EplSdoAsySeqAddInstance(fpSdoComCb_p, fpSdoComConCb_p);
276
277 return Ret;
278
279 }
280
281 //---------------------------------------------------------------------------
282 //
283 // Function: EplSdoAsySeqAddInstance
284 //
285 // Description: init following instances
286 //
287 //
288 //
289 // Parameters: fpSdoComCb_p = callback function to inform Command layer
290 // about new frames
291 // fpSdoComConCb_p = callback function to inform command layer
292 // about connection state
293 //
294 // Returns: tEplKernel = errorcode
295 //
296 //
297 // State:
298 //
299 //---------------------------------------------------------------------------
300 tEplKernel PUBLIC EplSdoAsySeqAddInstance(tEplSdoComReceiveCb fpSdoComCb_p,
301 tEplSdoComConCb fpSdoComConCb_p)
302 {
303 tEplKernel Ret;
304
305 Ret = kEplSuccessful;
306
307 // check functionpointer
308 if (fpSdoComCb_p == NULL) {
309 Ret = kEplSdoSeqMissCb;
310 goto Exit;
311 } else {
312 AsySdoSequInstance_g.m_fpSdoComReceiveCb = fpSdoComCb_p;
313 }
314
315 // check functionpointer
316 if (fpSdoComConCb_p == NULL) {
317 Ret = kEplSdoSeqMissCb;
318 goto Exit;
319 } else {
320 AsySdoSequInstance_g.m_fpSdoComConCb = fpSdoComConCb_p;
321 }
322
323 // set controllstructure to 0
324 EPL_MEMSET(&AsySdoSequInstance_g.m_AsySdoConnection[0], 0x00,
325 sizeof(AsySdoSequInstance_g.m_AsySdoConnection));
326
327 // init History
328 Ret = EplSdoAsyInitHistory();
329 if (Ret != kEplSuccessful) {
330 goto Exit;
331 }
332 #if defined(WIN32) || defined(_WIN32)
333 // create critical section for process function
334 AsySdoSequInstance_g.m_pCriticalSection =
335 &AsySdoSequInstance_g.m_CriticalSection;
336 InitializeCriticalSection(AsySdoSequInstance_g.m_pCriticalSection);
337
338 // init critical section for receive cb function
339 AsySdoSequInstance_g.m_pCriticalSectionReceive =
340 &AsySdoSequInstance_g.m_CriticalSectionReceive;
341 InitializeCriticalSection(AsySdoSequInstance_g.
342 m_pCriticalSectionReceive);
343 #endif
344
345 #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
346 // init lower layer
347 Ret = EplSdoUdpuAddInstance(EplSdoAsyReceiveCb);
348 if (Ret != kEplSuccessful) {
349 goto Exit;
350 }
351 #endif
352
353 #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
354 // init lower layer
355 Ret = EplSdoAsnduAddInstance(EplSdoAsyReceiveCb);
356 if (Ret != kEplSuccessful) {
357 goto Exit;
358 }
359 #endif
360
361 Exit:
362 return Ret;
363
364 }
365
366 //---------------------------------------------------------------------------
367 //
368 // Function: EplSdoAsySeqDelInstance
369 //
370 // Description: delete instances
371 //
372 //
373 //
374 // Parameters:
375 //
376 //
377 // Returns: tEplKernel = errorcode
378 //
379 //
380 // State:
381 //
382 //---------------------------------------------------------------------------
383 tEplKernel PUBLIC EplSdoAsySeqDelInstance()
384 {
385 tEplKernel Ret;
386 unsigned int uiCount;
387 tEplAsySdoSeqCon *pAsySdoSeqCon;
388
389 Ret = kEplSuccessful;
390
391 // delete timer of open connections
392 uiCount = 0;
393 pAsySdoSeqCon = &AsySdoSequInstance_g.m_AsySdoConnection[0];
394 while (uiCount < EPL_MAX_SDO_SEQ_CON) {
395 if (pAsySdoSeqCon->m_ConHandle != 0) {
396 EplTimeruDeleteTimer(&pAsySdoSeqCon->m_EplTimerHdl);
397 }
398 uiCount++;
399 pAsySdoSeqCon++;
400 }
401
402 #if defined(WIN32) || defined(_WIN32)
403 // delete critical section for process function
404 DeleteCriticalSection(AsySdoSequInstance_g.m_pCriticalSection);
405 #endif
406
407 // set instance-table to 0
408 EPL_MEMSET(&AsySdoSequInstance_g, 0x00, sizeof(AsySdoSequInstance_g));
409
410 #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
411 // delete lower layer
412 Ret = EplSdoUdpuDelInstance();
413 #endif
414
415 #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
416 // delete lower layer
417 Ret = EplSdoAsnduDelInstance();
418 #endif
419
420 return Ret;
421 }
422
423 //---------------------------------------------------------------------------
424 //
425 // Function: EplSdoAsySeqInitCon
426 //
427 // Description: start initialization of a sequence layer connection.
428 // It tries to reuse an existing connection to the same node.
429 //
430 //
431 // Parameters: pSdoSeqConHdl_p = pointer to the variable for the connection handle
432 // uiNodeId_p = Node Id of the target
433 // SdoType = Type of the SDO connection
434 //
435 //
436 // Returns: tEplKernel = errorcode
437 //
438 //
439 // State:
440 //
441 //---------------------------------------------------------------------------
442 tEplKernel PUBLIC EplSdoAsySeqInitCon(tEplSdoSeqConHdl * pSdoSeqConHdl_p,
443 unsigned int uiNodeId_p,
444 tEplSdoType SdoType)
445 {
446 tEplKernel Ret;
447 unsigned int uiCount;
448 unsigned int uiFreeCon;
449 tEplSdoConHdl ConHandle;
450 tEplAsySdoSeqCon *pAsySdoSeqCon;
451 Ret = kEplSuccessful;
452
453 // check SdoType
454 // call init function of the protcol abstraction layer
455 // which tries to find an existing connection to the same node
456 switch (SdoType) {
457 // SDO over UDP
458 case kEplSdoTypeUdp:
459 {
460 #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
461 Ret = EplSdoUdpuInitCon(&ConHandle, uiNodeId_p);
462 if (Ret != kEplSuccessful) {
463 goto Exit;
464 }
465 #else
466 Ret = kEplSdoSeqUnsupportedProt;
467 #endif
468 break;
469 }
470
471 // SDO over Asnd
472 case kEplSdoTypeAsnd:
473 {
474 #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
475 Ret = EplSdoAsnduInitCon(&ConHandle, uiNodeId_p);
476 if (Ret != kEplSuccessful) {
477 goto Exit;
478 }
479 #else
480 Ret = kEplSdoSeqUnsupportedProt;
481 #endif
482 break;
483 }
484
485 // unsupported protocols
486 // -> auto should be replaced by command layer
487 case kEplSdoTypeAuto:
488 case kEplSdoTypePdo:
489 default:
490 {
491 Ret = kEplSdoSeqUnsupportedProt;
492 goto Exit;
493 }
494
495 } // end of switch(SdoType)
496
497 // find existing connection to the same node or find empty entry for connection
498 uiCount = 0;
499 uiFreeCon = EPL_MAX_SDO_SEQ_CON;
500 pAsySdoSeqCon = &AsySdoSequInstance_g.m_AsySdoConnection[0];
501
502 while (uiCount < EPL_MAX_SDO_SEQ_CON) {
503 if (pAsySdoSeqCon->m_ConHandle == ConHandle) { // existing connection found
504 break;
505 }
506 if (pAsySdoSeqCon->m_ConHandle == 0) {
507 uiFreeCon = uiCount;
508 }
509 uiCount++;
510 pAsySdoSeqCon++;
511 }
512
513 if (uiCount == EPL_MAX_SDO_SEQ_CON) {
514 if (uiFreeCon == EPL_MAX_SDO_SEQ_CON) { // no free entry found
515 switch (SdoType) {
516 // SDO over UDP
517 case kEplSdoTypeUdp:
518 {
519 #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
520 Ret = EplSdoUdpuDelCon(ConHandle);
521 if (Ret != kEplSuccessful) {
522 goto Exit;
523 }
524 #endif
525 break;
526 }
527
528 // SDO over Asnd
529 case kEplSdoTypeAsnd:
530 {
531 #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
532 Ret = EplSdoAsnduDelCon(ConHandle);
533 if (Ret != kEplSuccessful) {
534 goto Exit;
535 }
536 #endif
537 break;
538 }
539
540 // unsupported protocols
541 // -> auto should be replaced by command layer
542 case kEplSdoTypeAuto:
543 case kEplSdoTypePdo:
544 default:
545 {
546 Ret = kEplSdoSeqUnsupportedProt;
547 goto Exit;
548 }
549
550 } // end of switch(SdoType)
551
552 Ret = kEplSdoSeqNoFreeHandle;
553 goto Exit;
554 } else { // free entry found
555 pAsySdoSeqCon =
556 &AsySdoSequInstance_g.m_AsySdoConnection[uiFreeCon];
557 pAsySdoSeqCon->m_ConHandle = ConHandle;
558 uiCount = uiFreeCon;
559 }
560 }
561 // set handle
562 *pSdoSeqConHdl_p = (uiCount | EPL_SDO_ASY_HANDLE);
563
564 // increment use counter
565 pAsySdoSeqCon->m_uiUseCount++;
566
567 // call intern process function
568 Ret = EplSdoAsySeqProcess(uiCount,
569 0, NULL, NULL, kAsySdoSeqEventInitCon);
570
571 Exit:
572 return Ret;
573 }
574
575 //---------------------------------------------------------------------------
576 //
577 // Function: EplSdoAsySeqSendData
578 //
579 // Description: send sata unsing a established connection
580 //
581 //
582 //
583 // Parameters: pSdoSeqConHdl_p = connection handle
584 // uiDataSize_p = Size of Frame to send
585 // -> wihtout SDO sequence layer header, Asnd header
586 // and ethernetnet
587 // ==> SDO Sequence layer payload
588 // SdoType = Type of the SDO connection
589 //
590 //
591 // Returns: tEplKernel = errorcode
592 //
593 //
594 // State:
595 //
596 //---------------------------------------------------------------------------
597 tEplKernel PUBLIC EplSdoAsySeqSendData(tEplSdoSeqConHdl SdoSeqConHdl_p,
598 unsigned int uiDataSize_p,
599 tEplFrame * pabData_p)
600 {
601 tEplKernel Ret;
602 unsigned int uiHandle;
603
604 uiHandle = (SdoSeqConHdl_p & ~EPL_SDO_SEQ_HANDLE_MASK);
605
606 // check if connection ready
607 if (AsySdoSequInstance_g.m_AsySdoConnection[uiHandle].m_SdoState ==
608 kEplAsySdoStateIdle) {
609 // no connection with this handle
610 Ret = kEplSdoSeqInvalidHdl;
611 goto Exit;
612 } else if (AsySdoSequInstance_g.m_AsySdoConnection[uiHandle].
613 m_SdoState != kEplAsySdoStateConnected) {
614 Ret = kEplSdoSeqConnectionBusy;
615 goto Exit;
616 }
617
618 Ret = EplSdoAsySeqProcess(uiHandle,
619 uiDataSize_p,
620 pabData_p, NULL, kAsySdoSeqEventFrameSend);
621 Exit:
622 return Ret;
623 }
624
625 //---------------------------------------------------------------------------
626 //
627 // Function: EplSdoAsySeqProcessEvent
628 //
629 // Description: function processes extern events
630 // -> later needed for timeout controll with timer-module
631 //
632 //
633 //
634 // Parameters: pEvent_p = pointer to event
635 //
636 //
637 // Returns: tEplKernel = errorcode
638 //
639 //
640 // State:
641 //
642 //---------------------------------------------------------------------------
643 tEplKernel PUBLIC EplSdoAsySeqProcessEvent(tEplEvent * pEvent_p)
644 {
645 tEplKernel Ret;
646 tEplTimerEventArg *pTimerEventArg;
647 tEplAsySdoSeqCon *pAsySdoSeqCon;
648 tEplTimerHdl EplTimerHdl;
649 unsigned int uiCount;
650
651 Ret = kEplSuccessful;
652 // check parameter
653 if (pEvent_p == NULL) {
654 Ret = kEplSdoSeqInvalidEvent;
655 goto Exit;
656 }
657
658 if (pEvent_p->m_EventType != kEplEventTypeTimer) {
659 Ret = kEplSdoSeqInvalidEvent;
660 goto Exit;
661 }
662 // get timerhdl
663 pTimerEventArg = (tEplTimerEventArg *) pEvent_p->m_pArg;
664 EplTimerHdl = pTimerEventArg->m_TimerHdl;
665
666 // get pointer to intern control structure of connection
667 if (pTimerEventArg->m_ulArg == 0) {
668 goto Exit;
669 }
670 pAsySdoSeqCon = (tEplAsySdoSeqCon *) pTimerEventArg->m_ulArg;
671
672 // check if time is current
673 if (EplTimerHdl != pAsySdoSeqCon->m_EplTimerHdl) {
674 // delete timer
675 EplTimeruDeleteTimer(&EplTimerHdl);
676 goto Exit;
677 }
678 // delete timer
679 EplTimeruDeleteTimer(&pAsySdoSeqCon->m_EplTimerHdl);
680
681 // get indexnumber of control structure
682 uiCount = 0;
683 while ((&AsySdoSequInstance_g.m_AsySdoConnection[uiCount]) !=
684 pAsySdoSeqCon) {
685 uiCount++;
686 if (uiCount > EPL_MAX_SDO_SEQ_CON) {
687 goto Exit;
688 }
689 }
690
691 // process event and call processfunction if needed
692 Ret = EplSdoAsySeqProcess(uiCount,
693 0, NULL, NULL, kAsySdoSeqEventTimeout);
694
695 Exit:
696 return Ret;
697
698 }
699
700 //---------------------------------------------------------------------------
701 //
702 // Function: EplSdoAsySeqDelCon
703 //
704 // Description: del and close one connection
705 //
706 //
707 //
708 // Parameters: SdoSeqConHdl_p = handle of connection
709 //
710 //
711 // Returns: tEplKernel = errorcode
712 //
713 //
714 // State:
715 //
716 //---------------------------------------------------------------------------
717 tEplKernel PUBLIC EplSdoAsySeqDelCon(tEplSdoSeqConHdl SdoSeqConHdl_p)
718 {
719 tEplKernel Ret = kEplSuccessful;
720 unsigned int uiHandle;
721 tEplAsySdoSeqCon *pAsySdoSeqCon;
722
723 uiHandle = (SdoSeqConHdl_p & ~EPL_SDO_SEQ_HANDLE_MASK);
724
725 // check if handle invalid
726 if (uiHandle >= EPL_MAX_SDO_SEQ_CON) {
727 Ret = kEplSdoSeqInvalidHdl;
728 goto Exit;
729 }
730 // get pointer to connection
731 pAsySdoSeqCon = &AsySdoSequInstance_g.m_AsySdoConnection[uiHandle];
732
733 // decrement use counter
734 pAsySdoSeqCon->m_uiUseCount--;
735
736 if (pAsySdoSeqCon->m_uiUseCount == 0) {
737 // process close in processfunction
738 Ret = EplSdoAsySeqProcess(uiHandle,
739 0,
740 NULL, NULL, kAsySdoSeqEventCloseCon);
741
742 //check protocol
743 if ((pAsySdoSeqCon->m_ConHandle & EPL_SDO_ASY_HANDLE_MASK) ==
744 EPL_SDO_UDP_HANDLE) {
745 #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
746 // call close function of lower layer
747 EplSdoUdpuDelCon(pAsySdoSeqCon->m_ConHandle);
748 #endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
749 } else {
750 #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
751 // call close function of lower layer
752 EplSdoAsnduDelCon(pAsySdoSeqCon->m_ConHandle);
753 #endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
754 }
755
756 // delete timer
757 EplTimeruDeleteTimer(&pAsySdoSeqCon->m_EplTimerHdl);
758
759 // clean controllstructure
760 EPL_MEMSET(pAsySdoSeqCon, 0x00, sizeof(tEplAsySdoSeqCon));
761 pAsySdoSeqCon->m_SdoConHistory.m_bFreeEntries =
762 EPL_SDO_HISTORY_SIZE;
763 }
764
765 Exit:
766 return Ret;
767
768 }
769
770 //=========================================================================//
771 // //
772 // P R I V A T E F U N C T I O N S //
773 // //
774 //=========================================================================//
775
776 //---------------------------------------------------------------------------
777 //
778 // Function: EplEplSdoAsySeqProcess
779 //
780 // Description: intern function to process the asynchronus SDO Sequence Layer
781 // state maschine
782 //
783 //
784 //
785 // Parameters: uiHandle_p = index of the control structure of the connection
786 // uiDataSize_p = size of data frame to process (can be 0)
787 // -> without size of sequence header and Asnd header!!!
788 //
789 // pData_p = pointer to frame to send (can be NULL)
790 // pRecFrame_p = pointer to received frame (can be NULL)
791 // Event_p = Event to process
792 //
793 //
794 //
795 // Returns: tEplKernel = errorcode
796 //
797 //
798 // State:
799 //
800 //---------------------------------------------------------------------------
801 static tEplKernel EplSdoAsySeqProcess(unsigned int uiHandle_p,
802 unsigned int uiDataSize_p,
803 tEplFrame * pData_p,
804 tEplAsySdoSeq * pRecFrame_p,
805 tEplAsySdoSeqEvent Event_p)
806 {
807 tEplKernel Ret;
808 unsigned int uiFrameSize;
809 tEplFrame *pEplFrame;
810 tEplAsySdoSeqCon *pAsySdoSeqCon;
811 tEplSdoSeqConHdl SdoSeqConHdl;
812 unsigned int uiFreeEntries;
813
814 #if defined(WIN32) || defined(_WIN32)
815 // enter critical section for process function
816 EnterCriticalSection(AsySdoSequInstance_g.m_pCriticalSection);
817 #endif
818
819 Ret = kEplSuccessful;
820
821 // get handle for hinger layer
822 SdoSeqConHdl = uiHandle_p | EPL_SDO_ASY_HANDLE;
823
824 // check if handle invalid
825 if ((SdoSeqConHdl & ~EPL_SDO_SEQ_HANDLE_MASK) ==
826 EPL_SDO_SEQ_INVALID_HDL) {
827 Ret = kEplSdoSeqInvalidHdl;
828 goto Exit;
829 }
830 // get pointer to connection
831 pAsySdoSeqCon = &AsySdoSequInstance_g.m_AsySdoConnection[uiHandle_p];
832
833 // check size
834 if ((pData_p == NULL) && (pRecFrame_p == NULL) && (uiDataSize_p != 0)) {
835 Ret = kEplSdoSeqInvalidFrame;
836 goto Exit;
837 }
838 // check state
839 switch (pAsySdoSeqCon->m_SdoState) {
840 // idle state
841 case kEplAsySdoStateIdle:
842 {
843 // check event
844 switch (Event_p) {
845 // new connection
846 // -> send init frame and change to
847 // kEplAsySdoStateInit1
848 case kAsySdoSeqEventInitCon:
849 {
850 // set sending scon to 1
851 pAsySdoSeqCon->m_bRecSeqNum = 0x01;
852 // set set send rcon to 0
853 pAsySdoSeqCon->m_bSendSeqNum = 0x00;
854 Ret =
855 EplSdoAsySeqSendIntern
856 (pAsySdoSeqCon, 0, NULL, FALSE);
857 if (Ret != kEplSuccessful) {
858 goto Exit;
859 }
860 // change state
861 pAsySdoSeqCon->m_SdoState =
862 kEplAsySdoStateInit1;
863
864 // set timer
865 Ret =
866 EplSdoAsySeqSetTimer(pAsySdoSeqCon,
867 EPL_SEQ_DEFAULT_TIMEOUT);
868
869 break;
870 }
871
872 // init con from extern
873 // check rcon and scon
874 // -> send answer
875 case kAsySdoSeqEventFrameRec:
876 {
877 /*
878 PRINTF3("%s scon=%u rcon=%u\n",
879 __func__,
880 pRecFrame_p->m_le_bSendSeqNumCon,
881 pRecFrame_p->m_le_bRecSeqNumCon);
882 */
883 // check if scon == 1 and rcon == 0
884 if (((pRecFrame_p->
885 m_le_bRecSeqNumCon &
886 EPL_ASY_SDO_CON_MASK) == 0x00)
887 &&
888 ((pRecFrame_p->
889 m_le_bSendSeqNumCon &
890 EPL_ASY_SDO_CON_MASK) == 0x01)) {
891 // save sequence numbers
892 pAsySdoSeqCon->m_bRecSeqNum =
893 AmiGetByteFromLe
894 (&pRecFrame_p->
895 m_le_bRecSeqNumCon);
896 pAsySdoSeqCon->m_bSendSeqNum =
897 AmiGetByteFromLe
898 (&pRecFrame_p->
899 m_le_bSendSeqNumCon);
900 // create answer and send answer
901 // set rcon to 1 (in send direction own scon)
902 pAsySdoSeqCon->m_bRecSeqNum++;
903 Ret =
904 EplSdoAsySeqSendIntern
905 (pAsySdoSeqCon, 0, NULL,
906 FALSE);
907 if (Ret != kEplSuccessful) {
908 goto Exit;
909 }
910 // change state to kEplAsySdoStateInit2
911 pAsySdoSeqCon->m_SdoState =
912 kEplAsySdoStateInit2;
913
914 // set timer
915 Ret =
916 EplSdoAsySeqSetTimer
917 (pAsySdoSeqCon,
918 EPL_SEQ_DEFAULT_TIMEOUT);
919 } else { // error -> close
920 // delete timer
921 EplTimeruDeleteTimer
922 (&pAsySdoSeqCon->
923 m_EplTimerHdl);
924 if (((pRecFrame_p->
925 m_le_bRecSeqNumCon &
926 EPL_ASY_SDO_CON_MASK) !=
927 0x00)
928 || ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) != 0x00)) { // d.k. only answer with close message if the message sent was not a close message
929 // save sequence numbers
930 pAsySdoSeqCon->
931 m_bRecSeqNum =
932 AmiGetByteFromLe
933 (&pRecFrame_p->
934 m_le_bRecSeqNumCon);
935 pAsySdoSeqCon->
936 m_bSendSeqNum =
937 AmiGetByteFromLe
938 (&pRecFrame_p->
939 m_le_bSendSeqNumCon);
940 // set rcon and scon to 0
941 pAsySdoSeqCon->
942 m_bSendSeqNum &=
943 EPL_SEQ_NUM_MASK;
944 pAsySdoSeqCon->
945 m_bRecSeqNum &=
946 EPL_SEQ_NUM_MASK;
947 // send frame
948 EplSdoAsySeqSendIntern
949 (pAsySdoSeqCon, 0,
950 NULL, FALSE);
951 }
952 // call Command Layer Cb
953 AsySdoSequInstance_g.
954 m_fpSdoComConCb
955 (SdoSeqConHdl,
956 kAsySdoConStateInitError);
957 }
958 break;
959 }
960
961 default:
962 // d.k. do nothing
963 break;
964
965 } // end of switch(Event_p)
966 break;
967 }
968
969 // init connection step 1
970 // wait for frame with scon = 1
971 // and rcon = 1
972 case kEplAsySdoStateInit1:
973 {
974 // PRINTF0("EplSdoAsySequ: StateInit1\n");
975
976 // check event
977 switch (Event_p) {
978 // frame received
979 case kAsySdoSeqEventFrameRec:
980 {
981 // check scon == 1 and rcon == 1
982 if (((pRecFrame_p->
983 m_le_bRecSeqNumCon &
984 EPL_ASY_SDO_CON_MASK) == 0x01)
985 && ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) == 0x01)) { // create answer own scon = 2
986 // save sequence numbers
987 pAsySdoSeqCon->m_bRecSeqNum =
988 AmiGetByteFromLe
989 (&pRecFrame_p->
990 m_le_bRecSeqNumCon);
991 pAsySdoSeqCon->m_bSendSeqNum =
992 AmiGetByteFromLe
993 (&pRecFrame_p->
994 m_le_bSendSeqNumCon);
995
996 pAsySdoSeqCon->m_bRecSeqNum++;
997 Ret =
998 EplSdoAsySeqSendIntern
999 (pAsySdoSeqCon, 0, NULL,
1000 FALSE);
1001 if (Ret != kEplSuccessful) {
1002 goto Exit;
1003 }
1004 // change state to kEplAsySdoStateInit3
1005 pAsySdoSeqCon->m_SdoState =
1006 kEplAsySdoStateInit3;
1007
1008 // set timer
1009 Ret =
1010 EplSdoAsySeqSetTimer
1011 (pAsySdoSeqCon,
1012 EPL_SEQ_DEFAULT_TIMEOUT);
1013
1014 }
1015 // check if scon == 1 and rcon == 0, i.e. other side wants me to be server
1016 else if (((pRecFrame_p->
1017 m_le_bRecSeqNumCon &
1018 EPL_ASY_SDO_CON_MASK) ==
1019 0x00)
1020 &&
1021 ((pRecFrame_p->
1022 m_le_bSendSeqNumCon &
1023 EPL_ASY_SDO_CON_MASK) ==
1024 0x01)) {
1025 // save sequence numbers
1026 pAsySdoSeqCon->m_bRecSeqNum =
1027 AmiGetByteFromLe
1028 (&pRecFrame_p->
1029 m_le_bRecSeqNumCon);
1030 pAsySdoSeqCon->m_bSendSeqNum =
1031 AmiGetByteFromLe
1032 (&pRecFrame_p->
1033 m_le_bSendSeqNumCon);
1034 // create answer and send answer
1035 // set rcon to 1 (in send direction own scon)
1036 pAsySdoSeqCon->m_bRecSeqNum++;
1037 Ret =
1038 EplSdoAsySeqSendIntern
1039 (pAsySdoSeqCon, 0, NULL,
1040 FALSE);
1041 if (Ret != kEplSuccessful) {
1042 goto Exit;
1043 }
1044 // change state to kEplAsySdoStateInit2
1045 pAsySdoSeqCon->m_SdoState =
1046 kEplAsySdoStateInit2;
1047
1048 // set timer
1049 Ret =
1050 EplSdoAsySeqSetTimer
1051 (pAsySdoSeqCon,
1052 EPL_SEQ_DEFAULT_TIMEOUT);
1053 } else { // error -> Close
1054 pAsySdoSeqCon->m_SdoState =
1055 kEplAsySdoStateIdle;
1056 // delete timer
1057 EplTimeruDeleteTimer
1058 (&pAsySdoSeqCon->
1059 m_EplTimerHdl);
1060 if (((pRecFrame_p->
1061 m_le_bRecSeqNumCon &
1062 EPL_ASY_SDO_CON_MASK) !=
1063 0x00)
1064 || ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) != 0x00)) { // d.k. only answer with close message if the message sent was not a close message
1065 // save sequence numbers
1066 pAsySdoSeqCon->
1067 m_bRecSeqNum =
1068 AmiGetByteFromLe
1069 (&pRecFrame_p->
1070 m_le_bRecSeqNumCon);
1071 pAsySdoSeqCon->
1072 m_bSendSeqNum =
1073 AmiGetByteFromLe
1074 (&pRecFrame_p->
1075 m_le_bSendSeqNumCon);
1076
1077 // set rcon and scon to 0
1078 pAsySdoSeqCon->
1079 m_bSendSeqNum &=
1080 EPL_SEQ_NUM_MASK;
1081 pAsySdoSeqCon->
1082 m_bRecSeqNum &=
1083 EPL_SEQ_NUM_MASK;
1084 // send frame
1085 EplSdoAsySeqSendIntern
1086 (pAsySdoSeqCon, 0,
1087 NULL, FALSE);
1088 }
1089 // call Command Layer Cb
1090 AsySdoSequInstance_g.
1091 m_fpSdoComConCb
1092 (SdoSeqConHdl,
1093 kAsySdoConStateInitError);
1094 }
1095 break;
1096 }
1097
1098 // timeout
1099 case kAsySdoSeqEventTimeout:
1100 { // error -> Close
1101 pAsySdoSeqCon->m_SdoState =
1102 kEplAsySdoStateIdle;
1103
1104 // set rcon and scon to 0
1105 pAsySdoSeqCon->m_bSendSeqNum &=
1106 EPL_SEQ_NUM_MASK;
1107 pAsySdoSeqCon->m_bRecSeqNum &=
1108 EPL_SEQ_NUM_MASK;
1109 // send frame
1110 EplSdoAsySeqSendIntern(pAsySdoSeqCon,
1111 0, NULL, FALSE);
1112 // call Command Layer Cb
1113 AsySdoSequInstance_g.
1114 m_fpSdoComConCb(SdoSeqConHdl,
1115 kAsySdoConStateInitError);
1116 break;
1117 }
1118
1119 default:
1120 // d.k. do nothing
1121 break;
1122
1123 } // end of switch(Event_p)
1124 break;
1125 }
1126
1127 // init connection step 2
1128 case kEplAsySdoStateInit2:
1129 {
1130 // PRINTF0("EplSdoAsySequ: StateInit2\n");
1131
1132 // check event
1133 switch (Event_p) {
1134 // frame received
1135 case kAsySdoSeqEventFrameRec:
1136 {
1137 // check scon == 2 and rcon == 1
1138 if (((pRecFrame_p->
1139 m_le_bRecSeqNumCon &
1140 EPL_ASY_SDO_CON_MASK) == 0x01)
1141 && ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) == 0x02)) { // create answer own rcon = 2
1142 // save sequence numbers
1143 pAsySdoSeqCon->m_bRecSeqNum =
1144 AmiGetByteFromLe
1145 (&pRecFrame_p->
1146 m_le_bRecSeqNumCon);
1147 pAsySdoSeqCon->m_bSendSeqNum =
1148 AmiGetByteFromLe
1149 (&pRecFrame_p->
1150 m_le_bSendSeqNumCon);
1151
1152 pAsySdoSeqCon->m_bRecSeqNum++;
1153 Ret =
1154 EplSdoAsySeqSendIntern
1155 (pAsySdoSeqCon, 0, NULL,
1156 FALSE);
1157 if (Ret != kEplSuccessful) {
1158 goto Exit;
1159 }
1160 // change state to kEplAsySdoStateConnected
1161 pAsySdoSeqCon->m_SdoState =
1162 kEplAsySdoStateConnected;
1163
1164 // set timer
1165 Ret =
1166 EplSdoAsySeqSetTimer
1167 (pAsySdoSeqCon,
1168 EPL_SEQ_DEFAULT_TIMEOUT);
1169
1170 // call Command Layer Cb
1171 AsySdoSequInstance_g.
1172 m_fpSdoComConCb
1173 (SdoSeqConHdl,
1174 kAsySdoConStateConnected);
1175
1176 }
1177 // check scon == 1 and rcon == 1, i.e. other side wants me to initiate the connection
1178 else if (((pRecFrame_p->
1179 m_le_bRecSeqNumCon &
1180 EPL_ASY_SDO_CON_MASK) ==
1181 0x01)
1182 &&
1183 ((pRecFrame_p->
1184 m_le_bSendSeqNumCon &
1185 EPL_ASY_SDO_CON_MASK) ==
1186 0x01)) {
1187 // save sequence numbers
1188 pAsySdoSeqCon->m_bRecSeqNum =
1189 AmiGetByteFromLe
1190 (&pRecFrame_p->
1191 m_le_bRecSeqNumCon);
1192 pAsySdoSeqCon->m_bSendSeqNum =
1193 AmiGetByteFromLe
1194 (&pRecFrame_p->
1195 m_le_bSendSeqNumCon);
1196 // create answer and send answer
1197 // set rcon to 1 (in send direction own scon)
1198 pAsySdoSeqCon->m_bRecSeqNum++;
1199 Ret =
1200 EplSdoAsySeqSendIntern
1201 (pAsySdoSeqCon, 0, NULL,
1202 FALSE);
1203 if (Ret != kEplSuccessful) {
1204 goto Exit;
1205 }
1206 // set timer
1207 Ret =
1208 EplSdoAsySeqSetTimer
1209 (pAsySdoSeqCon,
1210 EPL_SEQ_DEFAULT_TIMEOUT);
1211 // change state to kEplAsySdoStateInit3
1212 pAsySdoSeqCon->m_SdoState =
1213 kEplAsySdoStateInit3;
1214
1215 } else { // error -> Close
1216 pAsySdoSeqCon->m_SdoState =
1217 kEplAsySdoStateIdle;
1218 // delete timer
1219 EplTimeruDeleteTimer
1220 (&pAsySdoSeqCon->
1221 m_EplTimerHdl);
1222 if (((pRecFrame_p->
1223 m_le_bRecSeqNumCon &
1224 EPL_ASY_SDO_CON_MASK) !=
1225 0x00)
1226 || ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) != 0x00)) { // d.k. only answer with close message if the message sent was not a close message
1227 // save sequence numbers
1228 pAsySdoSeqCon->
1229 m_bRecSeqNum =
1230 AmiGetByteFromLe
1231 (&pRecFrame_p->
1232 m_le_bRecSeqNumCon);
1233 pAsySdoSeqCon->
1234 m_bSendSeqNum =
1235 AmiGetByteFromLe
1236 (&pRecFrame_p->
1237 m_le_bSendSeqNumCon);
1238 // set rcon and scon to 0
1239 pAsySdoSeqCon->
1240 m_bSendSeqNum &=
1241 EPL_SEQ_NUM_MASK;
1242 pAsySdoSeqCon->
1243 m_bRecSeqNum &=
1244 EPL_SEQ_NUM_MASK;
1245 // send frame
1246 EplSdoAsySeqSendIntern
1247 (pAsySdoSeqCon, 0,
1248 NULL, FALSE);
1249 }
1250 // call Command Layer Cb
1251 AsySdoSequInstance_g.
1252 m_fpSdoComConCb
1253 (SdoSeqConHdl,
1254 kAsySdoConStateInitError);
1255 }
1256 break;
1257 }
1258
1259 // timeout
1260 case kAsySdoSeqEventTimeout:
1261 { // error -> Close
1262 pAsySdoSeqCon->m_SdoState =
1263 kEplAsySdoStateIdle;
1264 // set rcon and scon to 0
1265 pAsySdoSeqCon->m_bSendSeqNum &=
1266 EPL_SEQ_NUM_MASK;
1267 pAsySdoSeqCon->m_bRecSeqNum &=
1268 EPL_SEQ_NUM_MASK;
1269 // send frame
1270 EplSdoAsySeqSendIntern(pAsySdoSeqCon,
1271 0, NULL, FALSE);
1272
1273 // call Command Layer Cb
1274 AsySdoSequInstance_g.
1275 m_fpSdoComConCb(SdoSeqConHdl,
1276 kAsySdoConStateInitError);
1277 break;
1278 }
1279
1280 default:
1281 // d.k. do nothing
1282 break;
1283
1284 } // end of switch(Event_p)
1285 break;
1286 }
1287
1288 // init connection step 3
1289 case kEplAsySdoStateInit3:
1290 {
1291 // check event
1292 switch (Event_p) {
1293 // frame received
1294 case kAsySdoSeqEventFrameRec:
1295 {
1296 // check scon == 2 and rcon == 2
1297 if (((pRecFrame_p->
1298 m_le_bRecSeqNumCon &
1299 EPL_ASY_SDO_CON_MASK) == 0x02)
1300 &&
1301 ((pRecFrame_p->
1302 m_le_bSendSeqNumCon &
1303 EPL_ASY_SDO_CON_MASK) == 0x02)) {
1304 // save sequence numbers
1305 pAsySdoSeqCon->m_bRecSeqNum =
1306 AmiGetByteFromLe
1307 (&pRecFrame_p->
1308 m_le_bRecSeqNumCon);
1309 pAsySdoSeqCon->m_bSendSeqNum =
1310 AmiGetByteFromLe
1311 (&pRecFrame_p->
1312 m_le_bSendSeqNumCon);
1313 // change state to kEplAsySdoStateConnected
1314 pAsySdoSeqCon->m_SdoState =
1315 kEplAsySdoStateConnected;
1316
1317 // set timer
1318 Ret =
1319 EplSdoAsySeqSetTimer
1320 (pAsySdoSeqCon,
1321 EPL_SEQ_DEFAULT_TIMEOUT);
1322 // call Command Layer Cb
1323 AsySdoSequInstance_g.
1324 m_fpSdoComConCb
1325 (SdoSeqConHdl,
1326 kAsySdoConStateConnected);
1327
1328 }
1329 // check scon == 2 and rcon == 1
1330 else if (((pRecFrame_p->
1331 m_le_bRecSeqNumCon &
1332 EPL_ASY_SDO_CON_MASK) ==
1333 0x01)
1334 && ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) == 0x02)) { // create answer own rcon = 2
1335 // save sequence numbers
1336 pAsySdoSeqCon->m_bRecSeqNum =
1337 AmiGetByteFromLe
1338 (&pRecFrame_p->
1339 m_le_bRecSeqNumCon);
1340 pAsySdoSeqCon->m_bSendSeqNum =
1341 AmiGetByteFromLe
1342 (&pRecFrame_p->
1343 m_le_bSendSeqNumCon);
1344
1345 pAsySdoSeqCon->m_bRecSeqNum++;
1346 Ret =
1347 EplSdoAsySeqSendIntern
1348 (pAsySdoSeqCon, 0, NULL,
1349 FALSE);
1350 if (Ret != kEplSuccessful) {
1351 goto Exit;
1352 }
1353 // change state to kEplAsySdoStateConnected
1354 pAsySdoSeqCon->m_SdoState =
1355 kEplAsySdoStateConnected;
1356
1357 // set timer
1358 Ret =
1359 EplSdoAsySeqSetTimer
1360 (pAsySdoSeqCon,
1361 EPL_SEQ_DEFAULT_TIMEOUT);
1362
1363 // call Command Layer Cb
1364 AsySdoSequInstance_g.
1365 m_fpSdoComConCb
1366 (SdoSeqConHdl,
1367 kAsySdoConStateConnected);
1368
1369 } else { // error -> Close
1370 pAsySdoSeqCon->m_SdoState =
1371 kEplAsySdoStateIdle;
1372 // delete timer
1373 EplTimeruDeleteTimer
1374 (&pAsySdoSeqCon->
1375 m_EplTimerHdl);
1376 if (((pRecFrame_p->
1377 m_le_bRecSeqNumCon &
1378 EPL_ASY_SDO_CON_MASK) !=
1379 0x00)
1380 || ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) != 0x00)) { // d.k. only answer with close message if the message sent was not a close message
1381 // save sequence numbers
1382 pAsySdoSeqCon->
1383 m_bRecSeqNum =
1384 AmiGetByteFromLe
1385 (&pRecFrame_p->
1386 m_le_bRecSeqNumCon);
1387 pAsySdoSeqCon->
1388 m_bSendSeqNum =
1389 AmiGetByteFromLe
1390 (&pRecFrame_p->
1391 m_le_bSendSeqNumCon);
1392 // set rcon and scon to 0
1393 pAsySdoSeqCon->
1394 m_bSendSeqNum &=
1395 EPL_SEQ_NUM_MASK;
1396 pAsySdoSeqCon->
1397 m_bRecSeqNum &=
1398 EPL_SEQ_NUM_MASK;
1399 // send frame
1400 EplSdoAsySeqSendIntern
1401 (pAsySdoSeqCon, 0,
1402 NULL, FALSE);
1403 }
1404 // call Command Layer Cb
1405 AsySdoSequInstance_g.
1406 m_fpSdoComConCb
1407 (SdoSeqConHdl,
1408 kAsySdoConStateInitError);
1409 }
1410 break;
1411 }
1412
1413 // timeout
1414 case kAsySdoSeqEventTimeout:
1415 { // error -> Close
1416 pAsySdoSeqCon->m_SdoState =
1417 kEplAsySdoStateIdle;
1418 // set rcon and scon to 0
1419 pAsySdoSeqCon->m_bSendSeqNum &=
1420 EPL_SEQ_NUM_MASK;
1421 pAsySdoSeqCon->m_bRecSeqNum &=
1422 EPL_SEQ_NUM_MASK;
1423 // send frame
1424 EplSdoAsySeqSendIntern(pAsySdoSeqCon,
1425 0, NULL, FALSE);
1426
1427 // call Command Layer Cb
1428 AsySdoSequInstance_g.
1429 m_fpSdoComConCb(SdoSeqConHdl,
1430 kAsySdoConStateInitError);
1431 break;
1432 }
1433
1434 default:
1435 // d.k. do nothing
1436 break;
1437
1438 } // end of switch(Event_p)
1439 break;
1440 }
1441
1442 // connection established
1443 case kEplAsySdoStateConnected:
1444 {
1445 // check event
1446 switch (Event_p) {
1447
1448 // frame to send
1449 case kAsySdoSeqEventFrameSend:
1450 {
1451 // set timer
1452 Ret =
1453 EplSdoAsySeqSetTimer(pAsySdoSeqCon,
1454 EPL_SEQ_DEFAULT_TIMEOUT);
1455 // check if data frame or ack
1456 if (pData_p == NULL) { // send ack
1457 // inc scon
1458 //pAsySdoSeqCon->m_bRecSeqNum += 4;
1459 Ret =
1460 EplSdoAsySeqSendIntern
1461 (pAsySdoSeqCon, 0, NULL,
1462 FALSE);
1463 if (Ret != kEplSuccessful) {
1464 goto Exit;
1465 }
1466 } else { // send dataframe
1467 // increment send sequence number
1468 pAsySdoSeqCon->m_bRecSeqNum +=
1469 4;
1470 Ret =
1471 EplSdoAsySeqSendIntern
1472 (pAsySdoSeqCon,
1473 uiDataSize_p, pData_p,
1474 TRUE);
1475 if (Ret == kEplSdoSeqRequestAckNeeded) { // request ack
1476 // change state to wait ack
1477 pAsySdoSeqCon->
1478 m_SdoState =
1479 kEplAsySdoStateWaitAck;
1480 // set Ret to kEplSuccessful, because no error
1481 // for higher layer
1482 Ret = kEplSuccessful;
1483
1484 } else if (Ret !=
1485 kEplSuccessful) {
1486 goto Exit;
1487 } else {
1488 // call Command Layer Cb
1489 AsySdoSequInstance_g.
1490 m_fpSdoComConCb
1491 (SdoSeqConHdl,
1492 kAsySdoConStateFrameSended);
1493 }
1494 }
1495 break;
1496 } // end of case kAsySdoSeqEventFrameSend
1497
1498 // frame received
1499 case kAsySdoSeqEventFrameRec:
1500 {
1501 BYTE bSendSeqNumCon =
1502 AmiGetByteFromLe(&pRecFrame_p->
1503 m_le_bSendSeqNumCon);
1504
1505 // set timer
1506 Ret =
1507 EplSdoAsySeqSetTimer(pAsySdoSeqCon,
1508 EPL_SEQ_DEFAULT_TIMEOUT);
1509 // check scon
1510 switch (bSendSeqNumCon &
1511 EPL_ASY_SDO_CON_MASK) {
1512 // close from other node
1513 case 0:
1514 case 1:
1515 {
1516 // return to idle
1517 pAsySdoSeqCon->
1518 m_SdoState =
1519 kEplAsySdoStateIdle;
1520 // delete timer
1521 EplTimeruDeleteTimer
1522 (&pAsySdoSeqCon->
1523 m_EplTimerHdl);
1524 // call Command Layer Cb
1525 AsySdoSequInstance_g.
1526 m_fpSdoComConCb
1527 (SdoSeqConHdl,
1528 kAsySdoConStateConClosed);
1529
1530 break;
1531 }
1532
1533 // Request Ack or Error Ack
1534 // possible contain data
1535 case 3:
1536 // normal frame
1537 case 2:
1538 {
1539 if ((AmiGetByteFromLe
1540 (&pRecFrame_p->
1541 m_le_bRecSeqNumCon)
1542 &
1543 EPL_ASY_SDO_CON_MASK)
1544 == 3) {
1545 // PRINTF0("EplSdoAsySequ: error response received\n");
1546
1547 // error response (retransmission request)
1548 // resend frames from history
1549
1550 // read frame from history
1551 Ret =
1552 EplSdoAsyReadFromHistory
1553 (pAsySdoSeqCon,
1554 &pEplFrame,
1555 &uiFrameSize,
1556 TRUE);
1557 if (Ret !=
1558 kEplSuccessful)
1559 {
1560 goto Exit;
1561 }
1562
1563 while ((pEplFrame != NULL)
1564 &&
1565 (uiFrameSize
1566 != 0)) {
1567 // send frame
1568 Ret =
1569 EplSdoAsySeqSendLowerLayer
1570 (pAsySdoSeqCon,
1571 uiFrameSize,
1572 pEplFrame);
1573 if (Ret
1574 !=
1575 kEplSuccessful)
1576 {
1577 goto Exit;
1578 }
1579 // read next frame from history
1580 Ret =
1581 EplSdoAsyReadFromHistory
1582 (pAsySdoSeqCon,
1583 &pEplFrame,
1584 &uiFrameSize,
1585 FALSE);
1586 if (Ret
1587 !=
1588 kEplSuccessful)
1589 {
1590 goto Exit;
1591 }
1592 } // end of while((pabFrame != NULL)
1593 } // end of if (error response)
1594
1595 if (((pAsySdoSeqCon->m_bSendSeqNum + 4) & EPL_SEQ_NUM_MASK) == (bSendSeqNumCon & EPL_SEQ_NUM_MASK)) { // next frame of sequence received
1596 // save send sequence number (without ack request)
1597 pAsySdoSeqCon->
1598 m_bSendSeqNum
1599 =
1600 bSendSeqNumCon
1601 & ~0x01;
1602
1603 // check if ack or data-frame
1604 //ignore ack -> already processed
1605 if (uiDataSize_p
1606 >
1607 EPL_SEQ_HEADER_SIZE)
1608 {
1609 AsySdoSequInstance_g.
1610 m_fpSdoComReceiveCb
1611 (SdoSeqConHdl,
1612 ((tEplAsySdoCom *) & pRecFrame_p->m_le_abSdoSeqPayload), (uiDataSize_p - EPL_SEQ_HEADER_SIZE));
1613 // call Command Layer Cb
1614 AsySdoSequInstance_g.
1615 m_fpSdoComConCb
1616 (SdoSeqConHdl,
1617 kAsySdoConStateFrameSended);
1618
1619 } else {
1620 // call Command Layer Cb
1621 AsySdoSequInstance_g.
1622 m_fpSdoComConCb
1623 (SdoSeqConHdl,
1624 kAsySdoConStateAckReceived);
1625 }
1626 } else if (((bSendSeqNumCon - pAsySdoSeqCon->m_bSendSeqNum - 4) & EPL_SEQ_NUM_MASK) < EPL_SEQ_NUM_THRESHOLD) { // frame of sequence was lost,
1627 // because difference of received and old value
1628 // is less then halve of the values range.
1629
1630 // send error frame with own rcon = 3
1631 pAsySdoSeqCon->
1632 m_bSendSeqNum
1633 |= 0x03;
1634 Ret =
1635 EplSdoAsySeqSendIntern
1636 (pAsySdoSeqCon,
1637 0, NULL,
1638 FALSE);
1639 // restore send sequence number
1640 pAsySdoSeqCon->
1641 m_bSendSeqNum
1642 =
1643 (pAsySdoSeqCon->
1644 m_bSendSeqNum
1645 &
1646 EPL_SEQ_NUM_MASK)
1647 | 0x02;
1648 if (Ret !=
1649 kEplSuccessful)
1650 {
1651 goto Exit;
1652 }
1653 // break here, because a requested acknowledge
1654 // was sent implicitly above
1655 break;
1656 }
1657 // else, ignore repeated frame
1658
1659 if ((bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) == 3) { // ack request received
1660
1661 // create ack with own scon = 2
1662 Ret =
1663 EplSdoAsySeqSendIntern
1664 (pAsySdoSeqCon,
1665 0, NULL,
1666 FALSE);
1667 if (Ret !=
1668 kEplSuccessful)
1669 {
1670 goto Exit;
1671 }
1672 }
1673
1674 break;
1675 }
1676
1677 } // switch(pAsySdoSeqCon->m_bSendSeqNum & EPL_ASY_SDO_CON_MASK)
1678 break;
1679 } // end of case kAsySdoSeqEventFrameRec:
1680
1681 //close event from higher layer
1682 case kAsySdoSeqEventCloseCon:
1683 {
1684 pAsySdoSeqCon->m_SdoState =
1685 kEplAsySdoStateIdle;
1686 // set rcon and scon to 0
1687 pAsySdoSeqCon->m_bSendSeqNum &=
1688 EPL_SEQ_NUM_MASK;
1689 pAsySdoSeqCon->m_bRecSeqNum &=
1690 EPL_SEQ_NUM_MASK;
1691 // send frame
1692 EplSdoAsySeqSendIntern(pAsySdoSeqCon,
1693 0, NULL, FALSE);
1694
1695 // delete timer
1696 EplTimeruDeleteTimer(&pAsySdoSeqCon->
1697 m_EplTimerHdl);
1698 // call Command Layer Cb is not necessary, because the event came from there
1699 // AsySdoSequInstance_g.m_fpSdoComConCb(SdoSeqConHdl,
1700 // kAsySdoConStateInitError);
1701 break;
1702 }
1703
1704 // timeout
1705 case kAsySdoSeqEventTimeout:
1706 {
1707
1708 uiFreeEntries =
1709 EplSdoAsyGetFreeEntriesFromHistory
1710 (pAsySdoSeqCon);
1711 if ((uiFreeEntries <
1712 EPL_SDO_HISTORY_SIZE)
1713 && (pAsySdoSeqCon->m_uiRetryCount < EPL_SEQ_RETRY_COUNT)) { // unacknowlegded frames in history
1714 // and retry counter not exceeded
1715
1716 // resend data with acknowledge request
1717
1718 // increment retry counter
1719 pAsySdoSeqCon->m_uiRetryCount++;
1720
1721 // set timer
1722 Ret =
1723 EplSdoAsySeqSetTimer
1724 (pAsySdoSeqCon,
1725 EPL_SEQ_DEFAULT_TIMEOUT);
1726
1727 // read first frame from history
1728 Ret =
1729 EplSdoAsyReadFromHistory
1730 (pAsySdoSeqCon, &pEplFrame,
1731 &uiFrameSize, TRUE);
1732 if (Ret != kEplSuccessful) {
1733 goto Exit;
1734 }
1735
1736 if ((pEplFrame != NULL)
1737 && (uiFrameSize != 0)) {
1738
1739 // set ack request in scon
1740 AmiSetByteToLe
1741 (&pEplFrame->m_Data.
1742 m_Asnd.m_Payload.
1743 m_SdoSequenceFrame.
1744 m_le_bSendSeqNumCon,
1745 AmiGetByteFromLe
1746 (&pEplFrame->
1747 m_Data.m_Asnd.
1748 m_Payload.
1749 m_SdoSequenceFrame.
1750 m_le_bSendSeqNumCon)
1751 | 0x03);
1752
1753 // send frame
1754 Ret =
1755 EplSdoAsySeqSendLowerLayer
1756 (pAsySdoSeqCon,
1757 uiFrameSize,
1758 pEplFrame);
1759 if (Ret !=
1760 kEplSuccessful) {
1761 goto Exit;
1762 }
1763
1764 }
1765 } else {
1766 // timeout, because of no traffic -> Close
1767 pAsySdoSeqCon->m_SdoState =
1768 kEplAsySdoStateIdle;
1769 // set rcon and scon to 0
1770 pAsySdoSeqCon->m_bSendSeqNum &=
1771 EPL_SEQ_NUM_MASK;
1772 pAsySdoSeqCon->m_bRecSeqNum &=
1773 EPL_SEQ_NUM_MASK;
1774 // send frame
1775 EplSdoAsySeqSendIntern
1776 (pAsySdoSeqCon, 0, NULL,
1777 FALSE);
1778
1779 // call Command Layer Cb
1780 AsySdoSequInstance_g.
1781 m_fpSdoComConCb
1782 (SdoSeqConHdl,
1783 kAsySdoConStateTimeout);
1784 }
1785
1786 break;
1787 }
1788
1789 default:
1790 // d.k. do nothing
1791 break;
1792
1793 } // end of switch(Event_p)
1794 break;
1795 }
1796
1797 // wait for Acknowledge (history buffer full)
1798 case kEplAsySdoStateWaitAck:
1799 {
1800 PRINTF0("EplSdoAsySequ: StateWaitAck\n");
1801
1802 // set timer
1803 Ret = EplSdoAsySeqSetTimer(pAsySdoSeqCon,
1804 EPL_SEQ_DEFAULT_TIMEOUT);
1805
1806 //TODO: retry of acknowledge
1807 if (Event_p == kAsySdoSeqEventFrameRec) {
1808 // check rcon
1809 switch (pRecFrame_p->
1810 m_le_bRecSeqNumCon &
1811 EPL_ASY_SDO_CON_MASK) {
1812 // close-frome other node
1813 case 0:
1814 {
1815 // return to idle
1816 pAsySdoSeqCon->m_SdoState =
1817 kEplAsySdoStateIdle;
1818 // delete timer
1819 EplTimeruDeleteTimer
1820 (&pAsySdoSeqCon->
1821 m_EplTimerHdl);
1822 // call Command Layer Cb
1823 AsySdoSequInstance_g.
1824 m_fpSdoComConCb
1825 (SdoSeqConHdl,
1826 kAsySdoConStateConClosed);
1827
1828 break;
1829 }
1830
1831 // normal frame
1832 case 2:
1833 {
1834 // should be ack
1835 // -> change to state kEplAsySdoStateConnected
1836 pAsySdoSeqCon->m_SdoState =
1837 kEplAsySdoStateConnected;
1838 // call Command Layer Cb
1839 AsySdoSequInstance_g.
1840 m_fpSdoComConCb
1841 (SdoSeqConHdl,
1842 kAsySdoConStateAckReceived);
1843 // send data to higher layer if needed
1844 if (uiDataSize_p >
1845 EPL_SEQ_HEADER_SIZE) {
1846 AsySdoSequInstance_g.
1847 m_fpSdoComReceiveCb
1848 (SdoSeqConHdl,
1849 ((tEplAsySdoCom *)
1850 & pRecFrame_p->
1851 m_le_abSdoSeqPayload),
1852 (uiDataSize_p -
1853 EPL_SEQ_HEADER_SIZE));
1854 }
1855 break;
1856 }
1857
1858 // Request Ack or Error Ack
1859 case 3:
1860 {
1861 // -> change to state kEplAsySdoStateConnected
1862 pAsySdoSeqCon->m_SdoState =
1863 kEplAsySdoStateConnected;
1864
1865 if (pRecFrame_p->m_le_bRecSeqNumCon == pAsySdoSeqCon->m_bRecSeqNum) { // ack request
1866 // -> send ack
1867 // save sequence numbers
1868 pAsySdoSeqCon->
1869 m_bRecSeqNum =
1870 AmiGetByteFromLe
1871 (&pRecFrame_p->
1872 m_le_bRecSeqNumCon);
1873 pAsySdoSeqCon->
1874 m_bSendSeqNum =
1875 AmiGetByteFromLe
1876 (&pRecFrame_p->
1877 m_le_bSendSeqNumCon);
1878
1879 // create answer own rcon = 2
1880 pAsySdoSeqCon->
1881 m_bRecSeqNum--;
1882
1883 // check if ack or data-frame
1884 if (uiDataSize_p >
1885 EPL_SEQ_HEADER_SIZE)
1886 {
1887 AsySdoSequInstance_g.
1888 m_fpSdoComReceiveCb
1889 (SdoSeqConHdl,
1890 ((tEplAsySdoCom *) & pRecFrame_p->m_le_abSdoSeqPayload), (uiDataSize_p - EPL_SEQ_HEADER_SIZE));
1891 // call Command Layer Cb
1892 AsySdoSequInstance_g.
1893 m_fpSdoComConCb
1894 (SdoSeqConHdl,
1895 kAsySdoConStateFrameSended);
1896
1897 } else {
1898 Ret =
1899 EplSdoAsySeqSendIntern
1900 (pAsySdoSeqCon,
1901 0, NULL,
1902 FALSE);
1903 if (Ret !=
1904 kEplSuccessful)
1905 {
1906 goto Exit;
1907 }
1908 }
1909
1910 } else {
1911 // error ack
1912 // resend frames from history
1913
1914 // read frame from history
1915 Ret =
1916 EplSdoAsyReadFromHistory
1917 (pAsySdoSeqCon,
1918 &pEplFrame,
1919 &uiFrameSize,
1920 TRUE);
1921 while ((pEplFrame !=
1922 NULL)
1923 && (uiFrameSize
1924 != 0)) {
1925 // send frame
1926 Ret =
1927 EplSdoAsySeqSendLowerLayer
1928 (pAsySdoSeqCon,
1929 uiFrameSize,
1930 pEplFrame);
1931 if (Ret !=
1932 kEplSuccessful)
1933 {
1934 goto Exit;
1935 }
1936 // read next frame
1937
1938 // read frame from history
1939 Ret =
1940 EplSdoAsyReadFromHistory
1941 (pAsySdoSeqCon,
1942 &pEplFrame,
1943 &uiFrameSize,
1944 FALSE);
1945 } // end of while((pabFrame != NULL)
1946 }
1947 break;
1948 }
1949 } // end of switch(pRecFrame_p->m_le_bRecSeqNumCon & EPL_ASY_SDO_CON_MASK)
1950
1951 } else if (Event_p == kAsySdoSeqEventTimeout) { // error -> Close
1952 pAsySdoSeqCon->m_SdoState = kEplAsySdoStateIdle;
1953 // set rcon and scon to 0
1954 pAsySdoSeqCon->m_bSendSeqNum &=
1955 EPL_SEQ_NUM_MASK;
1956 pAsySdoSeqCon->m_bRecSeqNum &= EPL_SEQ_NUM_MASK;
1957 // send frame
1958 EplSdoAsySeqSendIntern(pAsySdoSeqCon,
1959 0, NULL, FALSE);
1960
1961 // call Command Layer Cb
1962 AsySdoSequInstance_g.
1963 m_fpSdoComConCb(SdoSeqConHdl,
1964 kAsySdoConStateTimeout);
1965 }
1966
1967 break;
1968 }
1969
1970 // unknown state
1971 default:
1972 {
1973 EPL_DBGLVL_SDO_TRACE0
1974 ("Error: Unknown State in EplSdoAsySeqProcess\n");
1975
1976 }
1977 } // end of switch(pAsySdoSeqCon->m_SdoState)
1978
1979 Exit:
1980
1981 #if defined(WIN32) || defined(_WIN32)
1982 // leave critical section for process function
1983 LeaveCriticalSection(AsySdoSequInstance_g.m_pCriticalSection);
1984 #endif
1985 return Ret;
1986
1987 }
1988
1989 //---------------------------------------------------------------------------
1990 //
1991 // Function: EplSdoAsySeqSendIntern
1992 //
1993 // Description: intern function to create and send a frame
1994 // -> if uiDataSize_p == 0 create a frame with infos from
1995 // pAsySdoSeqCon_p
1996 //
1997 //
1998 //
1999 // Parameters: pAsySdoSeqCon_p = pointer to control structure of the connection
2000 // uiDataSize_p = size of data frame to process (can be 0)
2001 // -> without size of sequence header and Asnd header!!!
2002 // pData_p = pointer to frame to process (can be NULL)
2003 // fFrameInHistory = if TRUE frame is saved to history else not
2004 //
2005 //
2006 //
2007 // Returns: tEplKernel = errorcode
2008 //
2009 //
2010 // State:
2011 //
2012 //---------------------------------------------------------------------------
2013 static tEplKernel EplSdoAsySeqSendIntern(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
2014 unsigned int uiDataSize_p,
2015 tEplFrame * pData_p,
2016 BOOL fFrameInHistory_p)
2017 {
2018 tEplKernel Ret;
2019 BYTE abFrame[EPL_SEQ_FRAME_SIZE];
2020 tEplFrame *pEplFrame;
2021 unsigned int uiFreeEntries;
2022
2023 if (pData_p == NULL) { // set pointer to own frame
2024 EPL_MEMSET(&abFrame[0], 0x00, sizeof(abFrame));
2025 pEplFrame = (tEplFrame *) & abFrame[0];
2026 } else { // set pointer to frame from calling function
2027 pEplFrame = pData_p;
2028 }
2029
2030 if (fFrameInHistory_p != FALSE) {
2031 // check if only one free entry in history buffer
2032 uiFreeEntries =
2033 EplSdoAsyGetFreeEntriesFromHistory(pAsySdoSeqCon_p);
2034 if (uiFreeEntries == 1) { // request an acknowledge in dataframe
2035 // own scon = 3
2036 pAsySdoSeqCon_p->m_bRecSeqNum |= 0x03;
2037 }
2038 }
2039 // fillin header informations
2040 // set service id sdo
2041 AmiSetByteToLe(&pEplFrame->m_Data.m_Asnd.m_le_bServiceId, 0x05);
2042 AmiSetByteToLe(&pEplFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame.
2043 m_le_abReserved, 0x00);
2044 // set receive sequence number and rcon
2045 AmiSetByteToLe(&pEplFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame.
2046 m_le_bRecSeqNumCon, pAsySdoSeqCon_p->m_bSendSeqNum);
2047 // set send sequence number and scon
2048 AmiSetByteToLe(&pEplFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame.
2049 m_le_bSendSeqNumCon, pAsySdoSeqCon_p->m_bRecSeqNum);
2050
2051 // add size
2052 uiDataSize_p += EPL_SEQ_HEADER_SIZE;
2053
2054 // forward frame to appropriate lower layer
2055 Ret = EplSdoAsySeqSendLowerLayer(pAsySdoSeqCon_p, uiDataSize_p, pEplFrame); // pointer to frame
2056
2057 // check if all allright
2058 if ((Ret == kEplSuccessful)
2059 && (fFrameInHistory_p != FALSE)) {
2060 // set own scon to 2 if needed
2061 if ((pAsySdoSeqCon_p->m_bRecSeqNum & 0x03) == 0x03) {
2062 pAsySdoSeqCon_p->m_bRecSeqNum--;
2063 }
2064 // save frame to history
2065 Ret = EplSdoAsyAddFrameToHistory(pAsySdoSeqCon_p,
2066 pEplFrame, uiDataSize_p);
2067 if (Ret == kEplSdoSeqNoFreeHistory) { // request Ack needed
2068 Ret = kEplSdoSeqRequestAckNeeded;
2069 }
2070
2071 }
2072
2073 return Ret;
2074 }
2075
2076 //---------------------------------------------------------------------------
2077 //
2078 // Function: EplSdoAsySeqSendLowerLayer
2079 //
2080 // Description: intern function to send a previously created frame to lower layer
2081 //
2082 // Parameters: pAsySdoSeqCon_p = pointer to control structure of the connection
2083 // uiDataSize_p = size of data frame to process (can be 0)
2084 // -> without size of Asnd header!!!
2085 // pData_p = pointer to frame to process (can be NULL)
2086 //
2087 // Returns: tEplKernel = errorcode
2088 //
2089 //
2090 // State:
2091 //
2092 //---------------------------------------------------------------------------
2093 static tEplKernel EplSdoAsySeqSendLowerLayer(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
2094 unsigned int uiDataSize_p,
2095 tEplFrame * pEplFrame_p)
2096 {
2097 tEplKernel Ret;
2098
2099 // call send-function
2100 // check handle for UDP or Asnd
2101 if ((pAsySdoSeqCon_p->m_ConHandle & EPL_SDO_ASY_HANDLE_MASK) == EPL_SDO_UDP_HANDLE) { // send over UDP
2102 #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
2103 Ret = EplSdoUdpuSendData(pAsySdoSeqCon_p->m_ConHandle, pEplFrame_p, // pointer to frame
2104 uiDataSize_p);
2105 #else
2106 Ret = kEplSdoSeqUnsupportedProt;
2107 #endif
2108
2109 } else if ((pAsySdoSeqCon_p->m_ConHandle & EPL_SDO_ASY_HANDLE_MASK) == EPL_SDO_ASND_HANDLE) { // ASND
2110 #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
2111 Ret = EplSdoAsnduSendData(pAsySdoSeqCon_p->m_ConHandle, pEplFrame_p, // pointer to frame
2112 uiDataSize_p);
2113 #else
2114 Ret = kEplSdoSeqUnsupportedProt;
2115 #endif
2116 } else { // error
2117 Ret = kEplSdoSeqInvalidHdl;
2118 }
2119
2120 return Ret;
2121 }
2122
2123 //---------------------------------------------------------------------------
2124 //
2125 // Function: EplSdoAsyReceiveCb
2126 //
2127 // Description: callback-function for received frames from lower layer
2128 //
2129 //
2130 //
2131 // Parameters: ConHdl_p = handle of the connection
2132 // pSdoSeqData_p = pointer to frame
2133 // uiDataSize_p = size of frame
2134 //
2135 //
2136 // Returns: tEplKernel = errorcode
2137 //
2138 //
2139 // State:
2140 //
2141 //---------------------------------------------------------------------------
2142 tEplKernel PUBLIC EplSdoAsyReceiveCb(tEplSdoConHdl ConHdl_p,
2143 tEplAsySdoSeq * pSdoSeqData_p,
2144 unsigned int uiDataSize_p)
2145 {
2146 tEplKernel Ret;
2147 unsigned int uiCount = 0;
2148 unsigned int uiFreeEntry = EPL_MAX_SDO_SEQ_CON;
2149 tEplAsySdoSeqCon *pAsySdoSeqCon;
2150
2151 #if defined(WIN32) || defined(_WIN32)
2152 // enter critical section
2153 EnterCriticalSection(AsySdoSequInstance_g.m_pCriticalSectionReceive);
2154 #endif
2155
2156 EPL_DBGLVL_SDO_TRACE2("Handle: 0x%x , First Databyte 0x%x\n", ConHdl_p,
2157 ((BYTE *) pSdoSeqData_p)[0]);
2158
2159 // search controll structure for this connection
2160 pAsySdoSeqCon = &AsySdoSequInstance_g.m_AsySdoConnection[uiCount];
2161 while (uiCount < EPL_MAX_SDO_SEQ_CON) {
2162 if (pAsySdoSeqCon->m_ConHandle == ConHdl_p) {
2163 break;
2164 } else if ((pAsySdoSeqCon->m_ConHandle == 0)
2165 && (uiFreeEntry == EPL_MAX_SDO_SEQ_CON)) {
2166 // free entry
2167 uiFreeEntry = uiCount;
2168 }
2169 uiCount++;
2170 pAsySdoSeqCon++;
2171 }
2172
2173 if (uiCount == EPL_MAX_SDO_SEQ_CON) { // new connection
2174 if (uiFreeEntry == EPL_MAX_SDO_SEQ_CON) {
2175 Ret = kEplSdoSeqNoFreeHandle;
2176 goto Exit;
2177 } else {
2178 pAsySdoSeqCon =
2179 &AsySdoSequInstance_g.
2180 m_AsySdoConnection[uiFreeEntry];
2181 // save handle from lower layer
2182 pAsySdoSeqCon->m_ConHandle = ConHdl_p;
2183 // increment use counter
2184 pAsySdoSeqCon->m_uiUseCount++;
2185 uiCount = uiFreeEntry;
2186 }
2187 }
2188 // call history ack function
2189 Ret = EplSdoAsyAckFrameToHistory(pAsySdoSeqCon,
2190 (AmiGetByteFromLe
2191 (&pSdoSeqData_p->
2192 m_le_bRecSeqNumCon) &
2193 EPL_SEQ_NUM_MASK));
2194 if (Ret != kEplSuccessful) {
2195 goto Exit;
2196 }
2197 #if defined(WIN32) || defined(_WIN32)
2198 // leave critical section
2199 LeaveCriticalSection(AsySdoSequInstance_g.m_pCriticalSectionReceive);
2200 #endif
2201
2202 // call process function with pointer of frame and event kAsySdoSeqEventFrameRec
2203 Ret = EplSdoAsySeqProcess(uiCount,
2204 uiDataSize_p,
2205 NULL, pSdoSeqData_p, kAsySdoSeqEventFrameRec);
2206
2207 Exit:
2208 return Ret;
2209 }
2210
2211 //---------------------------------------------------------------------------
2212 //
2213 // Function: EplSdoAsyInitHistory
2214 //
2215 // Description: inti function for history buffer
2216 //
2217 //
2218 //
2219 // Parameters:
2220 //
2221 //
2222 // Returns: tEplKernel = errorcode
2223 //
2224 //
2225 // State:
2226 //
2227 //---------------------------------------------------------------------------
2228 static tEplKernel EplSdoAsyInitHistory(void)
2229 {
2230 tEplKernel Ret;
2231 unsigned int uiCount;
2232
2233 Ret = kEplSuccessful;
2234 // init m_bFreeEntries in history-buffer
2235 for (uiCount = 0; uiCount < EPL_MAX_SDO_SEQ_CON; uiCount++) {
2236 AsySdoSequInstance_g.m_AsySdoConnection[uiCount].
2237 m_SdoConHistory.m_bFreeEntries = EPL_SDO_HISTORY_SIZE;
2238 }
2239
2240 return Ret;
2241 }
2242
2243 //---------------------------------------------------------------------------
2244 //
2245 // Function: EplSdoAsyAddFrameToHistory
2246 //
2247 // Description: function to add a frame to the history buffer
2248 //
2249 //
2250 //
2251 // Parameters: pAsySdoSeqCon_p = pointer to control structure of this connection
2252 // pFrame_p = pointer to frame
2253 // uiSize_p = size of the frame
2254 // -> without size of the ethernet header
2255 // and the asnd header
2256 //
2257 // Returns: tEplKernel = errorcode
2258 //
2259 //
2260 // State:
2261 //
2262 //---------------------------------------------------------------------------
2263 static tEplKernel EplSdoAsyAddFrameToHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
2264 tEplFrame * pFrame_p,
2265 unsigned int uiSize_p)
2266 {
2267 tEplKernel Ret;
2268 tEplAsySdoConHistory *pHistory;
2269
2270 Ret = kEplSuccessful;
2271
2272 // add frame to history buffer
2273
2274 // check size
2275 // $$$ d.k. EPL_SEQ_HISTORY_FRAME_SIZE includes the header size, but uiSize_p does not!!!
2276 if (uiSize_p > EPL_SEQ_HISTROY_FRAME_SIZE) {
2277 Ret = kEplSdoSeqFrameSizeError;
2278 goto Exit;
2279 }
2280 // save pointer to history
2281 pHistory = &pAsySdoSeqCon_p->m_SdoConHistory;
2282
2283 // check if a free entry is available
2284 if (pHistory->m_bFreeEntries > 0) { // write message in free entry
2285 EPL_MEMCPY(&
2286 ((tEplFrame *) pHistory->
2287 m_aabHistoryFrame[pHistory->m_bWrite])->
2288 m_le_bMessageType, &pFrame_p->m_le_bMessageType,
2289 uiSize_p + EPL_ASND_HEADER_SIZE);
2290 // store size
2291 pHistory->m_auiFrameSize[pHistory->m_bWrite] = uiSize_p;
2292
2293 // decremend number of free bufferentries
2294 pHistory->m_bFreeEntries--;
2295
2296 // increment writeindex
2297 pHistory->m_bWrite++;
2298
2299 // check if write-index run over array-boarder
2300 if (pHistory->m_bWrite == EPL_SDO_HISTORY_SIZE) {
2301 pHistory->m_bWrite = 0;
2302 }
2303
2304 } else { // no free entry
2305 Ret = kEplSdoSeqNoFreeHistory;
2306 }
2307
2308 Exit:
2309 return Ret;
2310 }
2311
2312 //---------------------------------------------------------------------------
2313 //
2314 // Function: EplSdoAsyAckFrameToHistory
2315 //
2316 // Description: function to delete acknowledged frames fron history buffer
2317 //
2318 //
2319 //
2320 // Parameters: pAsySdoSeqCon_p = pointer to control structure of this connection
2321 // bRecSeqNumber_p = receive sequence number of the received frame
2322 //
2323 //
2324 // Returns: tEplKernel = errorcode
2325 //
2326 //
2327 // State:
2328 //
2329 //---------------------------------------------------------------------------
2330 static tEplKernel EplSdoAsyAckFrameToHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
2331 BYTE bRecSeqNumber_p)
2332 {
2333 tEplKernel Ret;
2334 tEplAsySdoConHistory *pHistory;
2335 BYTE bAckIndex;
2336 BYTE bCurrentSeqNum;
2337
2338 Ret = kEplSuccessful;
2339
2340 // get pointer to history buffer
2341 pHistory = &pAsySdoSeqCon_p->m_SdoConHistory;
2342
2343 // release all acknowledged frames from history buffer
2344
2345 // check if there are entries in history
2346 if (pHistory->m_bFreeEntries < EPL_SDO_HISTORY_SIZE) {
2347 bAckIndex = pHistory->m_bAck;
2348 do {
2349 bCurrentSeqNum =
2350 (((tEplFrame *) pHistory->
2351 m_aabHistoryFrame[bAckIndex])->m_Data.m_Asnd.
2352 m_Payload.m_SdoSequenceFrame.
2353 m_le_bSendSeqNumCon & EPL_SEQ_NUM_MASK);
2354 if (((bRecSeqNumber_p -
2355 bCurrentSeqNum) & EPL_SEQ_NUM_MASK)
2356 < EPL_SEQ_NUM_THRESHOLD) {
2357 pHistory->m_auiFrameSize[bAckIndex] = 0;
2358 bAckIndex++;
2359 pHistory->m_bFreeEntries++;
2360 if (bAckIndex == EPL_SDO_HISTORY_SIZE) { // read index run over array-boarder
2361 bAckIndex = 0;
2362 }
2363 } else { // nothing to do anymore,
2364 // because any further frame in history has larger sequence
2365 // number than the acknowledge
2366 goto Exit;
2367 }
2368 }
2369 while ((((bRecSeqNumber_p - 1 -
2370 bCurrentSeqNum) & EPL_SEQ_NUM_MASK)
2371 < EPL_SEQ_NUM_THRESHOLD)
2372 && (pHistory->m_bWrite != bAckIndex));
2373
2374 // store local read-index to global var
2375 pHistory->m_bAck = bAckIndex;
2376 }
2377
2378 Exit:
2379 return Ret;
2380 }
2381
2382 //---------------------------------------------------------------------------
2383 //
2384 // Function: EplSdoAsyReadFromHistory
2385 //
2386 // Description: function to one frame from history
2387 //
2388 //
2389 //
2390 // Parameters: pAsySdoSeqCon_p = pointer to control structure of this connection
2391 // ppFrame_p = pointer to pointer to the buffer of the stored frame
2392 // puiSize_p = OUT: size of the frame
2393 // fInitRead = bool which indicate a start of retransmission
2394 // -> return last not acknowledged message if TRUE
2395 //
2396 //
2397 // Returns: tEplKernel = errorcode
2398 //
2399 //
2400 // State:
2401 //
2402 //---------------------------------------------------------------------------
2403 static tEplKernel EplSdoAsyReadFromHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
2404 tEplFrame ** ppFrame_p,
2405 unsigned int *puiSize_p,
2406 BOOL fInitRead_p)
2407 {
2408 tEplKernel Ret;
2409 tEplAsySdoConHistory *pHistory;
2410
2411 Ret = kEplSuccessful;
2412
2413 // read one message from History
2414
2415 // get pointer to history buffer
2416 pHistory = &pAsySdoSeqCon_p->m_SdoConHistory;
2417
2418 // check if init
2419 if (fInitRead_p != FALSE) { // initialize read index to the index which shall be acknowledged next
2420 pHistory->m_bRead = pHistory->m_bAck;
2421 }
2422 // check if entries are available for reading
2423 if ((pHistory->m_bFreeEntries < EPL_SDO_HISTORY_SIZE)
2424 && (pHistory->m_bWrite != pHistory->m_bRead)) {
2425 // PRINTF4("EplSdoAsyReadFromHistory(): init = %d, read = %u, write = %u, ack = %u", (int) fInitRead_p, (WORD)pHistory->m_bRead, (WORD)pHistory->m_bWrite, (WORD)pHistory->m_bAck);
2426 // PRINTF2(", free entries = %u, next frame size = %u\n", (WORD)pHistory->m_bFreeEntries, pHistory->m_auiFrameSize[pHistory->m_bRead]);
2427
2428 // return pointer to stored frame
2429 *ppFrame_p =
2430 (tEplFrame *) pHistory->m_aabHistoryFrame[pHistory->
2431 m_bRead];
2432
2433 // save size
2434 *puiSize_p = pHistory->m_auiFrameSize[pHistory->m_bRead];
2435
2436 pHistory->m_bRead++;
2437 if (pHistory->m_bRead == EPL_SDO_HISTORY_SIZE) {
2438 pHistory->m_bRead = 0;
2439 }
2440
2441 } else {
2442 // PRINTF3("EplSdoAsyReadFromHistory(): read = %u, ack = %u, free entries = %u, no frame\n", (WORD)pHistory->m_bRead, (WORD)pHistory->m_bAck, (WORD)pHistory->m_bFreeEntries);
2443
2444 // no more frames to send
2445 // return null pointer
2446 *ppFrame_p = NULL;
2447
2448 *puiSize_p = 0;
2449 }
2450
2451 return Ret;
2452
2453 }
2454
2455 //---------------------------------------------------------------------------
2456 //
2457 // Function: EplSdoAsyGetFreeEntriesFromHistory
2458 //
2459 // Description: function returns the number of free histroy entries
2460 //
2461 //
2462 //
2463 // Parameters: pAsySdoSeqCon_p = pointer to control structure of this connection
2464 //
2465 //
2466 // Returns: unsigned int = number of free entries
2467 //
2468 //
2469 // State:
2470 //
2471 //---------------------------------------------------------------------------
2472 static unsigned int EplSdoAsyGetFreeEntriesFromHistory(tEplAsySdoSeqCon *
2473 pAsySdoSeqCon_p)
2474 {
2475 unsigned int uiFreeEntries;
2476
2477 uiFreeEntries =
2478 (unsigned int)pAsySdoSeqCon_p->m_SdoConHistory.m_bFreeEntries;
2479
2480 return uiFreeEntries;
2481 }
2482
2483 //---------------------------------------------------------------------------
2484 //
2485 // Function: EplSdoAsySeqSetTimer
2486 //
2487 // Description: function sets or modify timer in timermosule
2488 //
2489 //
2490 //
2491 // Parameters: pAsySdoSeqCon_p = pointer to control structure of this connection
2492 // ulTimeout = timeout in ms
2493 //
2494 //
2495 // Returns: unsigned int = number of free entries
2496 //
2497 //
2498 // State:
2499 //
2500 //---------------------------------------------------------------------------
2501 static tEplKernel EplSdoAsySeqSetTimer(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
2502 unsigned long ulTimeout)
2503 {
2504 tEplKernel Ret;
2505 tEplTimerArg TimerArg;
2506
2507 TimerArg.m_EventSink = kEplEventSinkSdoAsySeq;
2508 TimerArg.m_ulArg = (unsigned long)pAsySdoSeqCon_p;
2509
2510 if (pAsySdoSeqCon_p->m_EplTimerHdl == 0) { // create new timer
2511 Ret = EplTimeruSetTimerMs(&pAsySdoSeqCon_p->m_EplTimerHdl,
2512 ulTimeout, TimerArg);
2513 } else { // modify exisiting timer
2514 Ret = EplTimeruModifyTimerMs(&pAsySdoSeqCon_p->m_EplTimerHdl,
2515 ulTimeout, TimerArg);
2516
2517 }
2518
2519 return Ret;
2520 }
2521
2522 // EOF
Nao low-level kernelspace/userspace library that runs on our robots, Nao-Team HTWK