[linux-2.6-block.git] / drivers / staging / epl / EplPdou.c

/****************************************************************************

  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
      www.systec-electronic.com

  Project:      openPOWERLINK

  Description:  source file for user PDO module
                Currently, this module just implements a OD callback function
                to check if the PDO configuration is valid.

  License:

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions
    are met:

    1. Redistributions of source code must retain the above copyright
       notice, this list of conditions and the following disclaimer.

    2. Redistributions in binary form must reproduce the above copyright
       notice, this list of conditions and the following disclaimer in the
       documentation and/or other materials provided with the distribution.

    3. Neither the name of SYSTEC electronic GmbH nor the names of its
       contributors may be used to endorse or promote products derived
       from this software without prior written permission. For written
       permission, please contact info@systec-electronic.com.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    POSSIBILITY OF SUCH DAMAGE.

    Severability Clause:

        If a provision of this License is or becomes illegal, invalid or
        unenforceable in any jurisdiction, that shall not affect:
        1. the validity or enforceability in that jurisdiction of any other
           provision of this License; or
        2. the validity or enforceability in other jurisdictions of that or
           any other provision of this License.

  -------------------------------------------------------------------------

                $RCSfile: EplPdou.c,v $

                $Author: D.Krueger $

                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $

                $State: Exp $

                Build Environment:
                    GCC V3.4

  -------------------------------------------------------------------------

  Revision History:

  2006/05/22 d.k.:   start of the implementation, version 1.00

****************************************************************************/

#include "EplInc.h"
//#include "user/EplPdouCal.h"
#include "user/EplObdu.h"
#include "user/EplPdou.h"
#include "EplSdoAc.h"

#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOU)) != 0)

#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) == 0) && (EPL_OBD_USE_KERNEL == FALSE)
#error "EPL PDOu module needs EPL module OBDU or OBDK!"
#endif

/***************************************************************************/
/*                                                                         */
/*                                                                         */
/*          G L O B A L   D E F I N I T I O N S                            */
/*                                                                         */
/*                                                                         */
/***************************************************************************/

//---------------------------------------------------------------------------
// const defines
//---------------------------------------------------------------------------

#define EPL_PDOU_OBD_IDX_RX_COMM_PARAM  0x1400
#define EPL_PDOU_OBD_IDX_RX_MAPP_PARAM  0x1600
#define EPL_PDOU_OBD_IDX_TX_COMM_PARAM  0x1800
#define EPL_PDOU_OBD_IDX_TX_MAPP_PARAM  0x1A00
#define EPL_PDOU_OBD_IDX_MAPP_PARAM     0x0200
#define EPL_PDOU_OBD_IDX_MASK           0xFF00
#define EPL_PDOU_PDO_ID_MASK            0x00FF

//---------------------------------------------------------------------------
// local types
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------
// modul globale vars
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------
// local function prototypes
//---------------------------------------------------------------------------

/***************************************************************************/
/*                                                                         */
/*                                                                         */
/*          C L A S S  EplPdou                                             */
/*                                                                         */
/*                                                                         */
/***************************************************************************/
//
// Description:
//
//
/***************************************************************************/

//=========================================================================//
//                                                                         //
//          P R I V A T E   D E F I N I T I O N S                          //
//                                                                         //
//=========================================================================//

//---------------------------------------------------------------------------
// const defines
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------
// local types
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------
// local vars
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------
// local function prototypes
//---------------------------------------------------------------------------

static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam *pParam_p,
					  unsigned int uiIndex_p);

static void EplPdouDecodeObjectMapping(u64 qwObjectMapping_p,
				       unsigned int *puiIndex_p,
				       unsigned int *puiSubIndex_p,
				       unsigned int *puiBitOffset_p,
				       unsigned int *puiBitSize_p);

static tEplKernel EplPdouCheckObjectMapping(u64 qwObjectMapping_p,
					    tEplObdAccess AccessType_p,
					    u32 * pdwAbortCode_p,
					    unsigned int *puiPdoSize_p);

//=========================================================================//
//                                                                         //
//          P U B L I C   F U N C T I O N S                                //
//                                                                         //
//=========================================================================//

//---------------------------------------------------------------------------
//
// Function:    EplPdouAddInstance()
//
// Description: add and initialize new instance of EPL stack
//
// Parameters:  none
//
// Returns:     tEplKernel              = error code
//
//
// State:
//
//---------------------------------------------------------------------------

tEplKernel EplPdouAddInstance(void)
{

	return kEplSuccessful;
}

//---------------------------------------------------------------------------
//
// Function:    EplPdouDelInstance()
//
// Description: deletes an instance of EPL stack
//
// Parameters:  none
//
// Returns:     tEplKernel              = error code
//
//
// State:
//
//---------------------------------------------------------------------------

