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 /*                                                                         */
  89 /*          G L O B A L   D E F I N I T I O N S                            */
  90 /*                                                                         */
  91 /*                                                                         */
  92 /***************************************************************************/
  93
  94 //---------------------------------------------------------------------------
  95 // const defines
  96 //---------------------------------------------------------------------------
  97
  98 #define EPL_PDOU_OBD_IDX_RX_COMM_PARAM  0x1400
  99 #define EPL_PDOU_OBD_IDX_RX_MAPP_PARAM  0x1600
 100 #define EPL_PDOU_OBD_IDX_TX_COMM_PARAM  0x1800
 101 #define EPL_PDOU_OBD_IDX_TX_MAPP_PARAM  0x1A00
 102 #define EPL_PDOU_OBD_IDX_MAPP_PARAM     0x0200
 103 #define EPL_PDOU_OBD_IDX_MASK           0xFF00
 104 #define EPL_PDOU_PDO_ID_MASK            0x00FF
 105
 106
 107 //---------------------------------------------------------------------------
 108 // local types
 109 //---------------------------------------------------------------------------
 110
 111 //---------------------------------------------------------------------------
 112 // modul globale vars
 113 //---------------------------------------------------------------------------
 114
 115 //---------------------------------------------------------------------------
 116 // local function prototypes
 117 //---------------------------------------------------------------------------
 118
 119
 120 /***************************************************************************/
 121 /*                                                                         */
 122 /*                                                                         */
 123 /*          C L A S S  EplPdou                                             */
 124 /*                                                                         */
 125 /*                                                                         */
 126 /***************************************************************************/
 127 //
 128 // Description:
 129 //
 130 //
 131 /***************************************************************************/
 132
 133
 134 //=========================================================================//
 135 //                                                                         //
 136 //          P R I V A T E   D E F I N I T I O N S                          //
 137 //                                                                         //
 138 //=========================================================================//
 139
 140 //---------------------------------------------------------------------------
 141 // const defines
 142 //---------------------------------------------------------------------------
 143
 144 //---------------------------------------------------------------------------
 145 // local types
 146 //---------------------------------------------------------------------------
 147
 148 //---------------------------------------------------------------------------
 149 // local vars
 150 //---------------------------------------------------------------------------
 151
 152
 153 //---------------------------------------------------------------------------
 154 // local function prototypes
 155 //---------------------------------------------------------------------------
 156
 157 static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam MEM* pParam_p, unsigned int uiIndex_p);
 158
 159 static void EplPdouDecodeObjectMapping(QWORD qwObjectMapping_p,
 160                                     unsigned int* puiIndex_p,
 161                                     unsigned int* puiSubIndex_p,
 162                                     unsigned int* puiBitOffset_p,
 163                                     unsigned int* puiBitSize_p);
 164
 165 static tEplKernel EplPdouCheckObjectMapping(QWORD qwObjectMapping_p,
 166                                        tEplObdAccess AccessType_p,
 167                                        DWORD* pdwAbortCode_p,
 168                                        unsigned int* puiPdoSize_p);
 169
 170
 171
 172 //=========================================================================//
 173 //                                                                         //
 174 //          P U B L I C   F U N C T I O N S                                //
 175 //                                                                         //
 176 //=========================================================================//
 177
 178 //---------------------------------------------------------------------------
 179 //
 180 // Function:    EplPdouAddInstance()
 181 //
 182 // Description: add and initialize new instance of EPL stack
 183 //
 184 // Parameters:  none
 185 //
 186 // Returns:     tEplKernel              = error code
 187 //
 188 //
 189 // State:
 190 //
 191 //---------------------------------------------------------------------------
 192
 193 tEplKernel EplPdouAddInstance(void)
 194 {
 195
 196     return kEplSuccessful;
 197 }
 198
 199
 200 //---------------------------------------------------------------------------
 201 //
 202 // Function:    EplPdouDelInstance()
 203 //
 204 // Description: deletes an instance of EPL stack
 205 //
 206 // Parameters:  none
 207 //
 208 // Returns:     tEplKernel              = error code
 209 //
 210 //
 211 // State:
 212 //
 213 //---------------------------------------------------------------------------
 214
 215 tEplKernel EplPdouDelInstance(void)
 216 {
 217
 218     return kEplSuccessful;
 219 }
 220
 221
 222 //---------------------------------------------------------------------------
 223 //
 224 // Function:    EplPdouCbObdAccess
 225 //
 226 // Description: callback function for OD accesses
 227 //
 228 // Parameters:  pParam_p                = OBD parameter
 229 //
 230 // Returns:     tEplKernel              = error code
 231 //
 232 //
 233 // State:
 234 //
 235 //---------------------------------------------------------------------------
 236
 237 tEplKernel PUBLIC EplPdouCbObdAccess(tEplObdCbParam MEM* pParam_p)
 238 {
 239 tEplKernel          Ret = kEplSuccessful;
 240 unsigned int        uiPdoId;
 241 unsigned int        uiIndexType;
 242 tEplObdSize         ObdSize;
 243 BYTE                bObjectCount;
 244 QWORD               qwObjectMapping;
 245 tEplObdAccess       AccessType;
 246 BYTE                bMappSubindex;
 247 unsigned int        uiCurPdoSize;
 248 WORD                wMaxPdoSize;
 249 unsigned int        uiSubIndex;
 250
 251     // fetch PDO ID
 252     uiPdoId = pParam_p->m_uiIndex & EPL_PDOU_PDO_ID_MASK;
 253
 254     // fetch object index type
 255     uiIndexType = pParam_p->m_uiIndex & EPL_PDOU_OBD_IDX_MASK;
 256
 257     if (pParam_p->m_ObdEvent != kEplObdEvPreWrite)
 258     {   // read accesses, post write events etc. are OK
 259         pParam_p->m_dwAbortCode = 0;
 260         goto Exit;
 261     }
 262
 263     // check index type
 264     switch (uiIndexType)
 265     {
 266         case EPL_PDOU_OBD_IDX_RX_COMM_PARAM:
 267             // RPDO communication parameter accessed
 268         case EPL_PDOU_OBD_IDX_TX_COMM_PARAM:
 269         {   // TPDO communication parameter accessed
 270             Ret = EplPdouCheckPdoValidity(pParam_p,
 271                     (EPL_PDOU_OBD_IDX_MAPP_PARAM | pParam_p->m_uiIndex));
 272             if (Ret != kEplSuccessful)
 273             {   // PDO is valid or does not exist
 274                 goto Exit;
 275             }
 276
 277             goto Exit;
 278         }
 279
 280         case EPL_PDOU_OBD_IDX_RX_MAPP_PARAM:
 281         {   // RPDO mapping parameter accessed
 282
 283             AccessType = kEplObdAccWrite;
 284             break;
 285         }
 286
 287         case EPL_PDOU_OBD_IDX_TX_MAPP_PARAM:
 288         {   // TPDO mapping parameter accessed
 289
 290             AccessType = kEplObdAccRead;
 291             break;
 292         }
 293
 294         default:
 295         {   // this callback function is only for
 296             // PDO mapping and communication parameters
 297             pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR;
 298             goto Exit;
 299         }
 300     }
 301
 302     // RPDO and TPDO mapping parameter accessed
 303
 304     if (pParam_p->m_uiSubIndex == 0)
 305     {   // object mapping count accessed
 306
 307         // PDO is enabled or disabled
 308         bObjectCount = *((BYTE*) pParam_p->m_pArg);
 309
 310         if (bObjectCount == 0)
 311         {   // PDO shall be disabled
 312
 313             // that is always possible
 314             goto Exit;
 315         }
 316
 317         // PDO shall be enabled
 318         // it should have been disabled for this operation
 319         Ret = EplPdouCheckPdoValidity(pParam_p, pParam_p->m_uiIndex);
 320         if (Ret != kEplSuccessful)
 321         {   // PDO is valid or does not exist
 322             goto Exit;
 323         }
 324
 325         if (AccessType == kEplObdAccWrite)
 326         {
 327             uiSubIndex = 0x04;  // PReqActPayloadLimit_U16
 328         }
 329         else
 330         {
 331             uiSubIndex = 0x05;  // PResActPayloadLimit_U16
 332         }
 333
 334         // fetch maximum PDO size from Object 1F98h: NMT_CycleTiming_REC
 335         ObdSize = sizeof (wMaxPdoSize);
 336         Ret = EplObduReadEntry(0x1F98, uiSubIndex, &wMaxPdoSize, &ObdSize);
 337         if (Ret != kEplSuccessful)
 338         {   // other fatal error occured
 339             pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR;
 340             goto Exit;
 341         }
 342
 343         // check all objectmappings
 344         for (bMappSubindex = 1; bMappSubindex <= bObjectCount; bMappSubindex++)
 345         {
 346             // read object mapping from OD
 347             ObdSize = sizeof (qwObjectMapping); // QWORD
 348             Ret = EplObduReadEntry(pParam_p->m_uiIndex,
 349                                 bMappSubindex, &qwObjectMapping, &ObdSize);
 350             if (Ret != kEplSuccessful)
 351             {   // other fatal error occured
 352                 pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR;
 353                 goto Exit;
 354             }
 355
 356             // check object mapping
 357             Ret = EplPdouCheckObjectMapping(qwObjectMapping,
 358                                    AccessType,
 359                                    &pParam_p->m_dwAbortCode,
 360                                    &uiCurPdoSize);
 361             if (Ret != kEplSuccessful)
 362             {   // illegal object mapping
 363                 goto Exit;
 364             }
 365
 366             if (uiCurPdoSize > wMaxPdoSize)
 367             {   // mapping exceeds object size
 368                 pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR;
 369                 Ret = kEplPdoVarNotFound;
 370             }
 371
 372
 373         }
 374
 375     }
 376     else
 377     {   // ObjectMapping
 378         Ret = EplPdouCheckPdoValidity(pParam_p, pParam_p->m_uiIndex);
 379         if (Ret != kEplSuccessful)
 380         {   // PDO is valid or does not exist
 381             goto Exit;
 382         }
 383
 384         // check existence of object and validity of object length
 385
 386         qwObjectMapping = *((QWORD*) pParam_p->m_pArg);
 387
 388         Ret = EplPdouCheckObjectMapping(qwObjectMapping,
 389                                AccessType,
 390                                &pParam_p->m_dwAbortCode,
 391                                &uiCurPdoSize);
 392
 393     }
 394
 395 Exit:
 396     return Ret;
 397 }
 398
 399
 400
 401 //=========================================================================//
 402 //                                                                         //
 403 //          P R I V A T E   F U N C T I O N S                              //
 404 //                                                                         //
 405 //=========================================================================//
 406
 407 //---------------------------------------------------------------------------
 408 //
 409 // Function:    EplPdouCheckPdoValidity
 410 //
 411 // Description: check if PDO is valid
 412 //
 413 // Parameters:  pParam_p                = OBD parameter
 414 //
 415 // Returns:     tEplKernel              = error code
 416 //
 417 //
 418 // State:
 419 //
 420 //---------------------------------------------------------------------------
 421
 422 static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam MEM* pParam_p, unsigned int uiIndex_p)
 423 {
 424 tEplKernel          Ret = kEplSuccessful;
 425 tEplObdSize         ObdSize;
 426 BYTE                bObjectCount;
 427
 428     ObdSize = 1;
 429     // read number of mapped objects from OD; this indicates if the PDO is valid
 430     Ret = EplObduReadEntry(uiIndex_p, 0x00, &bObjectCount, &ObdSize);
 431     if (Ret != kEplSuccessful)
 432     {   // other fatal error occured
 433         pParam_p->m_dwAbortCode = EPL_SDOAC_GEN_INTERNAL_INCOMPATIBILITY;
 434         goto Exit;
 435     }
 436     // entry read successfully
 437     if (bObjectCount != 0)
 438     {   // PDO in OD is still valid
 439         pParam_p->m_dwAbortCode = EPL_SDOAC_GEN_PARAM_INCOMPATIBILITY;
 440         Ret = kEplPdoNotExist;
 441         goto Exit;
 442     }
 443
 444 Exit:
 445     return Ret;
 446 }
 447
 448
 449 //---------------------------------------------------------------------------
 450 //
 451 // Function:    EplPdouDecodeObjectMapping
 452 //
 453 // Description: decodes the given object mapping entry into index, subindex,
 454 //              bit offset and bit size.
 455 //
 456 // Parameters:  qwObjectMapping_p       = object mapping entry
 457 //              puiIndex_p              = [OUT] pointer to object index
 458 //              puiSubIndex_p           = [OUT] pointer to subindex
 459 //              puiBitOffset_p          = [OUT] pointer to bit offset
 460 //              puiBitSize_p            = [OUT] pointer to bit size
 461 //
 462 // Returns:     (void)
 463 //
 464 // State:
 465 //
 466 //---------------------------------------------------------------------------
 467
 468 static void EplPdouDecodeObjectMapping(QWORD qwObjectMapping_p,
 469                                     unsigned int* puiIndex_p,
 470                                     unsigned int* puiSubIndex_p,
 471                                     unsigned int* puiBitOffset_p,
 472                                     unsigned int* puiBitSize_p)
 473 {
 474     *puiIndex_p = (unsigned int)
 475                     (qwObjectMapping_p & 0x000000000000FFFFLL);
 476
 477     *puiSubIndex_p = (unsigned int)
 478                     ((qwObjectMapping_p & 0x0000000000FF0000LL) >> 16);
 479
 480     *puiBitOffset_p = (unsigned int)
 481                     ((qwObjectMapping_p & 0x0000FFFF00000000LL) >> 32);
 482
 483     *puiBitSize_p = (unsigned int)
 484                     ((qwObjectMapping_p & 0xFFFF000000000000LL) >> 48);
 485
 486 }
 487
 488
 489 //---------------------------------------------------------------------------
 490 //
 491 // Function:    EplPdouCheckObjectMapping
 492 //
 493 // Description: checks the given object mapping entry.
 494 //
 495 // Parameters:  qwObjectMapping_p       = object mapping entry
 496 //              AccessType_p            = access type to mapped object:
 497 //                                        write = RPDO and read = TPDO
 498 //              puiPdoSize_p            = [OUT] pointer to covered PDO size
 499 //                                        (offset + size) in byte;
 500 //                                        0 if mapping failed
 501 //              pdwAbortCode_p          = [OUT] pointer to SDO abort code;
 502 //                                        0 if mapping is possible
 503 //
 504 // Returns:     tEplKernel              = error code
 505 //
 506 // State:
 507 //
 508 //---------------------------------------------------------------------------
 509
 510 static tEplKernel EplPdouCheckObjectMapping(QWORD qwObjectMapping_p,
 511                                        tEplObdAccess AccessType_p,
 512                                        DWORD* pdwAbortCode_p,
 513                                        unsigned int* puiPdoSize_p)
 514 {
 515 tEplKernel          Ret = kEplSuccessful;
 516 tEplObdSize         ObdSize;
 517 unsigned int        uiIndex;
 518 unsigned int        uiSubIndex;
 519 unsigned int        uiBitOffset;
 520 unsigned int        uiBitSize;
 521 tEplObdAccess       AccessType;
 522 BOOL                fNumerical;
 523
 524     if (qwObjectMapping_p == 0)
 525     {   // discard zero value
 526         *puiPdoSize_p = 0;
 527         goto Exit;
 528     }
 529
 530     // decode object mapping
 531     EplPdouDecodeObjectMapping(qwObjectMapping_p,
 532                                &uiIndex,
 533                                &uiSubIndex,
 534                                &uiBitOffset,
 535                                &uiBitSize);
 536
 537     if ((uiBitOffset & 0x7) != 0x0)
 538     {   // bit mapping is not supported
 539         *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
 540         Ret = kEplPdoGranularityMismatch;
 541         goto Exit;
 542     }
 543
 544     if ((uiBitSize & 0x7) != 0x0)
 545     {   // bit mapping is not supported
 546         *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
 547         Ret = kEplPdoGranularityMismatch;
 548         goto Exit;
 549     }
 550
 551     // check access type
 552     Ret = EplObduGetAccessType(uiIndex, uiSubIndex, &AccessType);
 553     if (Ret != kEplSuccessful)
 554     {   // entry doesn't exist
 555         *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_EXIST;
 556         goto Exit;
 557     }
 558
 559     if ((AccessType & kEplObdAccPdo) == 0)
 560     {   // object is not mappable
 561         *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_MAPPABLE;
 562         Ret = kEplPdoVarNotFound;
 563         goto Exit;
 564     }
 565
 566     if ((AccessType & AccessType_p) == 0)
 567     {   // object is not writeable (RPDO) or readable (TPDO) respectively
 568         *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_MAPPABLE;
 569         Ret = kEplPdoVarNotFound;
 570         goto Exit;
 571     }
 572
 573     ObdSize = EplObduGetDataSize(uiIndex, uiSubIndex);
 574     if (ObdSize < (uiBitSize >> 3))
 575     {   // object does not exist or has smaller size
 576         *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
 577         Ret = kEplPdoVarNotFound;
 578     }
 579
 580     Ret = EplObduIsNumerical(uiIndex, uiSubIndex, &fNumerical);
 581     if (Ret != kEplSuccessful)
 582     {   // entry doesn't exist
 583         *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_EXIST;
 584         goto Exit;
 585     }
 586
 587     if ((fNumerical != FALSE)
 588         && ((uiBitSize >> 3) != ObdSize))
 589     {
 590         // object is numerical,
 591         // therefor size has to fit, but it does not.
 592         *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
 593         Ret = kEplPdoVarNotFound;
 594         goto Exit;
 595     }
 596
 597     // calucaled needed PDO size
 598     *puiPdoSize_p = (uiBitOffset >> 3) + (uiBitSize >> 3);
 599
 600 Exit:
 601     return Ret;
 602 }
 603
 604
 605 #endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOU)) != 0)
 606
 607 // EOF
 608