/*
* co_emcy.c - contains emcy routines
*
* Copyright (c) 2012-2023 emotas embedded communication GmbH
*-------------------------------------------------------------------
* $Id: co_emcy.c 54976 2024-08-07 06:46:12Z boe $
*
*
*-------------------------------------------------------------------
*
*
*/


/********************************************************************/
/**
* \brief Emergency handling
*
* \file co_emcy.c
* contains emcy routines
*
*/

/* header of standard C - libraries
---------------------------------------------------------------------------*/
#include <stddef.h>
#include <string.h>

/* header of project specific types
---------------------------------------------------------------------------*/
#include <gen_define.h>

#include <co_datatype.h>
#include <co_drv.h>
#include <co_timer.h>
#include <co_memcpy.h>
#include <co_emcy.h>
#include <co_odaccess.h>
#include <co_nmt.h>
#include "ico_cobhandler.h"
#include "ico_queue.h"
#include "ico_indication.h"
#include "ico_emcy.h"
#ifdef CO_EMCY_NETWORKING
# include "ico_network.h"
#endif /* CO_EMCY_NETWORKING */

/* constant definitions
---------------------------------------------------------------------------*/
#define CO_EMCY_MSG_CNT		8u

/* local defined data types
---------------------------------------------------------------------------*/
#if defined(CO_EMCY_CONSUMER_CNT) || defined(CO_EMCY_NETWORKING)
typedef struct {
	UNSIGNED32 cobId;
	COB_REFERENZ_T	cob;
# ifdef CO_EMCY_NETWORKING
	UNSIGNED32	routingList;
# endif /* CO_EMCY_NETWORKING */
} CO_EMCY_CONSUMER_T;
#endif /* defined(CO_EMCY_CONSUMER_CNT) || defined(CO_EMCY_NETWORKING) */


/* list of external used functions, if not in headers
---------------------------------------------------------------------------*/
#ifdef CO_EVENT_STATIC_EMCY
extern CO_CONST CO_EVENT_EMCY_T coEventEmcyInd;
#endif /* CO_EVENT_STATIC_EMCY */

#ifdef CO_EVENT_STATIC_EMCY_CONSUMER
extern CO_CONST CO_EVENT_EMCY_CONSUMER_T coEventEmcyConsumerInd;
#endif /* CO_EVENT_STATIC_EMCY_CONSUMER */


/* list of global defined functions
---------------------------------------------------------------------------*/

/* list of local defined functions
---------------------------------------------------------------------------*/
#ifdef CO_EMCY_PRODUCER
static RET_T classicalEmergency(CO_LINE_DECL, UNSIGNED16 emcyErrCode,
		CO_CONST UNSIGNED8	pData[]);
#endif /* CO_EMCY_PRODUCER */
#ifdef CO_EMCY_CONSUMER_CNT
static CO_EMCY_CONSUMER_T *getEmcyConsumer(CO_LINE_DECL, UNSIGNED8 idx);
#endif /* CO_EMCY_CONSUMER_CNT */

/* external variables
---------------------------------------------------------------------------*/

/* global variables
---------------------------------------------------------------------------*/

/* local defined variables
---------------------------------------------------------------------------*/
#ifdef CO_EMCY_PRODUCER
static UNSIGNED32 emcyCobId[CO_LINE_ARRAY_DECL] = { 0ul };
static COB_REFERENZ_T	emcyCob[CO_LINE_ARRAY_DECL];
# ifdef CO_EMCY_ERROR_HISTORY
static UNSIGNED32	errorHistory[CO_EMCY_ERROR_HISTORY];
static UNSIGNED8	errorHistoryCnt[CO_LINE_ARRAY_DECL] = { 0u };
static UNSIGNED16	errorHistoryLineCnt[CO_LINE_ARRAY_DECL];
static UNSIGNED16	errorHistoryLineOffs[CO_LINE_ARRAY_DECL];
# endif /* CO_EMCY_ERROR_HISTORY */
static UNSIGNED16	inhibit[CO_LINE_ARRAY_DECL] = { 0u };

# ifdef CO_EVENT_EMCY_CNT
static CO_EVENT_EMCY_T	emcyTable[CO_EVENT_EMCY_CNT];
static UNSIGNED8		emcyTableCnt;
# endif /* CO_EVENT_EMCY_CNT */
#endif /* CO_EMCY_PRODUCER */