tEplKernel EplPdouDelInstance(void)
{

	return kEplSuccessful;
}

//---------------------------------------------------------------------------
//
// Function:    EplPdouCbObdAccess
//
// Description: callback function for OD accesses
//
// Parameters:  pParam_p                = OBD parameter
//
// Returns:     tEplKernel              = error code
//
//
// State:
//
//---------------------------------------------------------------------------

tEplKernel EplPdouCbObdAccess(tEplObdCbParam *pParam_p)
{
	tEplKernel Ret = kEplSuccessful;
	unsigned int uiPdoId;
	unsigned int uiIndexType;
	tEplObdSize ObdSize;
	u8 bObjectCount;
	u64 qwObjectMapping;
	tEplObdAccess AccessType;
	u8 bMappSubindex;
	unsigned int uiCurPdoSize;
	u16 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 = *((u8 *) 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);	// u64
			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 = *((u64 *) 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                              //
//                                                                         //
//=========================================================================//

//---------------------------------------------------------------------------
//
// Function:    EplPdouCheckPdoValidity
//
// Description: check if PDO is valid
//
// Parameters:  pParam_p                = OBD parameter
//
// Returns:     tEplKernel              = error code
//
//
// State:
//
//---------------------------------------------------------------------------

static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam *pParam_p,
					  unsigned int uiIndex_p)
{
	tEplKernel Ret = kEplSuccessful;
	tEplObdSize ObdSize;
	u8 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
//
// Description: decodes the given object mapping entry into index, subindex,
//              bit offset and bit size.
//
// Parameters:  qwObjectMapping_p       = object mapping entry
//              puiIndex_p              = [OUT] pointer to object index
//              puiSubIndex_p           = [OUT] pointer to subindex
//              puiBitOffset_p          = [OUT] pointer to bit offset
//              puiBitSize_p            = [OUT] pointer to bit size
//
// Returns:     (void)
//
// State:
//
//---------------------------------------------------------------------------

static void EplPdouDecodeObjectMapping(u64 qwObjectMapping_p,
				       unsigned int *puiIndex_p,
				       unsigned int *puiSubIndex_p,
				       unsigned int *puiBitOffset_p,
				       unsigned int *puiBitSize_p)
{
	*puiIndex_p = (unsigned int)
	    (qwObjectMapping_p & 0x000000000000FFFFLL);

	*puiSubIndex_p = (unsigned int)
	    ((qwObjectMapping_p & 0x0000000000FF0000LL) >> 16);

	*puiBitOffset_p = (unsigned int)
	    ((qwObjectMapping_p & 0x0000FFFF00000000LL) >> 32);

	*puiBitSize_p = (unsigned int)
	    ((qwObjectMapping_p & 0xFFFF000000000000LL) >> 48);

}

//---------------------------------------------------------------------------
//
// Function:    EplPdouCheckObjectMapping
//
// Description: checks the given object mapping entry.
//
// Parameters:  qwObjectMapping_p       = object mapping entry
//              AccessType_p            = access type to mapped object:
//                                        write = RPDO and read = TPDO
//              puiPdoSize_p            = [OUT] pointer to covered PDO size
//                                        (offset + size) in byte;
//                                        0 if mapping failed
//              pdwAbortCode_p          = [OUT] pointer to SDO abort code;
//                                        0 if mapping is possible
//
// Returns:     tEplKernel              = error code
//
// State:
//
//---------------------------------------------------------------------------

static tEplKernel EplPdouCheckObjectMapping(u64 qwObjectMapping_p,
					    tEplObdAccess AccessType_p,
					    u32 * 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;
}

#endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOU)) != 0)

// EOF
Commit	Line	Data
9d7164cf DK	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
9d7164cf DK	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
9d7164cf DK	105	//---------------------------------------------------------------------------
	106	// local types
	107	//---------------------------------------------------------------------------
	108
	109	//---------------------------------------------------------------------------
	110	// modul globale vars
	111	//---------------------------------------------------------------------------
	112
	113	//---------------------------------------------------------------------------
	114	// local function prototypes
	115	//---------------------------------------------------------------------------
	116
9d7164cf DK	117	/***************************************************************************/
	118	/* */
	119	/* */
	120	/* C L A S S EplPdou */
	121	/* */
	122	/* */
	123	/***************************************************************************/
	124	//
	125	// Description:
	126	//
	127	//
	128	/***************************************************************************/
	129
9d7164cf DK	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
9d7164cf DK	148	//---------------------------------------------------------------------------
	149	// local function prototypes
	150	//---------------------------------------------------------------------------
	151
5e9f6bc6	152	static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam *pParam_p,
833dfbe7	153	unsigned int uiIndex_p);
9d7164cf	154
a5c30d94	155	static void EplPdouDecodeObjectMapping(u64 qwObjectMapping_p,
833dfbe7 GKH	156	unsigned int *puiIndex_p,
	157	unsigned int *puiSubIndex_p,
	158	unsigned int *puiBitOffset_p,
	159	unsigned int *puiBitSize_p);
9d7164cf	160
a5c30d94	161	static tEplKernel EplPdouCheckObjectMapping(u64 qwObjectMapping_p,
833dfbe7	162	tEplObdAccess AccessType_p,
d539cfb0	163	u32 * pdwAbortCode_p,
833dfbe7	164	unsigned int *puiPdoSize_p);
9d7164cf DK	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
833dfbe7	190	return kEplSuccessful;
9d7164cf DK	191	}
9d7164cf DK	192
9d7164cf DK	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
833dfbe7	211	return kEplSuccessful;
9d7164cf DK	212	}
9d7164cf DK	213
9d7164cf DK	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
5e9f6bc6	229	tEplKernel EplPdouCbObdAccess(tEplObdCbParam *pParam_p)
9d7164cf	230	{
833dfbe7 GKH	231	tEplKernel Ret = kEplSuccessful;
	232	unsigned int uiPdoId;
	233	unsigned int uiIndexType;
	234	tEplObdSize ObdSize;
2ed53cf8	235	u8 bObjectCount;
a5c30d94	236	u64 qwObjectMapping;
833dfbe7	237	tEplObdAccess AccessType;
2ed53cf8	238	u8 bMappSubindex;
833dfbe7	239	unsigned int uiCurPdoSize;
dcf5371c	240	u16 wMaxPdoSize;
833dfbe7 GKH	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
2ed53cf8	296	bObjectCount = ((u8 ) pParam_p->m_pArg);
833dfbe7 GKH	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
a5c30d94	329	ObdSize = sizeof(qwObjectMapping); // u64
833dfbe7 GKH	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
a5c30d94	363	qwObjectMapping = ((u64 ) pParam_p->m_pArg);
833dfbe7 GKH	364
	365	Ret = EplPdouCheckObjectMapping(qwObjectMapping,
	366	AccessType,
	367	&pParam_p->m_dwAbortCode,
	368	&uiCurPdoSize);
	369
	370	}
	371
	372	Exit:
	373	return Ret;
