git.oblomov.eu Git - linux-2.6/blob - 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