#ifdef CO_EMCY_CONSUMER_CNT
static UNSIGNED8	emcyConsumerCnt[CO_LINE_ARRAY_DECL];
static CO_EMCY_CONSUMER_T	emcyCons[CO_EMCY_CONSUMER_CNT];
static UNSIGNED16	emcyConsLineCnt[CO_LINE_ARRAY_DECL];
static UNSIGNED16	emcyConsLineOffs[CO_LINE_ARRAY_DECL];
# ifdef CO_EVENT_EMCY_CONSUMER_CNT
static CO_EVENT_EMCY_CONSUMER_T	emcyConsTable[CO_EVENT_EMCY_CONSUMER_CNT];
static UNSIGNED8		emcyConsTableCnt;
# endif /* CO_EVENT_EMCY_CONSUMER_CNT */
#ifdef CO_CAN_TIMESTAMP_SUPPORTED
static CO_CAN_TIMESTAMP_T	emcyTimeStamp;
#endif /* CO_CAN_TIMESTAMP_SUPPORTED */
#endif /* CO_EMCY_CONSUMER_CNT */


#ifdef CO_EMCY_PRODUCER


/***************************************************************************/
/**
* \brief coEmcyWriteReq - write an emergency message
*
* This function is only available in classical CAN mode.
*
* With this function, an emergency message can be send.<br>
* The message is automatically composed and transmitted by the given parameter 
* and the error register value (index 0x1001:0).
* After that, the error history (index 0x1003:n) is updated
* with the new data.<br>
* The parameter pData can be NULL,
* if no application specific data should be sent.<br>
*
* The error register (index 0x1001:0) has to be updated by the application.
*
* \return RET_T
*
*/
RET_T coEmcyWriteReq(
		CO_LINE_TYPE	CO_LINE,		/**< can line */
		UNSIGNED16	emcyErrCode,		/**< emergency error code */
		CO_CONST UNSIGNED8	pData[]		/**< pointer to additional 5 data bytes */
	)
{
RET_T	retVal;

	retVal = classicalEmergency(CO_LINE, emcyErrCode, pData);
	
	return(retVal);
}


static RET_T classicalEmergency(
		CO_LINE_TYPE	CO_LINE,		/* can line */
		UNSIGNED16	emcyErrCode,		/* emergency error code */
		CO_CONST UNSIGNED8	pData[]	/* pointer to additional 5 data bytes */
	)
{
RET_T	retVal;
UNSIGNED8	trData[CO_CAN_MAX_DATA_LEN];	/* data */
CO_NMT_STATE_T	nmtState;
UNSIGNED8	i;
#ifdef CO_EMCY_ERROR_HISTORY
UNSIGNED8	cnt;
UNSIGNED32	u32;
#endif /* CO_EMCY_ERROR_HISTORY */
UNSIGNED8	*pErrorRegister;

	pErrorRegister = (UNSIGNED8 *)coOdGetObjAddr(CO_LINE, 0x1001u, 0u);
	if (pErrorRegister != NULL)  {

		/* handling of generic error bit */
		/* set it if one of the other bits are set */
		if ((*pErrorRegister & 0xfeu) == 0u)  {
			*pErrorRegister = 0u;
		} else {
			*pErrorRegister |= 1u;
		}
	}

	/* OPERATIONAL ? */
	nmtState = coNmtGetState(CO_LINE);
	if ((nmtState != CO_NMT_STATE_PREOP)
	 && (nmtState != CO_NMT_STATE_OPERATIONAL))  {
		return(RET_INVALID_NMT_STATE);
	}

	trData[0] = (UNSIGNED8)(emcyErrCode & 0xffu);
	trData[1] = (UNSIGNED8)(emcyErrCode >> 8u);
	if (pErrorRegister != NULL)  {
		trData[2] = *pErrorRegister;
	} else {
		trData[2] = 0u;
	}

	/* set generic error bit temporyry if error code != 0x00xx */
	if ((emcyErrCode & 0xff00u) != 0u)  {
		trData[2] |= 1u;
	}

	/* allow to get NULL as pointer of manufacturer data */
	if (pData == NULL)  {
		for (i = 0u; i < 5u; i++)  {
			trData[3u + i] = 0u;
		}
	} else {
		for (i = 0u; i < 5u; i++)  {
			trData[3u + i] = pData[i];
		}
	}

	/* transmit data */
	retVal = icoTransmitMessage(CO_LINE, emcyCob[CO_LINE], &trData[0], 0u);

#ifdef CO_EMCY_ERROR_HISTORY
	if (emcyErrCode != 0x0000u)  {
		/* save at error history */
		cnt = errorHistoryCnt[CO_LINE];
		if (errorHistoryCnt[CO_LINE] >= errorHistoryLineCnt[CO_LINE])  {
			cnt--;
		}
		while (cnt > 0u)  {
			/* access to out-of-bounds not possible while cnt > 0 */
			if (cnt < errorHistoryLineCnt[CO_LINE])  {
				errorHistory[cnt + errorHistoryLineOffs[CO_LINE]] =
					errorHistory[cnt - 1u + errorHistoryLineOffs[CO_LINE]];
			}
			cnt--;
		} 
		if (pData == NULL)  {
			u32 = (UNSIGNED32)emcyErrCode;
		} else {
			u32 = (UNSIGNED32)emcyErrCode | ((UNSIGNED32)pData[1u] << 16u) | ((UNSIGNED32)pData[0u] << 24u);
		}
		errorHistory[0u + errorHistoryLineOffs[CO_LINE]] = u32;
		if (errorHistoryCnt[CO_LINE] < errorHistoryLineCnt[CO_LINE])  {
			errorHistoryCnt[CO_LINE]++;
		}
	}
#endif /* CO_EMCY_ERROR_HISTORY */

	return(retVal);
}