9d7164cf DK	374	}
9d7164cf DK	375
9d7164cf DK	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
5e9f6bc6	397	static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam *pParam_p,
833dfbe7	398	unsigned int uiIndex_p)
9d7164cf	399	{
833dfbe7 GKH	400	tEplKernel Ret = kEplSuccessful;
833dfbe7 GKH	401	tEplObdSize ObdSize;
2ed53cf8	402	u8 bObjectCount;
833dfbe7 GKH	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;
9d7164cf DK	421	}
9d7164cf DK	422
9d7164cf DK	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
a5c30d94	442	static void EplPdouDecodeObjectMapping(u64 qwObjectMapping_p,
833dfbe7 GKH	443	unsigned int *puiIndex_p,
	444	unsigned int *puiSubIndex_p,
	445	unsigned int *puiBitOffset_p,
	446	unsigned int *puiBitSize_p)
9d7164cf	447	{
833dfbe7 GKH	448	*puiIndex_p = (unsigned int)
833dfbe7 GKH	449	(qwObjectMapping_p & 0x000000000000FFFFLL);
9d7164cf	450
833dfbe7 GKH	451	*puiSubIndex_p = (unsigned int)
833dfbe7 GKH	452	((qwObjectMapping_p & 0x0000000000FF0000LL) >> 16);
9d7164cf	453
833dfbe7 GKH	454	*puiBitOffset_p = (unsigned int)
833dfbe7 GKH	455	((qwObjectMapping_p & 0x0000FFFF00000000LL) >> 32);
9d7164cf	456
833dfbe7 GKH	457	*puiBitSize_p = (unsigned int)
833dfbe7 GKH	458	((qwObjectMapping_p & 0xFFFF000000000000LL) >> 48);
9d7164cf DK	459
	460	}
	461
9d7164cf DK	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
a5c30d94	483	static tEplKernel EplPdouCheckObjectMapping(u64 qwObjectMapping_p,
833dfbe7	484	tEplObdAccess AccessType_p,
d539cfb0	485	u32 * pdwAbortCode_p,
833dfbe7	486	unsigned int *puiPdoSize_p)
9d7164cf	487	{
833dfbe7 GKH	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;
9d7164cf DK	561	}
9d7164cf DK	562
9d7164cf DK	563	#endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOU)) != 0)
	564
	565	// EOF