?  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 / demo_main.c
blob263fa042291d218d6931aecb5c08c0f2ad536b86
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: demoapplication for EPL MN (with SDO over UDP)
9 under Linux on X86 with RTL8139 Ethernet controller
10
11 License:
12
13 Redistribution and use in source and binary forms, with or without
14 modification, are permitted provided that the following conditions
15 are met:
16
17 1. Redistributions of source code must retain the above copyright
18 notice, this list of conditions and the following disclaimer.
19
20 2. Redistributions in binary form must reproduce the above copyright
21 notice, this list of conditions and the following disclaimer in the
22 documentation and/or other materials provided with the distribution.
23
24 3. Neither the name of SYSTEC electronic GmbH nor the names of its
25 contributors may be used to endorse or promote products derived
26 from this software without prior written permission. For written
27 permission, please contact info@systec-electronic.com.
28
29 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
30 "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
31 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
32 FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
33 COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
34 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
35 BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
36 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
37 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
38 LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
39 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
40 POSSIBILITY OF SUCH DAMAGE.
41
42 Severability Clause:
43
44 If a provision of this License is or becomes illegal, invalid or
45 unenforceable in any jurisdiction, that shall not affect:
46 1. the validity or enforceability in that jurisdiction of any other
47 provision of this License; or
48 2. the validity or enforceability in other jurisdictions of that or
49 any other provision of this License.
50
51 -------------------------------------------------------------------------
52
53 $RCSfile: demo_main.c,v $
54
55 $Author: D.Krueger $
56
57 $Revision: 1.10 $ $Date: 2008/11/19 18:11:43 $
58
59 $State: Exp $
60
61 Build Environment:
62 GCC
63
64 -------------------------------------------------------------------------
65
66 Revision History:
67
68 2006/09/01 d.k.: start of implementation
69
70 ****************************************************************************/
71
72 #include <linux/module.h>
73 #include <linux/kernel.h>
74 #include <linux/init.h>
75 #include <linux/errno.h>
76 #include <linux/major.h>
77 #include <linux/version.h>
78 #include <asm/io.h>
79 #include <asm/uaccess.h>
80 #include <asm/atomic.h>
81 #include <linux/sched.h>
82 #include <linux/kmod.h>
83 #include <linux/slab.h>
84 #include <linux/pci.h>
85 #include <linux/proc_fs.h>
86
87 #include "Epl.h"
88 #include "proc_fs.h"
89
90 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
91 // remove ("make invisible") obsolete symbols for kernel versions 2.6
92 // and higher
93 #define MOD_INC_USE_COUNT
94 #define MOD_DEC_USE_COUNT
95 #define EXPORT_NO_SYMBOLS
96 #else
97 #error "This driver needs a 2.6.x kernel or higher"
98 #endif
99
100 /***************************************************************************/
101 /* */
102 /* */
103 /* G L O B A L D E F I N I T I O N S */
104 /* */
105 /* */
106 /***************************************************************************/
107
108 // Metainformation
109 MODULE_LICENSE("Dual BSD/GPL");
110 #ifdef MODULE_AUTHOR
111 MODULE_AUTHOR("Daniel.Krueger@SYSTEC-electronic.com");
112 MODULE_DESCRIPTION("EPL MN demo");
113 #endif
114
115 //---------------------------------------------------------------------------
116 // const defines
117 //---------------------------------------------------------------------------
118
119 // TracePoint support for realtime-debugging
120 #ifdef _DBG_TRACE_POINTS_
121 void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
122 #define TGT_DBG_SIGNAL_TRACE_POINT(p) TgtDbgSignalTracePoint(p)
123 #else
124 #define TGT_DBG_SIGNAL_TRACE_POINT(p)
125 #endif
126
127 #define NODEID 0xF0 //=> MN
128 #define CYCLE_LEN 5000 // [us]
129 #define IP_ADDR 0xc0a86401 // 192.168.100.1
130 #define SUBNET_MASK 0xFFFFFF00 // 255.255.255.0
131 #define HOSTNAME "SYS TEC electronic EPL Stack "
132 #define IF_ETH EPL_VETH_NAME
133
134 // LIGHT EFFECT
135 #define DEFAULT_MAX_CYCLE_COUNT 20 // 6 is very fast
136 #define APP_DEFAULT_MODE 0x01
137 #define APP_LED_COUNT 5 // number of LEDs in one row
138 #define APP_LED_MASK ((1 << APP_LED_COUNT) - 1)
139 #define APP_DOUBLE_LED_MASK ((1 << (APP_LED_COUNT * 2)) - 1)
140 #define APP_MODE_COUNT 5
141 #define APP_MODE_MASK ((1 << APP_MODE_COUNT) - 1)
142
143 //---------------------------------------------------------------------------
144 // local types
145 //---------------------------------------------------------------------------
146
147 //---------------------------------------------------------------------------
148 // modul globale vars
149 //---------------------------------------------------------------------------
150
151 CONST BYTE abMacAddr[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
152
153 BYTE bVarIn1_l;
154 BYTE bVarOut1_l;
155 BYTE bVarOut1Old_l;
156 BYTE bModeSelect_l; // state of the pushbuttons to select the mode
157 BYTE bSpeedSelect_l; // state of the pushbuttons to increase/decrease the speed
158 BYTE bSpeedSelectOld_l; // old state of the pushbuttons
159 DWORD dwLeds_l; // current state of all LEDs
160 BYTE bLedsRow1_l; // current state of the LEDs in row 1
161 BYTE bLedsRow2_l; // current state of the LEDs in row 2
162 BYTE abSelect_l[3]; // pushbuttons from CNs
163
164 DWORD dwMode_l; // current mode
165 int iCurCycleCount_l; // current cycle count
166 int iMaxCycleCount_l; // maximum cycle count (i.e. number of cycles until next light movement step)
167 int iToggle; // indicates the light movement direction
168
169 BYTE abDomain_l[3000];
170
171 static wait_queue_head_t WaitQueueShutdown_g; // wait queue for tEplNmtEventSwitchOff
172 static atomic_t AtomicShutdown_g = ATOMIC_INIT(FALSE);
173
174 static DWORD dw_le_CycleLen_g;
175
176 static uint uiNodeId_g = EPL_C_ADR_INVALID;
177 module_param_named(nodeid, uiNodeId_g, uint, 0);
178
179 static uint uiCycleLen_g = CYCLE_LEN;
180 module_param_named(cyclelen, uiCycleLen_g, uint, 0);
181
182 //---------------------------------------------------------------------------
183 // local function prototypes
184 //---------------------------------------------------------------------------
185
186 // This function is the entry point for your object dictionary. It is defined
187 // in OBJDICT.C by define EPL_OBD_INIT_RAM_NAME. Use this function name to define
188 // this function prototype here. If you want to use more than one Epl
189 // instances then the function name of each object dictionary has to differ.
190
191 tEplKernel PUBLIC EplObdInitRam(tEplObdInitParam MEM * pInitParam_p);
192
193 tEplKernel PUBLIC AppCbEvent(tEplApiEventType EventType_p, // IN: event type (enum)
194 tEplApiEventArg * pEventArg_p, // IN: event argument (union)
195 void GENERIC * pUserArg_p);
196
197 tEplKernel PUBLIC AppCbSync(void);
198
199 static int __init EplLinInit(void);
200 static void __exit EplLinExit(void);
201
202 //---------------------------------------------------------------------------
203 // Kernel Module specific Data Structures
204 //---------------------------------------------------------------------------
205
206 EXPORT_NO_SYMBOLS;
207
208 //module_init(EplLinInit);
209 //module_exit(EplLinExit);
210
211 //=========================================================================//
212 // //
213 // P U B L I C F U N C T I O N S //
214 // //
215 //=========================================================================//
216
217 //---------------------------------------------------------------------------
218 //
219 // Function:
220 //
221 // Description:
222 //
223 //
224 //
225 // Parameters:
226 //
227 //
228 // Returns:
229 //
230 //
231 // State:
232 //
233 //---------------------------------------------------------------------------
234 static int __init EplLinInit(void)
235 {
236 tEplKernel EplRet;
237 int iRet;
238 static tEplApiInitParam EplApiInitParam = { 0 };
239 char *sHostname = HOSTNAME;
240 char *argv[4], *envp[3];
241 char sBuffer[16];
242 unsigned int uiVarEntries;
243 tEplObdSize ObdSize;
244
245 atomic_set(&AtomicShutdown_g, TRUE);
246
247 // get node ID from insmod command line
248 EplApiInitParam.m_uiNodeId = uiNodeId_g;
249
250 if (EplApiInitParam.m_uiNodeId == EPL_C_ADR_INVALID) { // invalid node ID set
251 // set default node ID
252 EplApiInitParam.m_uiNodeId = NODEID;
253 }
254
255 uiNodeId_g = EplApiInitParam.m_uiNodeId;
256
257 // calculate IP address
258 EplApiInitParam.m_dwIpAddress =
259 (0xFFFFFF00 & IP_ADDR) | EplApiInitParam.m_uiNodeId;
260
261 EplApiInitParam.m_fAsyncOnly = FALSE;
262
263 EplApiInitParam.m_uiSizeOfStruct = sizeof(EplApiInitParam);
264 EPL_MEMCPY(EplApiInitParam.m_abMacAddress, abMacAddr,
265 sizeof(EplApiInitParam.m_abMacAddress));
266 // EplApiInitParam.m_abMacAddress[5] = (BYTE) EplApiInitParam.m_uiNodeId;
267 EplApiInitParam.m_dwFeatureFlags = -1;
268 EplApiInitParam.m_dwCycleLen = uiCycleLen_g; // required for error detection
269 EplApiInitParam.m_uiIsochrTxMaxPayload = 100; // const
270 EplApiInitParam.m_uiIsochrRxMaxPayload = 100; // const
271 EplApiInitParam.m_dwPresMaxLatency = 50000; // const; only required for IdentRes
272 EplApiInitParam.m_uiPreqActPayloadLimit = 36; // required for initialisation (+28 bytes)
273 EplApiInitParam.m_uiPresActPayloadLimit = 36; // required for initialisation of Pres frame (+28 bytes)
274 EplApiInitParam.m_dwAsndMaxLatency = 150000; // const; only required for IdentRes
275 EplApiInitParam.m_uiMultiplCycleCnt = 0; // required for error detection
276 EplApiInitParam.m_uiAsyncMtu = 1500; // required to set up max frame size
277 EplApiInitParam.m_uiPrescaler = 2; // required for sync
278 EplApiInitParam.m_dwLossOfFrameTolerance = 500000;
279 EplApiInitParam.m_dwAsyncSlotTimeout = 3000000;
280 EplApiInitParam.m_dwWaitSocPreq = 150000;
281 EplApiInitParam.m_dwDeviceType = -1; // NMT_DeviceType_U32
282 EplApiInitParam.m_dwVendorId = -1; // NMT_IdentityObject_REC.VendorId_U32
283 EplApiInitParam.m_dwProductCode = -1; // NMT_IdentityObject_REC.ProductCode_U32
284 EplApiInitParam.m_dwRevisionNumber = -1; // NMT_IdentityObject_REC.RevisionNo_U32
285 EplApiInitParam.m_dwSerialNumber = -1; // NMT_IdentityObject_REC.SerialNo_U32
286 EplApiInitParam.m_dwSubnetMask = SUBNET_MASK;
287 EplApiInitParam.m_dwDefaultGateway = 0;
288 EPL_MEMCPY(EplApiInitParam.m_sHostname, sHostname,
289 sizeof(EplApiInitParam.m_sHostname));
290
291 // currently unset parameters left at default value 0
292 //EplApiInitParam.m_qwVendorSpecificExt1;
293 //EplApiInitParam.m_dwVerifyConfigurationDate; // CFM_VerifyConfiguration_REC.ConfDate_U32
294 //EplApiInitParam.m_dwVerifyConfigurationTime; // CFM_VerifyConfiguration_REC.ConfTime_U32
295 //EplApiInitParam.m_dwApplicationSwDate; // PDL_LocVerApplSw_REC.ApplSwDate_U32 on programmable device or date portion of NMT_ManufactSwVers_VS on non-programmable device
296 //EplApiInitParam.m_dwApplicationSwTime; // PDL_LocVerApplSw_REC.ApplSwTime_U32 on programmable device or time portion of NMT_ManufactSwVers_VS on non-programmable device
297 //EplApiInitParam.m_abVendorSpecificExt2[48];
298
299 // set callback functions
300 EplApiInitParam.m_pfnCbEvent = AppCbEvent;
301 EplApiInitParam.m_pfnCbSync = AppCbSync;
302
303 printk
304 ("\n\n Hello, I'm a simple POWERLINK node running as %s!\n (build: %s / %s)\n\n",
305 (uiNodeId_g ==
306 EPL_C_ADR_MN_DEF_NODE_ID ? "Managing Node" : "Controlled Node"),
307 __DATE__, __TIME__);
308
309 // initialize the Linux a wait queue for shutdown of this module
310 init_waitqueue_head(&WaitQueueShutdown_g);
311
312 // initialize the procfs device
313 EplRet = EplLinProcInit();
314 if (EplRet != kEplSuccessful) {
315 goto Exit;
316 }
317 // initialize POWERLINK stack
318 EplRet = EplApiInitialize(&EplApiInitParam);
319 if (EplRet != kEplSuccessful) {
320 goto Exit;
321 }
322 // link process variables used by CN to object dictionary
323 ObdSize = sizeof(bVarIn1_l);
324 uiVarEntries = 1;
325 EplRet =
326 EplApiLinkObject(0x6000, &bVarIn1_l, &uiVarEntries, &ObdSize, 0x01);
327 if (EplRet != kEplSuccessful) {
328 goto Exit;
329 }
330
331 ObdSize = sizeof(bVarOut1_l);
332 uiVarEntries = 1;
333 EplRet =
334 EplApiLinkObject(0x6200, &bVarOut1_l, &uiVarEntries, &ObdSize,
335 0x01);
336 if (EplRet != kEplSuccessful) {
337 goto Exit;
338 }
339 // link process variables used by MN to object dictionary
340 #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
341 ObdSize = sizeof(bLedsRow1_l);
342 uiVarEntries = 1;
343 EplRet =
344 EplApiLinkObject(0x2000, &bLedsRow1_l, &uiVarEntries, &ObdSize,
345 0x01);
346 if (EplRet != kEplSuccessful) {
347 goto Exit;
348 }
349
350 ObdSize = sizeof(bLedsRow2_l);
351 uiVarEntries = 1;
352 EplRet =
353 EplApiLinkObject(0x2000, &bLedsRow2_l, &uiVarEntries, &ObdSize,
354 0x02);
355 if (EplRet != kEplSuccessful) {
356 goto Exit;
357 }
358
359 ObdSize = sizeof(bSpeedSelect_l);
360 uiVarEntries = 1;
361 EplRet =
362 EplApiLinkObject(0x2000, &bSpeedSelect_l, &uiVarEntries, &ObdSize,
363 0x03);
364 if (EplRet != kEplSuccessful) {
365 goto Exit;
366 }
367
368 ObdSize = sizeof(bSpeedSelectOld_l);
369 uiVarEntries = 1;
370 EplRet =
371 EplApiLinkObject(0x2000, &bSpeedSelectOld_l, &uiVarEntries,
372 &ObdSize, 0x04);
373 if (EplRet != kEplSuccessful) {
374 goto Exit;
375 }
376
377 ObdSize = sizeof(abSelect_l[0]);
378 uiVarEntries = sizeof(abSelect_l);
379 EplRet =
380 EplApiLinkObject(0x2200, &abSelect_l[0], &uiVarEntries, &ObdSize,
381 0x01);
382 if (EplRet != kEplSuccessful) {
383 goto Exit;
384 }
385 #endif
386
387 // link a DOMAIN to object 0x6100, but do not exit, if it is missing
388 ObdSize = sizeof(abDomain_l);
389 uiVarEntries = 1;
390 EplRet =
391 EplApiLinkObject(0x6100, &abDomain_l, &uiVarEntries, &ObdSize,
392 0x00);
393 if (EplRet != kEplSuccessful) {
394 printk("EplApiLinkObject(0x6100): returns 0x%X\n", EplRet);
395 }
396 // reset old process variables
397 bVarOut1Old_l = 0;
398 bSpeedSelectOld_l = 0;
399 dwMode_l = APP_DEFAULT_MODE;
400 iMaxCycleCount_l = DEFAULT_MAX_CYCLE_COUNT;
401
402 // configure IP address of virtual network interface
403 // for TCP/IP communication over the POWERLINK network
404 sprintf(sBuffer, "%lu.%lu.%lu.%lu",
405 (EplApiInitParam.m_dwIpAddress >> 24),
406 ((EplApiInitParam.m_dwIpAddress >> 16) & 0xFF),
407 ((EplApiInitParam.m_dwIpAddress >> 8) & 0xFF),
408 (EplApiInitParam.m_dwIpAddress & 0xFF));
409 /* set up a minimal environment */
410 iRet = 0;
411 envp[iRet++] = "HOME=/";
412 envp[iRet++] = "PATH=/sbin:/bin:/usr/sbin:/usr/bin";
413 envp[iRet] = NULL;
414
415 /* set up the argument list */
416 iRet = 0;
417 argv[iRet++] = "/sbin/ifconfig";
418 argv[iRet++] = IF_ETH;
419 argv[iRet++] = sBuffer;
420 argv[iRet] = NULL;
421
422 /* call ifconfig to configure the virtual network interface */
423 iRet = call_usermodehelper(argv[0], argv, envp, 1);
424 printk("ifconfig %s %s returned %d\n", argv[1], argv[2], iRet);
425
426 // start the NMT state machine
427 EplRet = EplApiExecNmtCommand(kEplNmtEventSwReset);
428 atomic_set(&AtomicShutdown_g, FALSE);
429
430 Exit:
431 printk("EplLinInit(): returns 0x%X\n", EplRet);
432 return EplRet;
433 }
434
435 static void __exit EplLinExit(void)
436 {
437 tEplKernel EplRet;
438
439 // halt the NMT state machine
440 // so the processing of POWERLINK frames stops
441 EplRet = EplApiExecNmtCommand(kEplNmtEventSwitchOff);
442
443 // wait until NMT state machine is shut down
444 wait_event_interruptible(WaitQueueShutdown_g,
445 (atomic_read(&AtomicShutdown_g) == TRUE));
446 /* if ((iErr != 0) || (atomic_read(&AtomicShutdown_g) == EVENT_STATE_IOCTL))
447 { // waiting was interrupted by signal or application called wrong function
448 EplRet = kEplShutdown;
449 }*/
450 // delete instance for all modules
451 EplRet = EplApiShutdown();
452 printk("EplApiShutdown(): 0x%X\n", EplRet);
453
454 // deinitialize proc fs
455 EplRet = EplLinProcFree();
456 printk("EplLinProcFree(): 0x%X\n", EplRet);
457
458 }
459
460 //=========================================================================//
461 // //
462 // P R I V A T E F U N C T I O N S //
463 // //
464 //=========================================================================//
465
466 //---------------------------------------------------------------------------
467 //
468 // Function: AppCbEvent
469 //
470 // Description: event callback function called by EPL API layer within
471 // user part (low priority).
472 //
473 // Parameters: EventType_p = event type
474 // pEventArg_p = pointer to union, which describes
475 // the event in detail
476 // pUserArg_p = user specific argument
477 //
478 // Returns: tEplKernel = error code,
479 // kEplSuccessful = no error
480 // kEplReject = reject further processing
481 // otherwise = post error event to API layer
482 //
483 // State:
484 //
485 //---------------------------------------------------------------------------
486
487 tEplKernel PUBLIC AppCbEvent(tEplApiEventType EventType_p, // IN: event type (enum)
488 tEplApiEventArg * pEventArg_p, // IN: event argument (union)
489 void GENERIC * pUserArg_p)
490 {
491 tEplKernel EplRet = kEplSuccessful;
492
493 // check if NMT_GS_OFF is reached
494 switch (EventType_p) {
495 case kEplApiEventNmtStateChange:
496 {
497 switch (pEventArg_p->m_NmtStateChange.m_NewNmtState) {
498 case kEplNmtGsOff:
499 { // NMT state machine was shut down,
500 // because of user signal (CTRL-C) or critical EPL stack error
501 // -> also shut down EplApiProcess() and main()
502 EplRet = kEplShutdown;
503
504 printk
505 ("AppCbEvent(kEplNmtGsOff) originating event = 0x%X\n",
506 pEventArg_p->m_NmtStateChange.
507 m_NmtEvent);
508
509 // wake up EplLinExit()
510 atomic_set(&AtomicShutdown_g, TRUE);
511 wake_up_interruptible
512 (&WaitQueueShutdown_g);
513 break;
514 }
515
516 case kEplNmtGsResetCommunication:
517 {
518 DWORD dwBuffer;
519
520 // configure OD for MN in state ResetComm after reseting the OD
521 // TODO: setup your own network configuration here
522 dwBuffer = (EPL_NODEASSIGN_NODE_IS_CN | EPL_NODEASSIGN_NODE_EXISTS); // 0x00000003L
523 EplRet =
524 EplApiWriteLocalObject(0x1F81, 0x01,
525 &dwBuffer,
526 4);
527 EplRet =
528 EplApiWriteLocalObject(0x1F81, 0x02,
529 &dwBuffer,
530 4);
531 EplRet =
532 EplApiWriteLocalObject(0x1F81, 0x03,
533 &dwBuffer,
534 4);
535 EplRet =
536 EplApiWriteLocalObject(0x1F81, 0x04,
537 &dwBuffer,
538 4);
539 EplRet =
540 EplApiWriteLocalObject(0x1F81, 0x05,
541 &dwBuffer,
542 4);
543 EplRet =
544 EplApiWriteLocalObject(0x1F81, 0x06,
545 &dwBuffer,
546 4);
547 EplRet =
548 EplApiWriteLocalObject(0x1F81, 0x07,
549 &dwBuffer,
550 4);
551 EplRet =
552 EplApiWriteLocalObject(0x1F81, 0x08,
553 &dwBuffer,
554 4);
555 EplRet =
556 EplApiWriteLocalObject(0x1F81, 0x20,
557 &dwBuffer,
558 4);
559 EplRet =
560 EplApiWriteLocalObject(0x1F81, 0xFE,
561 &dwBuffer,
562 4);
563 EplRet =
564 EplApiWriteLocalObject(0x1F81, 0x6E,
565 &dwBuffer,
566 4);
567
568 // dwBuffer |= EPL_NODEASSIGN_MANDATORY_CN; // 0x0000000BL
569 // EplRet = EplApiWriteLocalObject(0x1F81, 0x6E, &dwBuffer, 4);
570 dwBuffer = (EPL_NODEASSIGN_MN_PRES | EPL_NODEASSIGN_NODE_EXISTS); // 0x00010001L
571 EplRet =
572 EplApiWriteLocalObject(0x1F81, 0xF0,
573 &dwBuffer,
574 4);
575
576 // continue
577 }
578
579 case kEplNmtGsResetConfiguration:
580 {
581 unsigned int uiSize;
582
583 // fetch object 0x1006 NMT_CycleLen_U32 from local OD (in little endian byte order)
584 // for configuration of remote CN
585 uiSize = 4;
586 EplRet =
587 EplApiReadObject(NULL, 0, 0x1006,
588 0x00,
589 &dw_le_CycleLen_g,
590 &uiSize,
591 kEplSdoTypeAsnd,
592 NULL);
593 if (EplRet != kEplSuccessful) { // local OD access failed
594 break;
595 }
596 // continue
597 }
598
599 case kEplNmtMsPreOperational1:
600 {
601 printk
602 ("AppCbEvent(0x%X) originating event = 0x%X\n",
603 pEventArg_p->m_NmtStateChange.
604 m_NewNmtState,
605 pEventArg_p->m_NmtStateChange.
606 m_NmtEvent);
607
608 // continue
609 }
610
611 case kEplNmtGsInitialising:
612 case kEplNmtGsResetApplication:
613 case kEplNmtMsNotActive:
614 case kEplNmtCsNotActive:
615 case kEplNmtCsPreOperational1:
616 {
617 break;
618 }
619
620 case kEplNmtCsOperational:
621 case kEplNmtMsOperational:
622 {
623 break;
624 }
625
626 default:
627 {
628 break;
629 }
630 }
631
632 /*
633 switch (pEventArg_p->m_NmtStateChange.m_NmtEvent)
634 {
635 case kEplNmtEventSwReset:
636 case kEplNmtEventResetNode:
637 case kEplNmtEventResetCom:
638 case kEplNmtEventResetConfig:
639 case kEplNmtEventInternComError:
640 case kEplNmtEventNmtCycleError:
641 {
642 printk("AppCbEvent(0x%X) originating event = 0x%X\n",
643 pEventArg_p->m_NmtStateChange.m_NewNmtState,
644 pEventArg_p->m_NmtStateChange.m_NmtEvent);
645 break;
646 }
647
648 default:
649 {
650 break;
651 }
652 }
653 */
654 break;
655 }
656
657 case kEplApiEventCriticalError:
658 case kEplApiEventWarning:
659 { // error or warning occured within the stack or the application
660 // on error the API layer stops the NMT state machine
661
662 printk
663 ("AppCbEvent(Err/Warn): Source=%02X EplError=0x%03X",
664 pEventArg_p->m_InternalError.m_EventSource,
665 pEventArg_p->m_InternalError.m_EplError);
666 // check additional argument
667 switch (pEventArg_p->m_InternalError.m_EventSource) {
668 case kEplEventSourceEventk:
669 case kEplEventSourceEventu:
670 { // error occured within event processing
671 // either in kernel or in user part
672 printk(" OrgSource=%02X\n",
673 pEventArg_p->m_InternalError.
674 m_Arg.m_EventSource);
675 break;
676 }
677
678 case kEplEventSourceDllk:
679 { // error occured within the data link layer (e.g. interrupt processing)
680 // the DWORD argument contains the DLL state and the NMT event
681 printk(" val=%lX\n",
682 pEventArg_p->m_InternalError.
683 m_Arg.m_dwArg);
684 break;
685 }
686
687 default:
688 {
689 printk("\n");
690 break;
691 }
692 }
693 break;
694 }
695
696 case kEplApiEventNode:
697 {
698 // printk("AppCbEvent(Node): Source=%02X EplError=0x%03X", pEventArg_p->m_InternalError.m_EventSource, pEventArg_p->m_InternalError.m_EplError);
699 // check additional argument
700 switch (pEventArg_p->m_Node.m_NodeEvent) {
701 case kEplNmtNodeEventCheckConf:
702 {
703 tEplSdoComConHdl SdoComConHdl;
704 // update object 0x1006 on CN
705 EplRet =
706 EplApiWriteObject(&SdoComConHdl,
707 pEventArg_p->
708 m_Node.m_uiNodeId,
709 0x1006, 0x00,
710 &dw_le_CycleLen_g,
711 4,
712 kEplSdoTypeAsnd,
713 NULL);
714 if (EplRet == kEplApiTaskDeferred) { // SDO transfer started
715 EplRet = kEplReject;
716 } else if (EplRet == kEplSuccessful) { // local OD access (should not occur)
717 printk
718 ("AppCbEvent(Node) write to local OD\n");
719 } else { // error occured
720 TGT_DBG_SIGNAL_TRACE_POINT(1);
721
722 EplRet =
723 EplApiFreeSdoChannel
724 (SdoComConHdl);
725 SdoComConHdl = 0;
726
727 EplRet =
728 EplApiWriteObject
729 (&SdoComConHdl,
730 pEventArg_p->m_Node.
731 m_uiNodeId, 0x1006, 0x00,
732 &dw_le_CycleLen_g, 4,
733 kEplSdoTypeAsnd, NULL);
734 if (EplRet == kEplApiTaskDeferred) { // SDO transfer started
735 EplRet = kEplReject;
736 } else {
737 printk
738 ("AppCbEvent(Node): EplApiWriteObject() returned 0x%02X\n",
739 EplRet);
740 }
741 }
742
743 break;
744 }
745
746 default:
747 {
748 break;
749 }
750 }
751 break;
752 }
753
754 case kEplApiEventSdo:
755 { // SDO transfer finished
756 EplRet =
757 EplApiFreeSdoChannel(pEventArg_p->m_Sdo.
758 m_SdoComConHdl);
759 if (EplRet != kEplSuccessful) {
760 break;
761 }
762 #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
763 if (pEventArg_p->m_Sdo.m_SdoComConState == kEplSdoComTransferFinished) { // continue boot-up of CN with NMT command Reset Configuration
764 EplRet =
765 EplApiMnTriggerStateChange(pEventArg_p->
766 m_Sdo.m_uiNodeId,
767 kEplNmtNodeCommandConfReset);
768 } else { // indicate configuration error CN
769 EplRet =
770 EplApiMnTriggerStateChange(pEventArg_p->
771 m_Sdo.m_uiNodeId,
772 kEplNmtNodeCommandConfErr);
773 }
774 #endif
775
776 break;
777 }
778
779 default:
780 break;
781 }
782
783 return EplRet;
784 }
785
786 //---------------------------------------------------------------------------
787 //
788 // Function: AppCbSync
789 //
790 // Description: sync event callback function called by event module within
791 // kernel part (high priority).
792 // This function sets the outputs, reads the inputs and runs
793 // the control loop.
794 //
795 // Parameters: void
796 //
797 // Returns: tEplKernel = error code,
798 // kEplSuccessful = no error
799 // otherwise = post error event to API layer
800 //
801 // State:
802 //
803 //---------------------------------------------------------------------------
804
805 tEplKernel PUBLIC AppCbSync(void)
806 {
807 tEplKernel EplRet = kEplSuccessful;
808
809 if (bVarOut1Old_l != bVarOut1_l) { // output variable has changed
810 bVarOut1Old_l = bVarOut1_l;
811 // set LEDs
812
813 // printk("bVarIn = 0x%02X bVarOut = 0x%02X\n", (WORD) bVarIn_l, (WORD) bVarOut_l);
814 }
815 if (uiNodeId_g != EPL_C_ADR_MN_DEF_NODE_ID) {
816 bVarIn1_l++;
817 }
818
819 if (uiNodeId_g == EPL_C_ADR_MN_DEF_NODE_ID) { // we are the master and must run the control loop
820
821 // collect inputs from CNs and own input
822 bSpeedSelect_l = (bVarIn1_l | abSelect_l[0]) & 0x07;
823
824 bModeSelect_l = abSelect_l[1] | abSelect_l[2];
825
826 if ((bModeSelect_l & APP_MODE_MASK) != 0) {
827 dwMode_l = bModeSelect_l & APP_MODE_MASK;
828 }
829
830 iCurCycleCount_l--;
831
832 if (iCurCycleCount_l <= 0) {
833 if ((dwMode_l & 0x01) != 0) { // fill-up
834 if (iToggle) {
835 if ((dwLeds_l & APP_DOUBLE_LED_MASK) ==
836 0x00) {
837 dwLeds_l = 0x01;
838 } else {
839 dwLeds_l <<= 1;
840 dwLeds_l++;
841 if (dwLeds_l >=
842 APP_DOUBLE_LED_MASK) {
843 iToggle = 0;
844 }
845 }
846 } else {
847 dwLeds_l <<= 1;
848 if ((dwLeds_l & APP_DOUBLE_LED_MASK) ==
849 0x00) {
850 iToggle = 1;
851 }
852 }
853 bLedsRow1_l =
854 (unsigned char)(dwLeds_l & APP_LED_MASK);
855 bLedsRow2_l =
856 (unsigned char)((dwLeds_l >> APP_LED_COUNT)
857 & APP_LED_MASK);
858 }
859
860 else if ((dwMode_l & 0x02) != 0) { // running light forward
861 dwLeds_l <<= 1;
862 if ((dwLeds_l > APP_DOUBLE_LED_MASK)
863 || (dwLeds_l == 0x00000000L)) {
864 dwLeds_l = 0x01;
865 }
866 bLedsRow1_l =
867 (unsigned char)(dwLeds_l & APP_LED_MASK);
868 bLedsRow2_l =
869 (unsigned char)((dwLeds_l >> APP_LED_COUNT)
870 & APP_LED_MASK);
871 }
872
873 else if ((dwMode_l & 0x04) != 0) { // running light backward
874 dwLeds_l >>= 1;
875 if ((dwLeds_l > APP_DOUBLE_LED_MASK)
876 || (dwLeds_l == 0x00000000L)) {
877 dwLeds_l = 1 << (APP_LED_COUNT * 2);
878 }
879 bLedsRow1_l =
880 (unsigned char)(dwLeds_l & APP_LED_MASK);
881 bLedsRow2_l =
882 (unsigned char)((dwLeds_l >> APP_LED_COUNT)
883 & APP_LED_MASK);
884 }
885
886 else if ((dwMode_l & 0x08) != 0) { // Knightrider
887 if (bLedsRow1_l == 0x00) {
888 bLedsRow1_l = 0x01;
889 iToggle = 1;
890 } else if (iToggle) {
891 bLedsRow1_l <<= 1;
892 if (bLedsRow1_l >=
893 (1 << (APP_LED_COUNT - 1))) {
894 iToggle = 0;
895 }
896 } else {
897 bLedsRow1_l >>= 1;
898 if (bLedsRow1_l <= 0x01) {
899 iToggle = 1;
900 }
901 }
902 bLedsRow2_l = bLedsRow1_l;
903 }
904
905 else if ((dwMode_l & 0x10) != 0) { // Knightrider
906 if ((bLedsRow1_l == 0x00)
907 || (bLedsRow2_l == 0x00)
908 || ((bLedsRow2_l & ~APP_LED_MASK) != 0)) {
909 bLedsRow1_l = 0x01;
910 bLedsRow2_l =
911 (1 << (APP_LED_COUNT - 1));
912 iToggle = 1;
913 } else if (iToggle) {
914 bLedsRow1_l <<= 1;
915 bLedsRow2_l >>= 1;
916 if (bLedsRow1_l >=
917 (1 << (APP_LED_COUNT - 1))) {
918 iToggle = 0;
919 }
920 } else {
921 bLedsRow1_l >>= 1;
922 bLedsRow2_l <<= 1;
923 if (bLedsRow1_l <= 0x01) {
924 iToggle = 1;
925 }
926 }
927 }
928 // set own output
929 bVarOut1_l = bLedsRow1_l;
930 // bVarOut1_l = (bLedsRow1_l & 0x03) | (bLedsRow2_l << 2);
931
932 // restart cycle counter
933 iCurCycleCount_l = iMaxCycleCount_l;
934 }
935
936 if (bSpeedSelectOld_l == 0) {
937 if ((bSpeedSelect_l & 0x01) != 0) {
938 if (iMaxCycleCount_l < 200) {
939 iMaxCycleCount_l++;
940 }
941 bSpeedSelectOld_l = bSpeedSelect_l;
942 } else if ((bSpeedSelect_l & 0x02) != 0) {
943 if (iMaxCycleCount_l > 1) {
944 iMaxCycleCount_l--;
945 }
946 bSpeedSelectOld_l = bSpeedSelect_l;
947 } else if ((bSpeedSelect_l & 0x04) != 0) {
948 iMaxCycleCount_l = DEFAULT_MAX_CYCLE_COUNT;
949 bSpeedSelectOld_l = bSpeedSelect_l;
950 }
951 } else if (bSpeedSelect_l == 0) {
952 bSpeedSelectOld_l = 0;
953 }
954 }
955
956 TGT_DBG_SIGNAL_TRACE_POINT(1);
957
958 return EplRet;
959 }
960
961 // EOF
Nao low-level kernelspace/userspace library that runs on our robots, Nao-Team HTWK