/***************************************************************************/
/**
* \internal
*
* \brief icoEmcyWriteReq - internal write of an emergency message
*
* This function writes an emergency message.
*
* \return RET_T
*
*/
RET_T icoEmcyWriteReq(
		CO_LINE_TYPE	CO_LINE,	/* can line */
		UNSIGNED16	emcyErrCode,	/* emergency error code */
		UNSIGNED8	*pAddErrorBytes	/* pointer to additional 5 data bytes */
	)
{
RET_T	retVal;

#ifdef CO_EVENT_EMCY_CNT
UNSIGNED8	cnt;

	/* call all user indications */
	cnt = emcyTableCnt;
	while (cnt > 0u)  {
		cnt--;
		/* call user indication */
		retVal = emcyTable[cnt](CO_LINE, emcyErrCode, pAddErrorBytes);
		if (retVal != RET_OK)  {
			return(RET_INTERNAL_ERROR);
		}
	}
#endif /* CO_EVENT_EMCY_CNT */

#ifdef CO_EVENT_STATIC_EMCY
	retVal = coEventEmcyInd(CO_LINE, emcyErrCode, pAddErrorBytes);
	if (retVal != RET_OK)  {
		return(RET_INTERNAL_ERROR);
	}
#endif /* CO_EVENT_STATIC_EMCY */

	retVal = coEmcyWriteReq(CO_LINE, emcyErrCode, pAddErrorBytes);


	return(retVal);
}


# ifdef CO_EVENT_EMCY_CNT
/***************************************************************************/
/**
* \brief coEventRegister_EMCY - register emergency event function
*
* This function registers an emergency indication function.
*
* \return RET_T
*
*/
RET_T coEventRegister_EMCY(
		CO_EVENT_EMCY_T pFunction
	)
{
	if (emcyTableCnt >= CO_EVENT_EMCY_CNT) {
		return(RET_EVENT_NO_RESSOURCE);
	}

	emcyTable[emcyTableCnt] = pFunction;	/* save function pointer */
	emcyTableCnt++;

	return(RET_OK);
}
# endif /* CO_EVENT_EMCY_CNT */
#endif /* CO_EMCY_PRODUCER */


