Merge branch 'for-linus' of git://git.kernel.org/pub/scm/linux/kernel/git/roland...
[linux-2.6] / drivers / staging / epl / EplPdou.c
1 /****************************************************************************
2
3   (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
4       www.systec-electronic.com
5
6   Project:      openPOWERLINK
7
8   Description:  source file for user PDO module
9                 Currently, this module just implements a OD callback function
10                 to check if the PDO configuration is valid.
11
12   License:
13
14     Redistribution and use in source and binary forms, with or without
15     modification, are permitted provided that the following conditions
16     are met:
17
18     1. Redistributions of source code must retain the above copyright
19        notice, this list of conditions and the following disclaimer.
20
21     2. Redistributions in binary form must reproduce the above copyright
22        notice, this list of conditions and the following disclaimer in the
23        documentation and/or other materials provided with the distribution.
24
25     3. Neither the name of SYSTEC electronic GmbH nor the names of its
26        contributors may be used to endorse or promote products derived
27        from this software without prior written permission. For written
28        permission, please contact info@systec-electronic.com.
29
30     THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
31     "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
32     LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
33     FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
34     COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
35     INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
36     BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
37     LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
38     CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
39     LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
40     ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
41     POSSIBILITY OF SUCH DAMAGE.
42
43     Severability Clause:
44
45         If a provision of this License is or becomes illegal, invalid or
46         unenforceable in any jurisdiction, that shall not affect:
47         1. the validity or enforceability in that jurisdiction of any other
48            provision of this License; or
49         2. the validity or enforceability in other jurisdictions of that or
50            any other provision of this License.
51
52   -------------------------------------------------------------------------
53
54                 $RCSfile: EplPdou.c,v $
55
56                 $Author: D.Krueger $
57
58                 $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
59
60                 $State: Exp $
61
62                 Build Environment:
63                     GCC V3.4
64
65   -------------------------------------------------------------------------
66
67   Revision History:
68
69   2006/05/22 d.k.:   start of the implementation, version 1.00
70
71 ****************************************************************************/
72
73 #include "EplInc.h"
74 //#include "user/EplPdouCal.h"
75 #include "user/EplObdu.h"
76 #include "user/EplPdou.h"
77 #include "EplSdoAc.h"
78
79 #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOU)) != 0)
80
81 #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) == 0) && (EPL_OBD_USE_KERNEL == FALSE)
82 #error "EPL PDOu module needs EPL module OBDU or OBDK!"
83 #endif
84
85 /***************************************************************************/
86 /*                                                                         */
87 /*                                                                         */
88 /*          G L O B A L   D E F I N I T I O N S                            */
89 /*                                                                         */
90 /*                                                                         */
91 /***************************************************************************/
92
93 //---------------------------------------------------------------------------
94 // const defines
95 //---------------------------------------------------------------------------
96
97 #define EPL_PDOU_OBD_IDX_RX_COMM_PARAM  0x1400
98 #define EPL_PDOU_OBD_IDX_RX_MAPP_PARAM  0x1600
99 #define EPL_PDOU_OBD_IDX_TX_COMM_PARAM  0x1800
100 #define EPL_PDOU_OBD_IDX_TX_MAPP_PARAM  0x1A00
101 #define EPL_PDOU_OBD_IDX_MAPP_PARAM     0x0200
102 #define EPL_PDOU_OBD_IDX_MASK           0xFF00
103 #define EPL_PDOU_PDO_ID_MASK            0x00FF
104
105 //---------------------------------------------------------------------------
106 // local types
107 //---------------------------------------------------------------------------
108
109 //---------------------------------------------------------------------------
110 // modul globale vars
111 //---------------------------------------------------------------------------
112
113 //---------------------------------------------------------------------------
114 // local function prototypes
115 //---------------------------------------------------------------------------
116
117 /***************************************************************************/
118 /*                                                                         */
119 /*                                                                         */
120 /*          C L A S S  EplPdou                                             */
121 /*                                                                         */
122 /*                                                                         */
123 /***************************************************************************/
124 //
125 // Description:
126 //
127 //
128 /***************************************************************************/
129
130 //=========================================================================//
131 //                                                                         //
132 //          P R I V A T E   D E F I N I T I O N S                          //
133 //                                                                         //
134 //=========================================================================//
135
136 //---------------------------------------------------------------------------
137 // const defines
138 //---------------------------------------------------------------------------
139
140 //---------------------------------------------------------------------------
141 // local types
142 //---------------------------------------------------------------------------
143
144 //---------------------------------------------------------------------------
145 // local vars
146 //---------------------------------------------------------------------------
147
148 //---------------------------------------------------------------------------
149 // local function prototypes
150 //---------------------------------------------------------------------------
151
152 static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam *pParam_p,
153                                           unsigned int uiIndex_p);
154
155 static void EplPdouDecodeObjectMapping(u64 qwObjectMapping_p,
156                                        unsigned int *puiIndex_p,
157                                        unsigned int *puiSubIndex_p,
158                                        unsigned int *puiBitOffset_p,
159                                        unsigned int *puiBitSize_p);
160
161 static tEplKernel EplPdouCheckObjectMapping(u64 qwObjectMapping_p,
162                                             tEplObdAccess AccessType_p,
163                                             u32 * pdwAbortCode_p,
164                                             unsigned int *puiPdoSize_p);
165
166 //=========================================================================//
167 //                                                                         //
168 //          P U B L I C   F U N C T I O N S                                //
169 //                                                                         //
170 //=========================================================================//
171
172 //---------------------------------------------------------------------------
173 //
174 // Function:    EplPdouAddInstance()
175 //
176 // Description: add and initialize new instance of EPL stack
177 //
178 // Parameters:  none
179 //
180 // Returns:     tEplKernel              = error code
181 //
182 //
183 // State:
184 //
185 //---------------------------------------------------------------------------
186
187 tEplKernel EplPdouAddInstance(void)
188 {
189
190         return kEplSuccessful;
191 }
192
193 //---------------------------------------------------------------------------
194 //
195 // Function:    EplPdouDelInstance()
196 //
197 // Description: deletes an instance of EPL stack
198 //
199 // Parameters:  none
200 //
201 // Returns:     tEplKernel              = error code
202 //
203 //
204 // State:
205 //
206 //---------------------------------------------------------------------------
207
208 tEplKernel EplPdouDelInstance(void)
209 {
210
211         return kEplSuccessful;
212 }
213
214 //---------------------------------------------------------------------------
215 //
216 // Function:    EplPdouCbObdAccess
217 //
218 // Description: callback function for OD accesses
219 //
220 // Parameters:  pParam_p                = OBD parameter
221 //
222 // Returns:     tEplKernel              = error code
223 //
224 //
225 // State:
226 //
227 //---------------------------------------------------------------------------
228
229 tEplKernel EplPdouCbObdAccess(tEplObdCbParam *pParam_p)
230 {
231         tEplKernel Ret = kEplSuccessful;
232         unsigned int uiPdoId;
233         unsigned int uiIndexType;
234         tEplObdSize ObdSize;
235         u8 bObjectCount;
236         u64 qwObjectMapping;
237         tEplObdAccess AccessType;
238         u8 bMappSubindex;
239         unsigned int uiCurPdoSize;
240         u16 wMaxPdoSize;
241         unsigned int uiSubIndex;
242
243         // fetch PDO ID
244         uiPdoId = pParam_p->m_uiIndex & EPL_PDOU_PDO_ID_MASK;
245
246         // fetch object index type
247         uiIndexType = pParam_p->m_uiIndex & EPL_PDOU_OBD_IDX_MASK;
248
249         if (pParam_p->m_ObdEvent != kEplObdEvPreWrite) {        // read accesses, post write events etc. are OK
250                 pParam_p->m_dwAbortCode = 0;
251                 goto Exit;
252         }
253         // check index type
254         switch (uiIndexType) {
255         case EPL_PDOU_OBD_IDX_RX_COMM_PARAM:
256                 // RPDO communication parameter accessed
257         case EPL_PDOU_OBD_IDX_TX_COMM_PARAM:
258                 {               // TPDO communication parameter accessed
259                         Ret = EplPdouCheckPdoValidity(pParam_p,
260                                                       (EPL_PDOU_OBD_IDX_MAPP_PARAM
261                                                        | pParam_p->m_uiIndex));
262                         if (Ret != kEplSuccessful) {    // PDO is valid or does not exist
263                                 goto Exit;
264                         }
265
266                         goto Exit;
267                 }
268
269         case EPL_PDOU_OBD_IDX_RX_MAPP_PARAM:
270                 {               // RPDO mapping parameter accessed
271
272                         AccessType = kEplObdAccWrite;
273                         break;
274                 }
275
276         case EPL_PDOU_OBD_IDX_TX_MAPP_PARAM:
277                 {               // TPDO mapping parameter accessed
278
279                         AccessType = kEplObdAccRead;
280                         break;
281                 }
282
283         default:
284                 {               // this callback function is only for
285                         // PDO mapping and communication parameters
286                         pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR;
287                         goto Exit;
288                 }
289         }
290
291         // RPDO and TPDO mapping parameter accessed
292
293         if (pParam_p->m_uiSubIndex == 0) {      // object mapping count accessed
294
295                 // PDO is enabled or disabled
296                 bObjectCount = *((u8 *) pParam_p->m_pArg);
297
298                 if (bObjectCount == 0) {        // PDO shall be disabled
299
300                         // that is always possible
301                         goto Exit;
302                 }
303                 // PDO shall be enabled
304                 // it should have been disabled for this operation
305                 Ret = EplPdouCheckPdoValidity(pParam_p, pParam_p->m_uiIndex);
306                 if (Ret != kEplSuccessful) {    // PDO is valid or does not exist
307                         goto Exit;
308                 }
309
310                 if (AccessType == kEplObdAccWrite) {
311                         uiSubIndex = 0x04;      // PReqActPayloadLimit_U16
312                 } else {
313                         uiSubIndex = 0x05;      // PResActPayloadLimit_U16
314                 }
315
316                 // fetch maximum PDO size from Object 1F98h: NMT_CycleTiming_REC
317                 ObdSize = sizeof(wMaxPdoSize);
318                 Ret =
319                     EplObduReadEntry(0x1F98, uiSubIndex, &wMaxPdoSize,
320                                      &ObdSize);
321                 if (Ret != kEplSuccessful) {    // other fatal error occured
322                         pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR;
323                         goto Exit;
324                 }
325                 // check all objectmappings
326                 for (bMappSubindex = 1; bMappSubindex <= bObjectCount;
327                      bMappSubindex++) {
328                         // read object mapping from OD
329                         ObdSize = sizeof(qwObjectMapping);      // u64
330                         Ret = EplObduReadEntry(pParam_p->m_uiIndex,
331                                                bMappSubindex, &qwObjectMapping,
332                                                &ObdSize);
333                         if (Ret != kEplSuccessful) {    // other fatal error occured
334                                 pParam_p->m_dwAbortCode =
335                                     EPL_SDOAC_GENERAL_ERROR;
336                                 goto Exit;
337                         }
338                         // check object mapping
339                         Ret = EplPdouCheckObjectMapping(qwObjectMapping,
340                                                         AccessType,
341                                                         &pParam_p->
342                                                         m_dwAbortCode,
343                                                         &uiCurPdoSize);
344                         if (Ret != kEplSuccessful) {    // illegal object mapping
345                                 goto Exit;
346                         }
347
348                         if (uiCurPdoSize > wMaxPdoSize) {       // mapping exceeds object size
349                                 pParam_p->m_dwAbortCode =
350                                     EPL_SDOAC_GENERAL_ERROR;
351                                 Ret = kEplPdoVarNotFound;
352                         }
353
354                 }
355
356         } else {                // ObjectMapping
357                 Ret = EplPdouCheckPdoValidity(pParam_p, pParam_p->m_uiIndex);
358                 if (Ret != kEplSuccessful) {    // PDO is valid or does not exist
359                         goto Exit;
360                 }
361                 // check existence of object and validity of object length
362
363                 qwObjectMapping = *((u64 *) pParam_p->m_pArg);
364
365                 Ret = EplPdouCheckObjectMapping(qwObjectMapping,
366                                                 AccessType,
367                                                 &pParam_p->m_dwAbortCode,
368                                                 &uiCurPdoSize);
369
370         }
371
372       Exit:
373         return Ret;
374 }
375
376 //=========================================================================//
377 //                                                                         //
378 //          P R I V A T E   F U N C T I O N S                              //
379 //                                                                         //
380 //=========================================================================//
381
382 //---------------------------------------------------------------------------
383 //
384 // Function:    EplPdouCheckPdoValidity
385 //
386 // Description: check if PDO is valid
387 //
388 // Parameters:  pParam_p                = OBD parameter
389 //
390 // Returns:     tEplKernel              = error code
391 //
392 //
393 // State:
394 //
395 //---------------------------------------------------------------------------
396
397 static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam *pParam_p,
398                                           unsigned int uiIndex_p)
399 {
400         tEplKernel Ret = kEplSuccessful;
401         tEplObdSize ObdSize;
402         u8 bObjectCount;
403
404         ObdSize = 1;
405         // read number of mapped objects from OD; this indicates if the PDO is valid
406         Ret = EplObduReadEntry(uiIndex_p, 0x00, &bObjectCount, &ObdSize);
407         if (Ret != kEplSuccessful) {    // other fatal error occured
408                 pParam_p->m_dwAbortCode =
409                     EPL_SDOAC_GEN_INTERNAL_INCOMPATIBILITY;
410                 goto Exit;
411         }
412         // entry read successfully
413         if (bObjectCount != 0) {        // PDO in OD is still valid
414                 pParam_p->m_dwAbortCode = EPL_SDOAC_GEN_PARAM_INCOMPATIBILITY;
415                 Ret = kEplPdoNotExist;
416                 goto Exit;
417         }
418
419       Exit:
420         return Ret;
421 }
422
423 //---------------------------------------------------------------------------
424 //
425 // Function:    EplPdouDecodeObjectMapping
426 //
427 // Description: decodes the given object mapping entry into index, subindex,
428 //              bit offset and bit size.
429 //
430 // Parameters:  qwObjectMapping_p       = object mapping entry
431 //              puiIndex_p              = [OUT] pointer to object index
432 //              puiSubIndex_p           = [OUT] pointer to subindex
433 //              puiBitOffset_p          = [OUT] pointer to bit offset
434 //              puiBitSize_p            = [OUT] pointer to bit size
435 //
436 // Returns:     (void)
437 //
438 // State:
439 //
440 //---------------------------------------------------------------------------
441
442 static void EplPdouDecodeObjectMapping(u64 qwObjectMapping_p,
443                                        unsigned int *puiIndex_p,
444                                        unsigned int *puiSubIndex_p,
445                                        unsigned int *puiBitOffset_p,
446                                        unsigned int *puiBitSize_p)
447 {
448         *puiIndex_p = (unsigned int)
449             (qwObjectMapping_p & 0x000000000000FFFFLL);
450
451         *puiSubIndex_p = (unsigned int)
452             ((qwObjectMapping_p & 0x0000000000FF0000LL) >> 16);
453
454         *puiBitOffset_p = (unsigned int)
455             ((qwObjectMapping_p & 0x0000FFFF00000000LL) >> 32);
456
457         *puiBitSize_p = (unsigned int)
458             ((qwObjectMapping_p & 0xFFFF000000000000LL) >> 48);
459
460 }
461
462 //---------------------------------------------------------------------------
463 //
464 // Function:    EplPdouCheckObjectMapping
465 //
466 // Description: checks the given object mapping entry.
467 //
468 // Parameters:  qwObjectMapping_p       = object mapping entry
469 //              AccessType_p            = access type to mapped object:
470 //                                        write = RPDO and read = TPDO
471 //              puiPdoSize_p            = [OUT] pointer to covered PDO size
472 //                                        (offset + size) in byte;
473 //                                        0 if mapping failed
474 //              pdwAbortCode_p          = [OUT] pointer to SDO abort code;
475 //                                        0 if mapping is possible
476 //
477 // Returns:     tEplKernel              = error code
478 //
479 // State:
480 //
481 //---------------------------------------------------------------------------
482
483 static tEplKernel EplPdouCheckObjectMapping(u64 qwObjectMapping_p,
484                                             tEplObdAccess AccessType_p,
485                                             u32 * pdwAbortCode_p,
486                                             unsigned int *puiPdoSize_p)
487 {
488         tEplKernel Ret = kEplSuccessful;
489         tEplObdSize ObdSize;
490         unsigned int uiIndex;
491         unsigned int uiSubIndex;
492         unsigned int uiBitOffset;
493         unsigned int uiBitSize;
494         tEplObdAccess AccessType;
495         BOOL fNumerical;
496
497         if (qwObjectMapping_p == 0) {   // discard zero value
498                 *puiPdoSize_p = 0;
499                 goto Exit;
500         }
501         // decode object mapping
502         EplPdouDecodeObjectMapping(qwObjectMapping_p,
503                                    &uiIndex,
504                                    &uiSubIndex, &uiBitOffset, &uiBitSize);
505
506         if ((uiBitOffset & 0x7) != 0x0) {       // bit mapping is not supported
507                 *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
508                 Ret = kEplPdoGranularityMismatch;
509                 goto Exit;
510         }
511
512         if ((uiBitSize & 0x7) != 0x0) { // bit mapping is not supported
513                 *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
514                 Ret = kEplPdoGranularityMismatch;
515                 goto Exit;
516         }
517         // check access type
518         Ret = EplObduGetAccessType(uiIndex, uiSubIndex, &AccessType);
519         if (Ret != kEplSuccessful) {    // entry doesn't exist
520                 *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_EXIST;
521                 goto Exit;
522         }
523
524         if ((AccessType & kEplObdAccPdo) == 0) {        // object is not mappable
525                 *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_MAPPABLE;
526                 Ret = kEplPdoVarNotFound;
527                 goto Exit;
528         }
529
530         if ((AccessType & AccessType_p) == 0) { // object is not writeable (RPDO) or readable (TPDO) respectively
531                 *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_MAPPABLE;
532                 Ret = kEplPdoVarNotFound;
533                 goto Exit;
534         }
535
536         ObdSize = EplObduGetDataSize(uiIndex, uiSubIndex);
537         if (ObdSize < (uiBitSize >> 3)) {       // object does not exist or has smaller size
538                 *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
539                 Ret = kEplPdoVarNotFound;
540         }
541
542         Ret = EplObduIsNumerical(uiIndex, uiSubIndex, &fNumerical);
543         if (Ret != kEplSuccessful) {    // entry doesn't exist
544                 *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_EXIST;
545                 goto Exit;
546         }
547
548         if ((fNumerical != FALSE)
549             && ((uiBitSize >> 3) != ObdSize)) {
550                 // object is numerical,
551                 // therefor size has to fit, but it does not.
552                 *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
553                 Ret = kEplPdoVarNotFound;
554                 goto Exit;
555         }
556         // calucaled needed PDO size
557         *puiPdoSize_p = (uiBitOffset >> 3) + (uiBitSize >> 3);
558
559       Exit:
560         return Ret;
561 }
562
563 #endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOU)) != 0)
564
565 // EOF