tjh.dev
Linux kernel mirror (for testing)
git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git
kernel
os
linux
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 kernel PDO 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: EplPdok.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/05/22 d.k.: start of the implementation, version 1.00
68
69****************************************************************************/
70
71#include "kernel/EplPdok.h"
72#include "kernel/EplPdokCal.h"
73#include "kernel/EplEventk.h"
74#include "kernel/EplObdk.h"
75
76#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
77
78#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) == 0)
79
80#error 'ERROR: Missing DLLk-Modul!'
81
82#endif
83
84#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) == 0)
85
86#error 'ERROR: Missing OBDk-Modul!'
87
88#endif
89/***************************************************************************/
90/* */
91/* */
92/* G L O B A L D E F I N I T I O N S */
93/* */
94/* */
95/***************************************************************************/
96
97//---------------------------------------------------------------------------
98// const defines
99//---------------------------------------------------------------------------
100
101#define EPL_PDOK_OBD_IDX_RX_COMM_PARAM 0x1400
102#define EPL_PDOK_OBD_IDX_RX_MAPP_PARAM 0x1600
103#define EPL_PDOK_OBD_IDX_TX_COMM_PARAM 0x1800
104#define EPL_PDOK_OBD_IDX_TX_MAPP_PARAM 0x1A00
105
106//---------------------------------------------------------------------------
107// local types
108//---------------------------------------------------------------------------
109
110//---------------------------------------------------------------------------
111// modul globale vars
112//---------------------------------------------------------------------------
113
114//---------------------------------------------------------------------------
115// local function prototypes
116//---------------------------------------------------------------------------
117
118/***************************************************************************/
119/* */
120/* */
121/* C L A S S EplPdok */
122/* */
123/* */
124/***************************************************************************/
125//
126// Description:
127//
128//
129/***************************************************************************/
130
131//=========================================================================//
132// //
133// P R I V A T E D E F I N I T I O N S //
134// //
135//=========================================================================//
136
137//---------------------------------------------------------------------------
138// const defines
139//---------------------------------------------------------------------------
140
141//---------------------------------------------------------------------------
142// local types
143//---------------------------------------------------------------------------
144
145//---------------------------------------------------------------------------
146// local vars
147//---------------------------------------------------------------------------
148
149//---------------------------------------------------------------------------
150// local function prototypes
151//---------------------------------------------------------------------------
152
153//=========================================================================//
154// //
155// P U B L I C F U N C T I O N S //
156// //
157//=========================================================================//
158
159//---------------------------------------------------------------------------
160//
161// Function: EplPdokAddInstance()
162//
163// Description: add and initialize new instance of EPL stack
164//
165// Parameters: none
166//
167// Returns: tEplKernel = error code
168//
169//
170// State:
171//
172//---------------------------------------------------------------------------
173
174tEplKernel EplPdokAddInstance(void)
175{
176
177 return kEplSuccessful;
178}
179
180//---------------------------------------------------------------------------
181//
182// Function: EplPdokDelInstance()
183//
184// Description: deletes an instance of EPL stack
185//
186// Parameters: none
187//
188// Returns: tEplKernel = error code
189//
190//
191// State:
192//
193//---------------------------------------------------------------------------
194
195tEplKernel EplPdokDelInstance(void)
196{
197
198 return kEplSuccessful;
199}
200
201//---------------------------------------------------------------------------
202//
203// Function: EplPdokCbPdoReceived
204//
205// Description: This function is called by DLL if PRes or PReq frame was
206// received. It posts the frame to the event queue.
207// It is called in states NMT_CS_READY_TO_OPERATE and NMT_CS_OPERATIONAL.
208// The passed PDO needs not to be valid.
209//
210// Parameters: pFrameInfo_p = pointer to frame info structure
211//
212// Returns: tEplKernel = error code
213//
214//
215// State:
216//
217//---------------------------------------------------------------------------
218
219tEplKernel EplPdokCbPdoReceived(tEplFrameInfo * pFrameInfo_p)
220{
221 tEplKernel Ret = kEplSuccessful;
222 tEplEvent Event;
223
224 Event.m_EventSink = kEplEventSinkPdok;
225 Event.m_EventType = kEplEventTypePdoRx;
226 // limit copied data to size of PDO (because from some CNs the frame is larger than necessary)
227 Event.m_uiSize = AmiGetWordFromLe(&pFrameInfo_p->m_pFrame->m_Data.m_Pres.m_le_wSize) + 24; // pFrameInfo_p->m_uiFrameSize;
228 Event.m_pArg = pFrameInfo_p->m_pFrame;
229 Ret = EplEventkPost(&Event);
230
231 return Ret;
232}
233
234//---------------------------------------------------------------------------
235//
236// Function: EplPdokCbPdoTransmitted
237//
238// Description: This function is called by DLL if PRes or PReq frame was
239// sent. It posts the pointer to the frame to the event queue.
240// It is called in NMT_CS_PRE_OPERATIONAL_2,
241// NMT_CS_READY_TO_OPERATE and NMT_CS_OPERATIONAL.
242//
243// Parameters: pFrameInfo_p = pointer to frame info structure
244//
245// Returns: tEplKernel = error code
246//
247//
248// State:
249//
250//---------------------------------------------------------------------------
251
252tEplKernel EplPdokCbPdoTransmitted(tEplFrameInfo * pFrameInfo_p)
253{
254 tEplKernel Ret = kEplSuccessful;
255 tEplEvent Event;
256
257 Event.m_EventSink = kEplEventSinkPdok;
258 Event.m_EventType = kEplEventTypePdoTx;
259 Event.m_uiSize = sizeof(tEplFrameInfo);
260 Event.m_pArg = pFrameInfo_p;
261 Ret = EplEventkPost(&Event);
262
263 return Ret;
264}
265
266//---------------------------------------------------------------------------
267//
268// Function: EplPdokCbSoa
269//
270// Description: This function is called by DLL if SoA frame was
271// received resp. sent. It posts this event to the event queue.
272//
273// Parameters: pFrameInfo_p = pointer to frame info structure
274//
275// Returns: tEplKernel = error code
276//
277//
278// State:
279//
280//---------------------------------------------------------------------------
281
282tEplKernel EplPdokCbSoa(tEplFrameInfo * pFrameInfo_p)
283{
284 tEplKernel Ret = kEplSuccessful;
285 tEplEvent Event;
286
287 Event.m_EventSink = kEplEventSinkPdok;
288 Event.m_EventType = kEplEventTypePdoSoa;
289 Event.m_uiSize = 0;
290 Event.m_pArg = NULL;
291 Ret = EplEventkPost(&Event);
292
293 return Ret;
294}
295
296//---------------------------------------------------------------------------
297//
298// Function: EplPdokProcess
299//
300// Description: This function processes all received and transmitted PDOs.
301// This function must not be interrupted by any other task
302// except ISRs (like the ethernet driver ISR, which may call
303// EplPdokCbFrameReceived() or EplPdokCbFrameTransmitted()).
304//
305// Parameters: pEvent_p = pointer to event structure
306//
307// Returns: tEplKernel = error code
308//
309//
310// State:
311//
312//---------------------------------------------------------------------------
313
314tEplKernel EplPdokProcess(tEplEvent * pEvent_p)
315{
316 tEplKernel Ret = kEplSuccessful;
317 u16 wPdoSize;
318 u16 wBitOffset;
319 u16 wBitSize;
320 u16 wVarSize;
321 u64 qwObjectMapping;
322 u8 bMappSubindex;
323 u8 bObdSubindex;
324 u16 wObdMappIndex;
325 u16 wObdCommIndex;
326 u16 wPdoId;
327 u8 bObdData;
328 u8 bObjectCount;
329 u8 bFrameData;
330 BOOL fValid;
331 tEplObdSize ObdSize;
332 tEplFrame *pFrame;
333 tEplFrameInfo *pFrameInfo;
334 unsigned int uiNodeId;
335 tEplMsgType MsgType;
336
337 // 0xFF=invalid, RPDO: 0x00=PReq, localNodeId=PRes, remoteNodeId=PRes
338 // TPDO: 0x00=PRes, MN: CnNodeId=PReq
339
340 switch (pEvent_p->m_EventType) {
341 case kEplEventTypePdoRx: // RPDO received
342 pFrame = (tEplFrame *) pEvent_p->m_pArg;
343
344 // check if received RPDO is valid
345 bFrameData =
346 AmiGetByteFromLe(&pFrame->m_Data.m_Pres.m_le_bFlag1);
347 if ((bFrameData & EPL_FRAME_FLAG1_RD) == 0) { // RPDO invalid
348 goto Exit;
349 }
350 // retrieve EPL message type
351 MsgType = AmiGetByteFromLe(&pFrame->m_le_bMessageType);
352 if (MsgType == kEplMsgTypePreq) { // RPDO is PReq frame
353 uiNodeId = EPL_PDO_PREQ_NODE_ID; // 0x00
354 } else { // RPDO is PRes frame
355 // retrieve node ID
356 uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bSrcNodeId);
357 }
358
359 // search for appropriate valid RPDO in OD
360 wObdMappIndex = EPL_PDOK_OBD_IDX_RX_MAPP_PARAM;
361 for (wObdCommIndex = EPL_PDOK_OBD_IDX_RX_COMM_PARAM;
362 wObdCommIndex < (EPL_PDOK_OBD_IDX_RX_COMM_PARAM + 0x00FF);
363 wObdCommIndex++, wObdMappIndex++) {
364 ObdSize = 1;
365 // read node ID from OD
366 Ret =
367 EplObdReadEntry(wObdCommIndex, 0x01, &bObdData,
368 &ObdSize);
369 if ((Ret == kEplObdIndexNotExist)
370 || (Ret == kEplObdSubindexNotExist)
371 || (Ret == kEplObdIllegalPart)) { // PDO does not exist; last PDO reached
372 Ret = kEplSuccessful;
373 goto Exit;
374 } else if (Ret != kEplSuccessful) { // other fatal error occured
375 goto Exit;
376 }
377 // entry read successfully
378 if (bObdData != uiNodeId) { // node ID does not equal - wrong PDO, try next PDO in OD
379 continue;
380 }
381 ObdSize = 1;
382 // read number of mapped objects from OD; this indicates if the PDO is valid
383 Ret =
384 EplObdReadEntry(wObdMappIndex, 0x00, &bObjectCount,
385 &ObdSize);
386 if ((Ret == kEplObdIndexNotExist)
387 || (Ret == kEplObdSubindexNotExist)
388 || (Ret == kEplObdIllegalPart)) { // PDO does not exist; last PDO reached
389 Ret = kEplSuccessful;
390 goto Exit;
391 } else if (Ret != kEplSuccessful) { // other fatal error occured
392 goto Exit;
393 }
394 // entry read successfully
395 if (bObjectCount == 0) { // PDO in OD not valid, try next PDO in OD
396 continue;
397 }
398
399 ObdSize = 1;
400 // check PDO mapping version
401 Ret =
402 EplObdReadEntry(wObdCommIndex, 0x02, &bObdData,
403 &ObdSize);
404 if (Ret != kEplSuccessful) { // other fatal error occured
405 goto Exit;
406 }
407 // entry read successfully
408 // retrieve PDO version from frame
409 bFrameData =
410 AmiGetByteFromLe(&pFrame->m_Data.m_Pres.
411 m_le_bPdoVersion);
412 if ((bObdData & EPL_VERSION_MAIN) != (bFrameData & EPL_VERSION_MAIN)) { // PDO versions do not match
413 // $$$ raise PDO error
414 // termiate processing of this RPDO
415 goto Exit;
416 }
417 // valid RPDO found
418
419 // retrieve PDO size
420 wPdoSize =
421 AmiGetWordFromLe(&pFrame->m_Data.m_Pres.m_le_wSize);
422
423 // process mapping
424 for (bMappSubindex = 1; bMappSubindex <= bObjectCount;
425 bMappSubindex++) {
426 ObdSize = 8; // u64
427 // read object mapping from OD
428 Ret =
429 EplObdReadEntry(wObdMappIndex,
430 bMappSubindex,
431 &qwObjectMapping, &ObdSize);
432 if (Ret != kEplSuccessful) { // other fatal error occured
433 goto Exit;
434 }
435 // check if object mapping entry is valid, i.e. unequal zero, because "empty" entries are allowed
436 if (qwObjectMapping == 0) { // invalid entry, continue with next entry
437 continue;
438 }
439 // decode object mapping
440 wObdCommIndex =
441 (u16) (qwObjectMapping &
442 0x000000000000FFFFLL);
443 bObdSubindex =
444 (u8) ((qwObjectMapping &
445 0x0000000000FF0000LL) >> 16);
446 wBitOffset =
447 (u16) ((qwObjectMapping &
448 0x0000FFFF00000000LL) >> 32);
449 wBitSize =
450 (u16) ((qwObjectMapping &
451 0xFFFF000000000000LL) >> 48);
452
453 // check if object exceeds PDO size
454 if (((wBitOffset + wBitSize) >> 3) > wPdoSize) { // wrong object mapping; PDO size is too low
455 // $$$ raise PDO error
456 // terminate processing of this RPDO
457 goto Exit;
458 }
459 // copy object from RPDO to process/OD variable
460 ObdSize = wBitSize >> 3;
461 Ret =
462 EplObdWriteEntryFromLe(wObdCommIndex,
463 bObdSubindex,
464 &pFrame->m_Data.
465 m_Pres.
466 m_le_abPayload[(wBitOffset >> 3)], ObdSize);
467 if (Ret != kEplSuccessful) { // other fatal error occured
468 goto Exit;
469 }
470
471 }
472
473 // processing finished successfully
474 goto Exit;
475 }
476 break;
477
478 case kEplEventTypePdoTx: // TPDO transmitted
479 pFrameInfo = (tEplFrameInfo *) pEvent_p->m_pArg;
480 pFrame = pFrameInfo->m_pFrame;
481
482 // set TPDO invalid, so that only fully processed TPDOs are sent as valid
483 bFrameData =
484 AmiGetByteFromLe(&pFrame->m_Data.m_Pres.m_le_bFlag1);
485 AmiSetByteToLe(&pFrame->m_Data.m_Pres.m_le_bFlag1,
486 (bFrameData & ~EPL_FRAME_FLAG1_RD));
487
488 // retrieve EPL message type
489 MsgType = AmiGetByteFromLe(&pFrame->m_le_bMessageType);
490 if (MsgType == kEplMsgTypePres) { // TPDO is PRes frame
491 uiNodeId = EPL_PDO_PRES_NODE_ID; // 0x00
492 } else { // TPDO is PReq frame
493 // retrieve node ID
494 uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bDstNodeId);
495 }
496
497 // search for appropriate valid TPDO in OD
498 wObdMappIndex = EPL_PDOK_OBD_IDX_TX_MAPP_PARAM;
499 wObdCommIndex = EPL_PDOK_OBD_IDX_TX_COMM_PARAM;
500 for (wPdoId = 0;; wPdoId++, wObdCommIndex++, wObdMappIndex++) {
501 ObdSize = 1;
502 // read node ID from OD
503 Ret =
504 EplObdReadEntry(wObdCommIndex, 0x01, &bObdData,
505 &ObdSize);
506 if ((Ret == kEplObdIndexNotExist)
507 || (Ret == kEplObdSubindexNotExist)
508 || (Ret == kEplObdIllegalPart)) { // PDO does not exist; last PDO reached
509 Ret = kEplSuccessful;
510 goto Exit;
511 } else if (Ret != kEplSuccessful) { // other fatal error occured
512 goto Exit;
513 }
514 // entry read successfully
515 if (bObdData != uiNodeId) { // node ID does not equal - wrong PDO, try next PDO in OD
516 continue;
517 }
518 ObdSize = 1;
519 // read number of mapped objects from OD; this indicates if the PDO is valid
520 Ret =
521 EplObdReadEntry(wObdMappIndex, 0x00, &bObjectCount,
522 &ObdSize);
523 if ((Ret == kEplObdIndexNotExist)
524 || (Ret == kEplObdSubindexNotExist)
525 || (Ret == kEplObdIllegalPart)) { // PDO does not exist; last PDO reached
526 Ret = kEplSuccessful;
527 goto Exit;
528 } else if (Ret != kEplSuccessful) { // other fatal error occured
529 goto Exit;
530 }
531 // entry read successfully
532 if (bObjectCount == 0) { // PDO in OD not valid, try next PDO in OD
533 continue;
534 }
535 // valid TPDO found
536
537 ObdSize = 1;
538 // get PDO mapping version from OD
539 Ret =
540 EplObdReadEntry(wObdCommIndex, 0x02, &bObdData,
541 &ObdSize);
542 if (Ret != kEplSuccessful) { // other fatal error occured
543 goto Exit;
544 }
545 // entry read successfully
546 // set PDO version in frame
547 AmiSetByteToLe(&pFrame->m_Data.m_Pres.m_le_bPdoVersion,
548 bObdData);
549
550 // calculate PDO size
551 wPdoSize = 0;
552
553 // process mapping
554 for (bMappSubindex = 1; bMappSubindex <= bObjectCount;
555 bMappSubindex++) {
556 ObdSize = 8; // u64
557 // read object mapping from OD
558 Ret =
559 EplObdReadEntry(wObdMappIndex,
560 bMappSubindex,
561 &qwObjectMapping, &ObdSize);
562 if (Ret != kEplSuccessful) { // other fatal error occured
563 goto Exit;
564 }
565 // check if object mapping entry is valid, i.e. unequal zero, because "empty" entries are allowed
566 if (qwObjectMapping == 0) { // invalid entry, continue with next entry
567 continue;
568 }
569 // decode object mapping
570 wObdCommIndex =
571 (u16) (qwObjectMapping &
572 0x000000000000FFFFLL);
573 bObdSubindex =
574 (u8) ((qwObjectMapping &
575 0x0000000000FF0000LL) >> 16);
576 wBitOffset =
577 (u16) ((qwObjectMapping &
578 0x0000FFFF00000000LL) >> 32);
579 wBitSize =
580 (u16) ((qwObjectMapping &
581 0xFFFF000000000000LL) >> 48);
582
583 // calculate max PDO size
584 ObdSize = wBitSize >> 3;
585 wVarSize = (wBitOffset >> 3) + (u16) ObdSize;
586 if ((unsigned int)(wVarSize + 24) > pFrameInfo->m_uiFrameSize) { // TPDO is too short
587 // $$$ raise PDO error, set Ret
588 goto Exit;
589 }
590 if (wVarSize > wPdoSize) { // memorize new PDO size
591 wPdoSize = wVarSize;
592 }
593 // copy object from process/OD variable to TPDO
594 Ret =
595 EplObdReadEntryToLe(wObdCommIndex,
596 bObdSubindex,
597 &pFrame->m_Data.m_Pres.
598 m_le_abPayload[(wBitOffset >> 3)], &ObdSize);
599 if (Ret != kEplSuccessful) { // other fatal error occured
600 goto Exit;
601 }
602
603 }
604
605 // set PDO size in frame
606 AmiSetWordToLe(&pFrame->m_Data.m_Pres.m_le_wSize,
607 wPdoSize);
608
609 Ret = EplPdokCalAreTpdosValid(&fValid);
610 if (fValid != FALSE) {
611 // set TPDO valid
612 bFrameData =
613 AmiGetByteFromLe(&pFrame->m_Data.m_Pres.
614 m_le_bFlag1);
615 AmiSetByteToLe(&pFrame->m_Data.m_Pres.
616 m_le_bFlag1,
617 (bFrameData |
618 EPL_FRAME_FLAG1_RD));
619 }
620 // processing finished successfully
621
622 goto Exit;
623 }
624 break;
625
626 case kEplEventTypePdoSoa: // SoA received
627
628 // invalidate TPDOs
629 Ret = EplPdokCalSetTpdosValid(FALSE);
630 break;
631
632 default:
633 {
634 ASSERTMSG(FALSE,
635 "EplPdokProcess(): unhandled event type!\n");
636 }
637 }
638
639 Exit:
640 return Ret;
641}
642
643//=========================================================================//
644// //
645// P R I V A T E F U N C T I O N S //
646// //
647//=========================================================================//
648
649//---------------------------------------------------------------------------
650//
651// Function:
652//
653// Description:
654//
655//
656//
657// Parameters:
658//
659//
660// Returns:
661//
662//
663// State:
664//
665//---------------------------------------------------------------------------
666
667#endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
668
669// EOF