#ifdef CO_EMCY_CONSUMER_CNT
/***************************************************************************/
/*
* \brief icoEmcyConsumerHandler
*
* This function registers an emergency indication function.
*
* \return RET_T
*
*/
void icoEmcyConsumerHandler(
		CO_LINE_TYPE	CO_LINE,		/* can line */
		const CO_REC_DATA_T	*pRecData
	)
{
# ifdef CO_EMCY_NETWORKING
UNSIGNED8	emcyBuf[5];
UNSIGNED32	mask;
CO_EMCY_CONSUMER_T	*pEmcyCons;
# endif /* CO_EMCY_NETWORKING */
# ifdef CO_EVENT_EMCY_CONSUMER_CNT
UNSIGNED8	cnt;
# endif /* CO_EVENT_EMCY_CONSUMER_CNT */
UNSIGNED8	msgCnt = CO_EMCY_MSG_CNT;


	/* check for correct message len */
	if (pRecData->msg.len != msgCnt)  {
		return;
	}

	if (pRecData->spec > emcyConsumerCnt[CO_LINE])  {
		return;
	}

# ifdef CO_CAN_TIMESTAMP_SUPPORTED
	/* save actual timestamp */
	emcyTimeStamp = pRecData->msg.timestamp;
# endif /* CO_CAN_TIMESTAMP_SUPPORTED */

# ifdef CO_EVENT_EMCY_CONSUMER_CNT
	/* call all user indications */
	cnt = emcyConsTableCnt;
	while (cnt > 0u)  {
		cnt--;
		/* call user indication */
		emcyConsTable[cnt](CO_LINE, (UNSIGNED8)(pRecData->msg.canId & 0x7fu),
			pRecData->msg.data[0]
				| (UNSIGNED16)((UNSIGNED16)pRecData->msg.data[1] << 8u),
			pRecData->msg.data[2],
			&pRecData->msg.data[3]);
	}
# endif /* CO_EVENT_EMCY_CONSUMER_CNT */


# ifdef CO_EVENT_STATIC_EMCY_CONSUMER
	coEventEmcyConsumerInd(CO_LINE, (UNSIGNED8)(pRecData->msg.canId & 0x7fu),
			pRecData->msg.data[0]
				| (UNSIGNED16)((UNSIGNED16)pRecData->msg.data[1] << 8u),
			pRecData->msg.data[2],
			&pRecData->msg.data[3]);
# endif /* CO_EVENT_STATIC_EMCY_CONSUMER */


# ifdef CO_EMCY_NETWORKING
	pEmcyCons = getEmcyConsumer(CO_LINE, (UNSIGNED8)pRecData->spec);
	mask = pEmcyCons->routingList;
	if (mask != 0u) {
		for (CO_LINE = 0u; CO_LINE < CO_MULTI_LINE_CNT; CO_LINE++)
		{
			if ((mask & (1ul << CO_LINE)) != 0u)  {
				/* forward emcy */
				UNSIGNED16	nwId = icoNetworkLocalId(CO_LINE);
				emcyBuf[0] = (UNSIGNED8)(nwId & 0xffu);
				emcyBuf[1] = (UNSIGNED8)(nwId >> 8u);
				emcyBuf[2] = (UNSIGNED8)(pRecData->msg.canId & 0x7fu);
				emcyBuf[3] = pRecData->msg.data[0];
				emcyBuf[4] = pRecData->msg.data[1];
				(void)icoEmcyWriteReq(CO_LINE, 0xf000u, &emcyBuf[0]);
			}
		}
	}
# endif /* CO_EMCY_NETWORKING */
}


# ifdef CO_CAN_TIMESTAMP_SUPPORTED
/***************************************************************************/
/**
* \brief coEmcyGetTimestamp - get EMCY timestamp
*
* This function returns the timestamp of the last received Emergency
* set by the CAN driver
*
* \return 
*
*/
CO_CAN_TIMESTAMP_T coEmcyGetTimestamp(
		CO_LINE_TYPE	CO_LINE		/**< can line */
	)
{
	return(emcyTimeStamp);
}
# endif /* CO_CAN_TIMESTAMP_SUPPORTED */


# ifdef CO_EVENT_EMCY_CONSUMER_CNT
/***************************************************************************/
/**
* \brief coEventRegister_EMCY_CONSUMER - register emergency consumer event function
*
* This function registers an emergency consumer indication function.
*
* \return RET_T
*
*/
RET_T coEventRegister_EMCY_CONSUMER(
		CO_EVENT_EMCY_CONSUMER_T pFunction
	)
{
	if (emcyConsTableCnt >= CO_EVENT_EMCY_CONSUMER_CNT) {
		return(RET_EVENT_NO_RESSOURCE);
	}

	emcyConsTable[emcyConsTableCnt] = pFunction;	/* save function pointer */
	emcyConsTableCnt++;

	return(RET_OK);
}
# endif /* CO_EVENT_EMCY_CONSUMER_CNT */
#endif /* CO_EMCY_CONSUMER_CNT */


