diff options
Diffstat (limited to 'drivers/staging/epl/EplPdou.c')
| -rw-r--r-- | drivers/staging/epl/EplPdou.c | 575 |
1 files changed, 266 insertions, 309 deletions
diff --git a/drivers/staging/epl/EplPdou.c b/drivers/staging/epl/EplPdou.c index 289d25ed272..e7b10653a0e 100644 --- a/drivers/staging/epl/EplPdou.c +++ b/drivers/staging/epl/EplPdou.c @@ -82,7 +82,6 @@ #error "EPL PDOu module needs EPL module OBDU or OBDK!" #endif - /***************************************************************************/ /* */ /* */ @@ -103,7 +102,6 @@ #define EPL_PDOU_OBD_IDX_MASK 0xFF00 #define EPL_PDOU_PDO_ID_MASK 0x00FF - //--------------------------------------------------------------------------- // local types //--------------------------------------------------------------------------- @@ -116,7 +114,6 @@ // local function prototypes //--------------------------------------------------------------------------- - /***************************************************************************/ /* */ /* */ @@ -130,7 +127,6 @@ // /***************************************************************************/ - //=========================================================================// // // // P R I V A T E D E F I N I T I O N S // @@ -149,25 +145,23 @@ // local vars //--------------------------------------------------------------------------- - //--------------------------------------------------------------------------- // local function prototypes //--------------------------------------------------------------------------- -static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam MEM* pParam_p, unsigned int uiIndex_p); +static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam MEM * pParam_p, + unsigned int uiIndex_p); static void EplPdouDecodeObjectMapping(QWORD qwObjectMapping_p, - unsigned int* puiIndex_p, - unsigned int* puiSubIndex_p, - unsigned int* puiBitOffset_p, - unsigned int* puiBitSize_p); + unsigned int *puiIndex_p, + unsigned int *puiSubIndex_p, + unsigned int *puiBitOffset_p, + unsigned int *puiBitSize_p); static tEplKernel EplPdouCheckObjectMapping(QWORD qwObjectMapping_p, - tEplObdAccess AccessType_p, - DWORD* pdwAbortCode_p, - unsigned int* puiPdoSize_p); - - + tEplObdAccess AccessType_p, + DWORD * pdwAbortCode_p, + unsigned int *puiPdoSize_p); //=========================================================================// // // @@ -193,10 +187,9 @@ static tEplKernel EplPdouCheckObjectMapping(QWORD qwObjectMapping_p, tEplKernel EplPdouAddInstance(void) { - return kEplSuccessful; + return kEplSuccessful; } - //--------------------------------------------------------------------------- // // Function: EplPdouDelInstance() @@ -215,10 +208,9 @@ tEplKernel EplPdouAddInstance(void) tEplKernel EplPdouDelInstance(void) { - return kEplSuccessful; + return kEplSuccessful; } - //--------------------------------------------------------------------------- // // Function: EplPdouCbObdAccess @@ -234,170 +226,153 @@ tEplKernel EplPdouDelInstance(void) // //--------------------------------------------------------------------------- -tEplKernel PUBLIC EplPdouCbObdAccess(tEplObdCbParam MEM* pParam_p) +tEplKernel PUBLIC EplPdouCbObdAccess(tEplObdCbParam MEM * pParam_p) { -tEplKernel Ret = kEplSuccessful; -unsigned int uiPdoId; -unsigned int uiIndexType; -tEplObdSize ObdSize; -BYTE bObjectCount; -QWORD qwObjectMapping; -tEplObdAccess AccessType; -BYTE bMappSubindex; -unsigned int uiCurPdoSize; -WORD wMaxPdoSize; -unsigned int uiSubIndex; - - // fetch PDO ID - uiPdoId = pParam_p->m_uiIndex & EPL_PDOU_PDO_ID_MASK; - - // fetch object index type - uiIndexType = pParam_p->m_uiIndex & EPL_PDOU_OBD_IDX_MASK; - - if (pParam_p->m_ObdEvent != kEplObdEvPreWrite) - { // read accesses, post write events etc. are OK - pParam_p->m_dwAbortCode = 0; - goto Exit; - } - - // check index type - switch (uiIndexType) - { - case EPL_PDOU_OBD_IDX_RX_COMM_PARAM: - // RPDO communication parameter accessed - case EPL_PDOU_OBD_IDX_TX_COMM_PARAM: - { // TPDO communication parameter accessed - Ret = EplPdouCheckPdoValidity(pParam_p, - (EPL_PDOU_OBD_IDX_MAPP_PARAM | pParam_p->m_uiIndex)); - if (Ret != kEplSuccessful) - { // PDO is valid or does not exist - goto Exit; - } - - goto Exit; - } - - case EPL_PDOU_OBD_IDX_RX_MAPP_PARAM: - { // RPDO mapping parameter accessed - - AccessType = kEplObdAccWrite; - break; - } - - case EPL_PDOU_OBD_IDX_TX_MAPP_PARAM: - { // TPDO mapping parameter accessed - - AccessType = kEplObdAccRead; - break; - } - - default: - { // this callback function is only for - // PDO mapping and communication parameters - pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR; - goto Exit; - } - } - - // RPDO and TPDO mapping parameter accessed - - if (pParam_p->m_uiSubIndex == 0) - { // object mapping count accessed - - // PDO is enabled or disabled - bObjectCount = *((BYTE*) pParam_p->m_pArg); - - if (bObjectCount == 0) - { // PDO shall be disabled - - // that is always possible - goto Exit; - } - - // PDO shall be enabled - // it should have been disabled for this operation - Ret = EplPdouCheckPdoValidity(pParam_p, pParam_p->m_uiIndex); - if (Ret != kEplSuccessful) - { // PDO is valid or does not exist - goto Exit; - } - - if (AccessType == kEplObdAccWrite) - { - uiSubIndex = 0x04; // PReqActPayloadLimit_U16 - } - else - { - uiSubIndex = 0x05; // PResActPayloadLimit_U16 - } - - // fetch maximum PDO size from Object 1F98h: NMT_CycleTiming_REC - ObdSize = sizeof (wMaxPdoSize); - Ret = EplObduReadEntry(0x1F98, uiSubIndex, &wMaxPdoSize, &ObdSize); - if (Ret != kEplSuccessful) - { // other fatal error occured - pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR; - goto Exit; - } - - // check all objectmappings - for (bMappSubindex = 1; bMappSubindex <= bObjectCount; bMappSubindex++) - { - // read object mapping from OD - ObdSize = sizeof (qwObjectMapping); // QWORD - Ret = EplObduReadEntry(pParam_p->m_uiIndex, - bMappSubindex, &qwObjectMapping, &ObdSize); - if (Ret != kEplSuccessful) - { // other fatal error occured - pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR; - goto Exit; - } - - // check object mapping - Ret = EplPdouCheckObjectMapping(qwObjectMapping, - AccessType, - &pParam_p->m_dwAbortCode, - &uiCurPdoSize); - if (Ret != kEplSuccessful) - { // illegal object mapping - goto Exit; - } - - if (uiCurPdoSize > wMaxPdoSize) - { // mapping exceeds object size - pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR; - Ret = kEplPdoVarNotFound; - } - - - } - - } - else - { // ObjectMapping - Ret = EplPdouCheckPdoValidity(pParam_p, pParam_p->m_uiIndex); - if (Ret != kEplSuccessful) - { // PDO is valid or does not exist - goto Exit; - } - - // check existence of object and validity of object length - - qwObjectMapping = *((QWORD*) pParam_p->m_pArg); - - Ret = EplPdouCheckObjectMapping(qwObjectMapping, - AccessType, - &pParam_p->m_dwAbortCode, - &uiCurPdoSize); - - } - -Exit: - return Ret; + tEplKernel Ret = kEplSuccessful; + unsigned int uiPdoId; + unsigned int uiIndexType; + tEplObdSize ObdSize; + BYTE bObjectCount; + QWORD qwObjectMapping; + tEplObdAccess AccessType; + BYTE bMappSubindex; + unsigned int uiCurPdoSize; + WORD wMaxPdoSize; + unsigned int uiSubIndex; + + // fetch PDO ID + uiPdoId = pParam_p->m_uiIndex & EPL_PDOU_PDO_ID_MASK; + + // fetch object index type + uiIndexType = pParam_p->m_uiIndex & EPL_PDOU_OBD_IDX_MASK; + + if (pParam_p->m_ObdEvent != kEplObdEvPreWrite) { // read accesses, post write events etc. are OK + pParam_p->m_dwAbortCode = 0; + goto Exit; + } + // check index type + switch (uiIndexType) { + case EPL_PDOU_OBD_IDX_RX_COMM_PARAM: + // RPDO communication parameter accessed + case EPL_PDOU_OBD_IDX_TX_COMM_PARAM: + { // TPDO communication parameter accessed + Ret = EplPdouCheckPdoValidity(pParam_p, + (EPL_PDOU_OBD_IDX_MAPP_PARAM + | pParam_p->m_uiIndex)); + if (Ret != kEplSuccessful) { // PDO is valid or does not exist + goto Exit; + } + + goto Exit; + } + + case EPL_PDOU_OBD_IDX_RX_MAPP_PARAM: + { // RPDO mapping parameter accessed + + AccessType = kEplObdAccWrite; + break; + } + + case EPL_PDOU_OBD_IDX_TX_MAPP_PARAM: + { // TPDO mapping parameter accessed + + AccessType = kEplObdAccRead; + break; + } + + default: + { // this callback function is only for + // PDO mapping and communication parameters + pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR; + goto Exit; + } + } + + // RPDO and TPDO mapping parameter accessed + + if (pParam_p->m_uiSubIndex == 0) { // object mapping count accessed + + // PDO is enabled or disabled + bObjectCount = *((BYTE *) pParam_p->m_pArg); + + if (bObjectCount == 0) { // PDO shall be disabled + + // that is always possible + goto Exit; + } + // PDO shall be enabled + // it should have been disabled for this operation + Ret = EplPdouCheckPdoValidity(pParam_p, pParam_p->m_uiIndex); + if (Ret != kEplSuccessful) { // PDO is valid or does not exist + goto Exit; + } + + if (AccessType == kEplObdAccWrite) { + uiSubIndex = 0x04; // PReqActPayloadLimit_U16 + } else { + uiSubIndex = 0x05; // PResActPayloadLimit_U16 + } + + // fetch maximum PDO size from Object 1F98h: NMT_CycleTiming_REC + ObdSize = sizeof(wMaxPdoSize); + Ret = + EplObduReadEntry(0x1F98, uiSubIndex, &wMaxPdoSize, + &ObdSize); + if (Ret != kEplSuccessful) { // other fatal error occured + pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR; + goto Exit; + } + // check all objectmappings + for (bMappSubindex = 1; bMappSubindex <= bObjectCount; + bMappSubindex++) { + // read object mapping from OD + ObdSize = sizeof(qwObjectMapping); // QWORD + Ret = EplObduReadEntry(pParam_p->m_uiIndex, + bMappSubindex, &qwObjectMapping, + &ObdSize); + if (Ret != kEplSuccessful) { // other fatal error occured + pParam_p->m_dwAbortCode = + EPL_SDOAC_GENERAL_ERROR; + goto Exit; + } + // check object mapping + Ret = EplPdouCheckObjectMapping(qwObjectMapping, + AccessType, + &pParam_p-> + m_dwAbortCode, + &uiCurPdoSize); + if (Ret != kEplSuccessful) { // illegal object mapping + goto Exit; + } + + if (uiCurPdoSize > wMaxPdoSize) { // mapping exceeds object size + pParam_p->m_dwAbortCode = + EPL_SDOAC_GENERAL_ERROR; + Ret = kEplPdoVarNotFound; + } + + } + + } else { // ObjectMapping + Ret = EplPdouCheckPdoValidity(pParam_p, pParam_p->m_uiIndex); + if (Ret != kEplSuccessful) { // PDO is valid or does not exist + goto Exit; + } + // check existence of object and validity of object length + + qwObjectMapping = *((QWORD *) pParam_p->m_pArg); + + Ret = EplPdouCheckObjectMapping(qwObjectMapping, + AccessType, + &pParam_p->m_dwAbortCode, + &uiCurPdoSize); + + } + + Exit: + return Ret; } - - //=========================================================================// // // // P R I V A T E F U N C T I O N S // @@ -419,33 +394,32 @@ Exit: // //--------------------------------------------------------------------------- -static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam MEM* pParam_p, unsigned int uiIndex_p) +static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam MEM * pParam_p, + unsigned int uiIndex_p) { -tEplKernel Ret = kEplSuccessful; -tEplObdSize ObdSize; -BYTE bObjectCount; - - ObdSize = 1; - // read number of mapped objects from OD; this indicates if the PDO is valid - Ret = EplObduReadEntry(uiIndex_p, 0x00, &bObjectCount, &ObdSize); - if (Ret != kEplSuccessful) - { // other fatal error occured - pParam_p->m_dwAbortCode = EPL_SDOAC_GEN_INTERNAL_INCOMPATIBILITY; - goto Exit; - } - // entry read successfully - if (bObjectCount != 0) - { // PDO in OD is still valid - pParam_p->m_dwAbortCode = EPL_SDOAC_GEN_PARAM_INCOMPATIBILITY; - Ret = kEplPdoNotExist; - goto Exit; - } - -Exit: - return Ret; + tEplKernel Ret = kEplSuccessful; + tEplObdSize ObdSize; + BYTE bObjectCount; + + ObdSize = 1; + // read number of mapped objects from OD; this indicates if the PDO is valid + Ret = EplObduReadEntry(uiIndex_p, 0x00, &bObjectCount, &ObdSize); + if (Ret != kEplSuccessful) { // other fatal error occured + pParam_p->m_dwAbortCode = + EPL_SDOAC_GEN_INTERNAL_INCOMPATIBILITY; + goto Exit; + } + // entry read successfully + if (bObjectCount != 0) { // PDO in OD is still valid + pParam_p->m_dwAbortCode = EPL_SDOAC_GEN_PARAM_INCOMPATIBILITY; + Ret = kEplPdoNotExist; + goto Exit; + } + + Exit: + return Ret; } - //--------------------------------------------------------------------------- // // Function: EplPdouDecodeObjectMapping @@ -466,26 +440,25 @@ Exit: //--------------------------------------------------------------------------- static void EplPdouDecodeObjectMapping(QWORD qwObjectMapping_p, - unsigned int* puiIndex_p, - unsigned int* puiSubIndex_p, - unsigned int* puiBitOffset_p, - unsigned int* puiBitSize_p) + unsigned int *puiIndex_p, + unsigned int *puiSubIndex_p, + unsigned int *puiBitOffset_p, + unsigned int *puiBitSize_p) { - *puiIndex_p = (unsigned int) - (qwObjectMapping_p & 0x000000000000FFFFLL); + *puiIndex_p = (unsigned int) + (qwObjectMapping_p & 0x000000000000FFFFLL); - *puiSubIndex_p = (unsigned int) - ((qwObjectMapping_p & 0x0000000000FF0000LL) >> 16); + *puiSubIndex_p = (unsigned int) + ((qwObjectMapping_p & 0x0000000000FF0000LL) >> 16); - *puiBitOffset_p = (unsigned int) - ((qwObjectMapping_p & 0x0000FFFF00000000LL) >> 32); + *puiBitOffset_p = (unsigned int) + ((qwObjectMapping_p & 0x0000FFFF00000000LL) >> 32); - *puiBitSize_p = (unsigned int) - ((qwObjectMapping_p & 0xFFFF000000000000LL) >> 48); + *puiBitSize_p = (unsigned int) + ((qwObjectMapping_p & 0xFFFF000000000000LL) >> 48); } - //--------------------------------------------------------------------------- // // Function: EplPdouCheckObjectMapping @@ -508,101 +481,85 @@ static void EplPdouDecodeObjectMapping(QWORD qwObjectMapping_p, //--------------------------------------------------------------------------- static tEplKernel EplPdouCheckObjectMapping(QWORD qwObjectMapping_p, - tEplObdAccess AccessType_p, - DWORD* pdwAbortCode_p, - unsigned int* puiPdoSize_p) + tEplObdAccess AccessType_p, + DWORD * pdwAbortCode_p, + unsigned int *puiPdoSize_p) { -tEplKernel Ret = kEplSuccessful; -tEplObdSize ObdSize; -unsigned int uiIndex; -unsigned int uiSubIndex; -unsigned int uiBitOffset; -unsigned int uiBitSize; -tEplObdAccess AccessType; -BOOL fNumerical; - - if (qwObjectMapping_p == 0) - { // discard zero value - *puiPdoSize_p = 0; - goto Exit; - } - - // decode object mapping - EplPdouDecodeObjectMapping(qwObjectMapping_p, - &uiIndex, - &uiSubIndex, - &uiBitOffset, - &uiBitSize); - - if ((uiBitOffset & 0x7) != 0x0) - { // bit mapping is not supported - *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR; - Ret = kEplPdoGranularityMismatch; - goto Exit; - } - - if ((uiBitSize & 0x7) != 0x0) - { // bit mapping is not supported - *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR; - Ret = kEplPdoGranularityMismatch; - goto Exit; - } - - // check access type - Ret = EplObduGetAccessType(uiIndex, uiSubIndex, &AccessType); - if (Ret != kEplSuccessful) - { // entry doesn't exist - *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_EXIST; - goto Exit; - } - - if ((AccessType & kEplObdAccPdo) == 0) - { // object is not mappable - *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_MAPPABLE; - Ret = kEplPdoVarNotFound; - goto Exit; - } - - if ((AccessType & AccessType_p) == 0) - { // object is not writeable (RPDO) or readable (TPDO) respectively - *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_MAPPABLE; - Ret = kEplPdoVarNotFound; - goto Exit; - } - - ObdSize = EplObduGetDataSize(uiIndex, uiSubIndex); - if (ObdSize < (uiBitSize >> 3)) - { // object does not exist or has smaller size - *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR; - Ret = kEplPdoVarNotFound; - } - - Ret = EplObduIsNumerical(uiIndex, uiSubIndex, &fNumerical); - if (Ret != kEplSuccessful) - { // entry doesn't exist - *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_EXIST; - goto Exit; - } - - if ((fNumerical != FALSE) - && ((uiBitSize >> 3) != ObdSize)) - { - // object is numerical, - // therefor size has to fit, but it does not. - *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR; - Ret = kEplPdoVarNotFound; - goto Exit; - } - - // calucaled needed PDO size - *puiPdoSize_p = (uiBitOffset >> 3) + (uiBitSize >> 3); - -Exit: - return Ret; + tEplKernel Ret = kEplSuccessful; + tEplObdSize ObdSize; + unsigned int uiIndex; + unsigned int uiSubIndex; + unsigned int uiBitOffset; + unsigned int uiBitSize; + tEplObdAccess AccessType; + BOOL fNumerical; + + if (qwObjectMapping_p == 0) { // discard zero value + *puiPdoSize_p = 0; + goto Exit; + } + // decode object mapping + EplPdouDecodeObjectMapping(qwObjectMapping_p, + &uiIndex, + &uiSubIndex, &uiBitOffset, &uiBitSize); + + if ((uiBitOffset & 0x7) != 0x0) { // bit mapping is not supported + *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR; + Ret = kEplPdoGranularityMismatch; + goto Exit; + } + + if ((uiBitSize & 0x7) != 0x0) { // bit mapping is not supported + *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR; + Ret = kEplPdoGranularityMismatch; + goto Exit; + } + // check access type + Ret = EplObduGetAccessType(uiIndex, uiSubIndex, &AccessType); + if (Ret != kEplSuccessful) { // entry doesn't exist + *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_EXIST; + goto Exit; + } + + if ((AccessType & kEplObdAccPdo) == 0) { // object is not mappable + *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_MAPPABLE; + Ret = kEplPdoVarNotFound; + goto Exit; + } + + if ((AccessType & AccessType_p) == 0) { // object is not writeable (RPDO) or readable (TPDO) respectively + *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_MAPPABLE; + Ret = kEplPdoVarNotFound; + goto Exit; + } + + ObdSize = EplObduGetDataSize(uiIndex, uiSubIndex); + if (ObdSize < (uiBitSize >> 3)) { // object does not exist or has smaller size + *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR; + Ret = kEplPdoVarNotFound; + } + + Ret = EplObduIsNumerical(uiIndex, uiSubIndex, &fNumerical); + if (Ret != kEplSuccessful) { // entry doesn't exist + *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_EXIST; + goto Exit; + } + + if ((fNumerical != FALSE) + && ((uiBitSize >> 3) != ObdSize)) { + // object is numerical, + // therefor size has to fit, but it does not. + *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR; + Ret = kEplPdoVarNotFound; + goto Exit; + } + // calucaled needed PDO size + *puiPdoSize_p = (uiBitOffset >> 3) + (uiBitSize >> 3); + + Exit: + return Ret; } - #endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOU)) != 0) // EOF - |