/***************************************************************************/
/***************************************************************************/
/***************************************************************************/
/***************************************************************************/
/***************************************************************************/
/**
* \internal
*
* \brief icoEmcyGetObjectAddr - get Emcy object address
*
*
* \return RET_T
*
*/
void *icoEmcyGetObjectAddr(
		CO_LINE_TYPE	CO_LINE,		/* can line */
		UNSIGNED16	idx,				/* index */
		UNSIGNED8	subIndex			/* subindex */
	)
{
void	*pAddr = NULL;
#if defined(CO_EMCY_CONSUMER_CNT) || defined(CO_EMCY_NETWORKING)
CO_EMCY_CONSUMER_T	*pEmcyCons;
#endif /* defined(CO_EMCY_CONSUMER_CNT) || defined(CO_EMCY_NETWORKING) */
(void)subIndex;

#if defined(CO_EMCY_CONSUMER_CNT) || defined(CO_EMCY_NETWORKING)
	pEmcyCons = getEmcyConsumer(CO_LINE, subIndex - 1u);
#endif /* defined(CO_EMCY_CONSUMER_CNT) || defined(CO_EMCY_NETWORKING) */

	switch (idx) {

#ifdef CO_EMCY_PRODUCER
# ifdef CO_EMCY_ERROR_HISTORY
		case 0x1003u:		/* error history */
			if (subIndex == 0u)  {
				pAddr = &errorHistoryCnt[CO_LINE];
			} else {
				if (subIndex > errorHistoryCnt[CO_LINE])  {
					pAddr = NULL;
				} else {
					pAddr = &errorHistory[subIndex - 1u + errorHistoryLineOffs[CO_LINE]];
				}
			}
			break;
# endif /* CO_EMCY_ERROR_HISTORY */

		case 0x1014u:	/* cob-id emcy producer */
			pAddr = &emcyCobId[CO_LINE];
			break;

		case 0x1015u:	/* inhibit */
			pAddr = &inhibit[CO_LINE];
			break;

#endif /* CO_EMCY_PRODUCER */

#ifdef CO_EMCY_CONSUMER_CNT
		case 0x1028u:	/* emcy consumer */
			pAddr = (void *)&pEmcyCons->cobId;
			break;
#endif /* CO_EMCY_CONSUMER_CNT */

#ifdef CO_EMCY_NETWORKING
		case 0x1f2fu:	/* emcy routing list */
			pAddr = (void *)&pEmcyCons->routingList;
			break;
# endif /* CO_EMCY_NETWORKING */

		default:
			break;
	}

	(void)CO_LINE;

	return(pAddr);
}


#if defined(CO_EMCY_PRODUCER) || defined (CO_EMCY_CONSUMER_CNT)
/***************************************************************************/
/**
* \internal
*
* \brief icoEmcyCheckObjLimitCobId - check cob-id
*
*
* \return RET_T
*
*/
RET_T icoEmcyCheckObjLimitCobid(
		CO_LINE_TYPE	CO_LINE,	/* can line */
		UNSIGNED16	index,		/* index */
		UNSIGNED8	subIndex,	/* subIndex */
		UNSIGNED32	canId		/* new cob ID*/
	)
{
RET_T	retVal = RET_OK;
#ifdef CO_EMCY_CONSUMER_CNT
CO_EMCY_CONSUMER_T	*pEmcyCons;
#endif /* CO_EMCY_CONSUMER_CNT */
(void)subIndex;

#ifdef CO_EMCY_PRODUCER
	if (index == 0x1014u)  {
		/* write the same value again is allowed */
		if (canId != emcyCobId[CO_LINE])  {
			if ((canId & CO_COB_VALID_MASK) == 0u)  {
				/* new cobid is valid, only allowed if cob was disabled before*/
				if ((emcyCobId[CO_LINE] & CO_COB_VALID_MASK) != CO_COB_INVALID)  {
					return(RET_SDO_INVALID_VALUE);
				}
			}
			if (icoCheckRestrictedCobs(canId, 0x81u, 0x81u) == CO_TRUE)  {
				return(RET_SDO_INVALID_VALUE);
			}
		}
	}
#endif /* CO_EMCY_PRODUCER */

#ifdef CO_EMCY_CONSUMER_CNT
	if (index == 0x1028u)  {
		if ((canId & CO_COB_VALID_MASK) == 0u)  {
			/* new cobid is valid, only allowed if cob was disabled before*/
			pEmcyCons = getEmcyConsumer(CO_LINE, subIndex - 1u);
			if ((pEmcyCons->cobId & CO_COB_VALID_MASK)
					!= CO_COB_INVALID)  {
				return(RET_SDO_INVALID_VALUE);
			}
			if (icoCheckRestrictedCobs(canId, 0x81u, 0x81u) == CO_TRUE)  {
				return(RET_SDO_INVALID_VALUE);
			}
		}
	}
#endif /* CO_EMCY_CONSUMER_CNT */

	return(retVal);
}



/***************************************************************************/
/**
* \internal
*
* \brief icoEmcyObjChanged - EMCY object was changed
*
*
* \return RET_T
*
*/
RET_T icoEmcyObjChanged(
		CO_LINE_TYPE	CO_LINE,	/* can line */
		UNSIGNED16	index,			/* object index */
		UNSIGNED8	subIndex		/* object subIndex */
	)
{
RET_T	retVal = RET_OK;
UNSIGNED8	cnt = CO_EMCY_MSG_CNT;
#ifdef CO_EMCY_CONSUMER_CNT
CO_EMCY_CONSUMER_T	*pEmcyCons;
#endif /* CO_EMCY_CONSUMER_CNT */
(void)subIndex;

	switch (index)  {
#ifdef CO_EMCY_PRODUCER
# ifdef CO_EMCY_ERROR_HISTORY
		case 0x1003u:
			errorHistoryCnt[CO_LINE] = 0u;
			break;
# endif /* CO_EMCY_ERROR_HISTORY */

		case 0x1014u:
			retVal = icoCobSet(CO_LINE, emcyCob[CO_LINE], emcyCobId[CO_LINE],
				CO_COB_RTR_NONE, cnt);
			break;

		case 0x1015u:	/* inhibit */
			retVal = icoCobSetInhibit(CO_LINE, emcyCob[CO_LINE], inhibit[CO_LINE]);
			break;
#endif /* CO_EMCY_PRODUCER */

#ifdef CO_EMCY_CONSUMER_CNT
		case 0x1028u:	/* emcy consumer */

			pEmcyCons = getEmcyConsumer(CO_LINE, subIndex - 1u);
			(void)icoCobSet(CO_LINE, pEmcyCons->cob,
					pEmcyCons->cobId, CO_COB_RTR_NONE, cnt);
			break;
#endif /* CO_EMCY_CONSUMER_CNT */
		default:
			break;
	}

	return(retVal);
}
#endif /* defined(CO_EMCY_PRODUCER) || defined (CO_EMCY_CONSUMER_CNT) */


#ifdef CO_EMCY_PRODUCER


/***************************************************************************/
/**
* \internal
*
* \brief icoEmcyCheckObjLimitHist - check object limits u8 value
*
*
* \return RET_T
*
*/
RET_T icoEmcyCheckObjLimitHist(
		CO_LINE_TYPE	CO_LINE,	/* can line */
		UNSIGNED16	index,			/* object index */
		UNSIGNED8	subIndex,	/* subindex */
		UNSIGNED8	u8			/* received data */
	)
{
RET_T	retVal = RET_OK;

	/* 0x1001 */
	if (index == 0x1001u)  {
		/* all values allowed */
	} else {
		/* index 1003 */
		if ((subIndex != 0u) || (u8 != 0u))  {
			retVal = RET_SDO_INVALID_VALUE;
		}
	}
	(void)CO_LINE;
	return(retVal);
}


/***************************************************************************/
/**
*
* \internal
*
* \brief coEmcyReset - reset comm for Emcy
*
* \return none
*/
void icoEmcyReset(
		CO_LINE_TYPE	CO_LINE		/* can line */
	)
{
UNSIGNED8 cnt = CO_EMCY_MSG_CNT;


	(void)icoCobSet(CO_LINE, emcyCob[CO_LINE], emcyCobId[CO_LINE],
		CO_COB_RTR_NONE, cnt);
}


/***************************************************************************/
/**
*
* \internal
*
* \brief coEmcySetDefaultValue - set default (predefined connection set)
*
* \return none
*/
void icoEmcyProducerSetDefaultValue(
		CO_LINE_TYPE	CO_LINE		/* can line */
	)
{
RET_T	retVal;
UNSIGNED16	u16;
UNSIGNED32	u32;

	emcyCobId[CO_LINE] = 0x80ul + coNmtGetNodeId(CO_LINE);

	retVal = coOdGetDefaultVal_u32(CO_LINE, 0x1014u, 0u, &u32);
	if (retVal == RET_OK)  {
		if ((u32 & CO_COB_VALID_MASK) != 0u)  {
			/* cob invalid */
			emcyCobId[CO_LINE] |= CO_COB_INVALID;
		}
	}
	retVal = coOdGetDefaultVal_u16(CO_LINE, 0x1015u, 0u, &u16);
	if (retVal == RET_OK)  {
		inhibit[CO_LINE] = u16;
		(void)icoCobSetInhibit(CO_LINE, emcyCob[CO_LINE], inhibit[CO_LINE]);
	}

	return;
}


/***************************************************************************/
/**
* \brief coEmcyProducerInit - initialization for emergency producer
*
* This function initializes the emergency producer functionality.
*
* \return RET_T
*
*/
RET_T coEmcyProducerInit(
		CO_LINE_TYPE	CO_LINE		/* can line */
	)
{
	emcyCob[CO_LINE] = icoCobCreate(CO_LINE, CO_COB_TYPE_TRANSMIT, CO_SERVICE_EMCY_TRANSMIT, 0u);
	if (emcyCob[CO_LINE] == 0xffffu)  {
		return(RET_NO_COB_AVAILABLE);
	}

	return(RET_OK);
}

#endif /* CO_EMCY_PRODUCER */


/***************************************************************************/
/*
* \brief icoEmcyVarInit - init emcy variables
*
*/
void icoEmcyVarInit(
		UNSIGNED8	*pEmcyErrHistList,	/* line counts */
		UNSIGNED8	*pEmcyActErrHistList,	/* line counts */
		UNSIGNED8	*pEmcyConsList		/* line counts */
	)
{
#ifdef CO_EMCY_CONSUMER_CNT
UNSIGNED16		i;
#endif /* CO_EMCY_CONSUMER_CNT */
#if defined(CO_EMCY_CONSUMER_CNT) || defined(CO_EMCY_PRODUCER)
CO_LINE_TYPE	CO_LINE;
#endif /* defined(CO_EMCY_CONSUMER_CNT) || defined(CO_EMCY_PRODUCER) */
#if defined(CO_EMCY_ERROR_HISTORY) || defined(CO_EMCY_CONSUMER_CNT)
UNSIGNED16		offs = 0u;
#endif /* defined(CO_EMCY_ERROR_HISTORY) || defined(CO_EMCY_CONSUMER_CNT) */
#ifdef CO_EMCY_PRODUCER
#endif /* CO_EMCY_PRODUCER */
(void)pEmcyErrHistList;
(void)pEmcyActErrHistList;
(void)pEmcyConsList;

#ifdef CO_EMCY_PRODUCER
	for (CO_LINE = 0u; CO_LINE < CO_MULTI_LINE_CNT; CO_LINE++) 
	{
		emcyCobId[CO_LINE] = 0ul;
		emcyCob[CO_LINE] = CO_INVALID_COB_REF;
		inhibit[CO_LINE] = 0u;
# ifdef CO_EMCY_ERROR_HISTORY
		errorHistoryCnt[CO_LINE] = 0u;
		errorHistoryLineCnt[CO_LINE] = pEmcyErrHistList[CO_LINE];
		errorHistoryLineOffs[CO_LINE] = offs;
		offs += pEmcyErrHistList[CO_LINE];
# endif /* CO_EMCY_ERROR_HISTORY */

	}

# ifdef CO_EVENT_EMCY_CNT
	emcyTableCnt = 0u;
# endif /* CO_EVENT_EMCY_CNT */
#endif /* CO_EMCY_PRODUCER */

#ifdef CO_EMCY_CONSUMER_CNT
	offs = 0;

	for (CO_LINE = 0; CO_LINE < CO_MULTI_LINE_CNT; CO_LINE++) 
	{
		emcyConsumerCnt[CO_LINE] = 0u;
		emcyConsLineCnt[CO_LINE] = pEmcyConsList[CO_LINE];
		emcyConsLineOffs[CO_LINE] = offs;
		offs += emcyConsLineCnt[CO_LINE];
	}

	/* reset cobs */
	for (i = 0u; i < CO_EMCY_CONSUMER_CNT; i++)  {
		emcyCons[i].cob = CO_INVALID_COB_REF;
	}

# ifdef CO_EVENT_EMCY_CONSUMER_CNT
	emcyConsTableCnt = 0u;
# endif /* CO_EVENT_EMCY_CONSUMER_CNT */
#endif /* CO_EMCY_CONSUMER_CNT */
}


#ifdef CO_EMCY_CONSUMER_CNT
/***************************************************************************/
/**
* \internal
*
* \brief getEmcyConsumer - get emcy consumer
*
* \return none
*/
static CO_EMCY_CONSUMER_T *getEmcyConsumer(
		CO_LINE_TYPE	CO_LINE,	/* can line */
		UNSIGNED8		idx
	)
{
	return(&emcyCons[idx + emcyConsLineOffs[CO_LINE]]);
}


/***************************************************************************/
/**
* \internal
*
* \brief coEmcyConsumerReset - reset comm for Emcy
*
* \return none
*/
void icoEmcyConsumerReset(
		CO_LINE_TYPE	CO_LINE		/* can line */
	)
{
UNSIGNED8	i;
UNSIGNED8	cnt = CO_EMCY_MSG_CNT;
CO_EMCY_CONSUMER_T	*pEmcyCons;

	for (i = 0u; i < emcyConsumerCnt[CO_LINE]; i++)  {
		pEmcyCons = getEmcyConsumer(CO_LINE, i);


		(void)icoCobSet(CO_LINE, pEmcyCons->cob, pEmcyCons->cobId,
			CO_COB_RTR_NONE, cnt);
	}
}


/***************************************************************************/
/**
* \internal
*
* \brief coEmcyConsumerSetDefaultValue - reset comm for Emcy
*
* \return none
*/
void icoEmcyConsumerSetDefaultValue(
		CO_LINE_TYPE	CO_LINE		/* can line */
	)
{
RET_T	retVal;
UNSIGNED8	i;
CO_EMCY_CONSUMER_T	*pEmcyCons;

	for (i = 0u; i < emcyConsumerCnt[CO_LINE]; i++)  {
		pEmcyCons = getEmcyConsumer(CO_LINE, i);
		retVal = coOdGetDefaultVal_u32(CO_LINE, 0x1028u, i + 1u, &pEmcyCons->cobId);
		if (retVal != RET_OK)  {
			pEmcyCons->cobId |= CO_COB_INVALID;
		}

#ifdef CO_EMCY_NETWORKING
		(void)coOdGetDefaultVal_u32(CO_LINE, 0x1f2fu, i + 1u,
			&pEmcyCons->routingList);
#endif /* CO_EMCY_NETWORKING */
	}
}


/***************************************************************************/
/**
* \brief coEmcyConsumerInit - initialization for emergency consumer
*
* This function initializes the emergency consumers.
*
* \return RET_T
*
*/
RET_T coEmcyConsumerInit(
		CO_LINE_TYPE	CO_LINE,	/**< can line */
		UNSIGNED8		emcyCnt		/**< number of emergency consumers */
	)
{
UNSIGNED8	cnt;
CO_EMCY_CONSUMER_T	*pEmcyCons;

	if (emcyCnt > emcyConsLineCnt[CO_LINE])  {
		return(RET_EVENT_NO_RESSOURCE);
	}
 
	for (cnt = 0u; cnt < emcyCnt; cnt++)  {
		pEmcyCons = getEmcyConsumer(CO_LINE, cnt);
		pEmcyCons->cob = icoCobCreate(CO_LINE, CO_COB_TYPE_RECEIVE,
				CO_SERVICE_EMCY_RECEIVE, (UNSIGNED16)cnt);
		if (pEmcyCons->cob == CO_INVALID_COB_REF)  {
			return(RET_NO_COB_AVAILABLE);
		}
	}

	emcyConsumerCnt[CO_LINE] = emcyCnt;

	return(RET_OK);
}
#endif /* CO_EMCY_CONSUMER_CNT */
