/*
* co_memcpy.c - contains routines for common memcpy
*
* Copyright (c) 2012-2023 emotas embedded communication GmbH
*-------------------------------------------------------------------
* $Id: co_memcpy.c 51834 2024-01-17 15:11:31Z boe $
*
*
*-------------------------------------------------------------------
*
*
*/

/********************************************************************/
/**
* \brief common memcpy routines
*
* \file co_memcpy.c
* contains routines for common memcpy
*
*/

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

/* header of project specific types
---------------------------------------------------------------------------*/
#include <gen_define.h>
#ifdef CANOPEN_SUPPORTED
# include "ico_indication.h"
#endif /* CANOPEN_SUPPORTED */
#include <co_memcpy.h>
#include "ico_memcpy.h"

/* constant definitions
---------------------------------------------------------------------------*/
#if defined(CO_EVENT_OBJECT_CHANGED) || defined(RAWCAN_CHANGED_INDICATION_CNT) || defined(J1939_EVENT_SPN_CHANGED)
# define CO_OBJECT_CHANGED	1
#endif /* defined(CO_EVENT_OBJECT_CHANGED) || defined(RAWCAN_CHANGED_INDICATION_CNT) */

/* local defined data types
---------------------------------------------------------------------------*/

/* list of external used functions, if not in headers
---------------------------------------------------------------------------*/

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

/* list of local defined functions
---------------------------------------------------------------------------*/
CO_INLINE BOOL_T memcpyPack_dsp(void *pDest, CO_CONST void *pSrc,
		UNSIGNED32	cnt, UNSIGNED32 dstOffset,
		CORE_DATA_TYPE_T dType,	UNSIGNED8	bigEndianProtocol);
CO_INLINE void memcpyUnpack_dsp(void *pDest, CO_CONST void *pSrc,
		UNSIGNED32	cnt, UNSIGNED32 srcOffset,
		CORE_DATA_TYPE_T dType,	UNSIGNED8	bigEndianProtocol);
static UNSIGNED32 getDTypeLen(CORE_DATA_TYPE_T dType);

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

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

/* local defined variables
---------------------------------------------------------------------------*/


/***************************************************************************/
/**
*
* \brief coreMemcpyPack - memcpy with numeric dependencies
*
* This function provide a memcpy from CAN-Controller to an internal variable.
* If the parameter numeric is unequal 0,
* then the data are copied as numeric value,
* so byte swapping or packaging is done.
*
* \return BOOL_T changed
*
*/
CO_INLINE BOOL_T coreMemcpyPack(
		void		*pDest,				/**< pointer to destionation addr */
		CO_CONST	void *pSrc,			/**< pointer to source addr */
		UNSIGNED32	size,				/**< data size */
		UNSIGNED32	dstOffset,			/**< destination Offset */
		CORE_DATA_TYPE_T dType,			/**< data type */
		UNSIGNED8	bigEndianProtocol	/**< big endian protocol data */
	)
{
BOOL_T		changed = CO_FALSE;
#ifdef CO_OBJECT_CHANGED
INTEGER32	cmp = 0;
#endif /* CO_OBJECT_CHANGED */

	/* string copy ?*/
	if (dType >= CORE_DTYPE_NON_NUMERIC)  {
#ifdef CO_OBJECT_CHANGED
		cmp = MEMCMP((UNSIGNED8 *)pDest + dstOffset, pSrc, (size_t)size);
		if (cmp != 0)  {
			changed = CO_TRUE;
		}
#endif /* CO_OBJECT_CHANGED */
		MEMCPY((UNSIGNED8 *)pDest + dstOffset, pSrc, (size_t)size);
	} else {

		/* numeric copy */

#ifdef CO_CPU_DSP
		changed = memcpyPack_dsp(pDest, pSrc, size, dstOffset, dType, bigEndianProtocol);
#else /* CO_CPU_DSP */
		UNSIGNED8	*pDest_u8 = (UNSIGNED8 *) pDest;
		CO_CONST UNSIGNED8 *pSrc_u8 = (CO_CONST UNSIGNED8 *) pSrc;

# ifdef CO_BIG_ENDIAN
		if (bigEndianProtocol == 0u)
# else /* CO_BIG_ENDIAN */
		if (bigEndianProtocol != 0u)
# endif /* CO_BIG_ENDIAN */
		{
			/* byte swapping necessary */

			UNSIGNED32 len = getDTypeLen(dType);
			pDest_u8 = (UNSIGNED8 *)pDest + len - dstOffset - 1u;

			while (size > 0u)  {
# ifdef CO_OBJECT_CHANGED
				if (*pDest_u8 != *pSrc_u8)  {
					changed = CO_TRUE;
				}
# endif /* CO_OBJECT_CHANGED */
				*pDest_u8 = *pSrc_u8;
				pDest_u8 --;
				pSrc_u8 ++;
				size--;
			}

		} else {
# ifdef CO_OBJECT_CHANGED
			cmp = MEMCMP((UNSIGNED8 *)pDest + dstOffset, pSrc, (size_t)size);
			if (cmp != 0)  {
				changed = CO_TRUE;
			}
# endif /* CO_OBJECT_CHANGED */

			MEMCPY((UNSIGNED8 *)pDest + dstOffset, pSrc, (size_t)size);
		}
#endif /* CO_CPU_DSP */

	}

	return(changed);
}


#ifdef CO_CPU_DSP
/***************************************************************************/
/**
* \internal
*
* \brief memcpyPack_dsp - memcpy with numeric dependencies for DSP
*
* \return BOOL_T changed
*
*/
CO_INLINE BOOL_T memcpyPack_dsp(
		void		*pDest,				/* pointer to destionation addr */
		CO_CONST	void *pSrc,			/* pointer to source addr */
		UNSIGNED32	cnt,				/* data cnt */
		UNSIGNED32	dstOffset,			/* destination Offset */
		CORE_DATA_TYPE_T dType,			/* data type */
		UNSIGNED8	bigEndianProtocol	/* big endian protocol data */
	)
{
CO_CONST UNSIGNED8 *pSrc_u8;
UNSIGNED8	*pDest_u8;
unsigned char	*pDest_char; /* ptr to 16bit or 32bit depend of the DSP type */
BOOL_T		changed = CO_FALSE;
UNSIGNED32	val;
UNSIGNED8	packOffset;
# ifdef CO_OBJECT_CHANGED
unsigned char oldDestVal; /* DSP bitsize! */
# endif /* CO_OBJECT_CHANGED */

	if (bigEndianProtocol != 0u)  {
		/* src = src + cnt (ende can buffer)
		 * dst = dst + offs
		 */
		pSrc_u8 = (CO_CONST UNSIGNED8 *) pSrc + cnt - 1u;
		pDest_u8 = (UNSIGNED8 *) pDest + dstOffset / CO_CPU_DSP_BYTESIZE;
		packOffset = dstOffset  % CO_CPU_DSP_BYTESIZE;
	} else {
		/* src = src (anfang can buffer)
		 * dst = dst + offs
		 */
		pSrc_u8 = (CO_CONST UNSIGNED8 *) pSrc;
		pDest_u8 = (UNSIGNED8 *) pDest + (dstOffset / CO_CPU_DSP_BYTESIZE);
		packOffset = dstOffset % CO_CPU_DSP_BYTESIZE;
	}
	pDest_char = (unsigned char *)pDest_u8;


	while (cnt > 0u)  {

#  ifdef CO_OBJECT_CHANGED
		oldDestVal = *pDest_char;
#  endif /* CO_OBJECT_CHANGED */

		val = *pSrc_u8;
		*pDest_char &= ~(0xfful << (packOffset * 8u));
		*pDest_char |= (val << (packOffset * 8u));

#  ifdef CO_OBJECT_CHANGED
		if (*pDest_char != oldDestVal)  {
			changed = CO_TRUE;
		}
#  endif /* CO_OBJECT_CHANGED */

		if (bigEndianProtocol != 0u)  {
			pSrc_u8 --;
		} else {
			pSrc_u8 ++;
		}

		/* incr packOffset */
		packOffset++;

		if ((packOffset % CO_CPU_DSP_BYTESIZE) == 0u)  {
			packOffset = 0u;
			pDest_u8 ++;
			pDest_char = (unsigned char *) pDest_u8;
		}

		cnt --;
	}

	return(changed);
}
#endif /* CO_CPU_DSP */


/***************************************************************************/
/**
*
* \brief coreMemcpyUnpack - memcpy with numeric dependencies
*
* This function provide a memcpy from OD variable to CAN-Controller.
* If the parameter numeric is unequal 0,
* then the data are copied as numeric value,
* so byte swapping or unpackaging is done.
*
* \return none
*
*/
CO_INLINE void coreMemcpyUnpack(
		void		*pDest,			/**< destination address (CAN buffer) */
		CO_CONST void	*pSrc,		/**< source address */
		UNSIGNED32	size,			/**< size in bytes */
		UNSIGNED32	srcOffset,		/**< source offset */
		CORE_DATA_TYPE_T dType,		/**< data type */
		UNSIGNED8	bigEndianProtocol	/**< big endian protocol data */
	)
{
	/* no numeric copy, copy address by address */
	if (dType >= CORE_DTYPE_NON_NUMERIC)  {
		MEMCPY(pDest, (CO_CONST UNSIGNED8 *)pSrc + srcOffset, (size_t)size);
	} else {

#ifdef CO_CPU_DSP
		memcpyUnpack_dsp(pDest, pSrc, size, srcOffset, dType, bigEndianProtocol);
#else /* CO_CPU_DSP */

# ifdef CO_BIG_ENDIAN
		if (bigEndianProtocol == 0u)
# else /* CO_BIG_ENDIAN */
		if (bigEndianProtocol != 0u)
# endif /* CO_BIG_ENDIAN */
		{
			/* byte swapping necessary */
			CO_CONST UNSIGNED8 *pSrc_u8;
			UNSIGNED8 *pDst_u8;
			UNSIGNED32 len = getDTypeLen(dType);

			pDst_u8 = (UNSIGNED8 *)pDest;
			pSrc_u8 = (CO_CONST UNSIGNED8 *)pSrc + len - srcOffset - 1u;

			while (size > 0u)  {
				*pDst_u8 = *pSrc_u8;
				pDst_u8 ++;
				pSrc_u8 --;
				size--;
			}

		} else {
			/* no  byte swapping */
			MEMCPY(pDest, (CO_CONST UNSIGNED8 *)pSrc + srcOffset, (size_t)size);
		}
#endif /* CO_CPU_DSP */
	}
}


#ifdef CO_CPU_DSP
/***************************************************************************/
/**
* \internal
*
* \brief memcpyUnpack_dsp - memcpy with numeric dependencies
*
* \return none
*
*/
CO_INLINE void memcpyUnpack_dsp(
		void		*pDest,			/* destination address (CAN buffer) */
		CO_CONST void	*pSrc,		/* source address */
		UNSIGNED32	cnt,			/* cnt in bytes */
		UNSIGNED32	srcOffset,		/* source offset */
		CORE_DATA_TYPE_T dType,		/* data type */
		UNSIGNED8	bigEndianProtocol	/* big endian protocol data */
	)
{
CO_CONST UNSIGNED8 *pSrc_u8;
typedef unsigned char U_DSP_T;
U_DSP_T	*pSrc_u32;
UNSIGNED8	*pDest_u8;
UNSIGNED8	packOffset;

	if (bigEndianProtocol != 0u)  {
		/* byte swapping necessary */
		pDest_u8 = (UNSIGNED8 *) pDest + cnt - 1u;
		pSrc_u8 = (CO_CONST UNSIGNED8 *) pSrc + srcOffset / CO_CPU_DSP_BYTESIZE;
		packOffset = srcOffset % CO_CPU_DSP_BYTESIZE;
	} else {
		/* src = src + offs (var addr + offs)
		 * dst = dst
		 */
		pDest_u8 = (UNSIGNED8 *) pDest;
		pSrc_u8 = (CO_CONST UNSIGNED8 *) pSrc + (srcOffset / CO_CPU_DSP_BYTESIZE);
		packOffset = srcOffset % CO_CPU_DSP_BYTESIZE;
	}
	pSrc_u32 = (U_DSP_T *) pSrc_u8;


	while (cnt > 0u)  {
		*pDest_u8 = (*pSrc_u32 >> (packOffset * 8u)) & 0xffu;

		if (bigEndianProtocol != 0u)  {
			pDest_u8 --;
		} else {
			pDest_u8 ++;
		}

		/* incr packOffset */
		packOffset++;

		if ((packOffset % CO_CPU_DSP_BYTESIZE) == 0u)  {
			packOffset = 0u;
			pSrc_u8 ++;
			pSrc_u32 = (U_DSP_T *) pSrc_u8;
		}

		cnt --;
	}
}
#endif /* CO_CPU_DSP */


/***************************************************************************/
/**
* \internal
*
* \brief coreMemcpy - memcpy with numeric dependencies
*
* This function provide an internal memcpy.
* If the parameter numeric is unequal 0,
* then the data are copied as numeric value,
* so byte swapping or unpackaging is done.
*
* \return none
*
*/
CO_INLINE BOOL_T coreMemcpy(
		void		*pDest,			/* destination address */
		CO_CONST void	*pSrc,		/* source address */
		UNSIGNED32	size,			/* size in bytes */
		UNSIGNED16	numeric			/* numeric copy */
	)
{
#ifdef CO_OBJECT_CHANGED
INTEGER32		cmp;
#endif /* CO_OBJECT_CHANGED */
UNSIGNED32	corrSize = size;
BOOL_T	changed = CO_FALSE;
(void)numeric;

#ifdef CO_CPU_DSP
	if (numeric != 0u)  {
		corrSize = size / CO_CPU_DSP_BYTESIZE;
		if (corrSize == 0u)  {
			corrSize = 1u;
		}
	}
#endif /* CO_CPU_DSP */

#ifdef CO_OBJECT_CHANGED
	cmp = MEMCMP(pDest, pSrc, (size_t)corrSize);
	if (cmp != 0)  {
		changed = CO_TRUE;
	}
#endif /* CO_OBJECT_CHANGED */

	MEMCPY(pDest, pSrc, (size_t)corrSize);

	return(changed);
}


/***************************************************************************/
/**
* \internal
*
* \brief getDTypeLen - get dataype len
*
* This function returns the datalen of given data type for numeric data
* (not for DSP !!)
*
* \return none
*
*/
static UNSIGNED32 getDTypeLen(
		CORE_DATA_TYPE_T dType		/* data type */
	)
{
UNSIGNED32	len = 0u;

	switch (dType)  {
	case CORE_DTYPE_U8:
	case CORE_DTYPE_I8:
		len = 1u;
		break;

	case CORE_DTYPE_U16:
	case CORE_DTYPE_I16:
		len = 2u;
		break;

	case CORE_DTYPE_U32:
	case CORE_DTYPE_I32:
		len = 4u;
		break;

#if defined(J1939_SUPPORTED) || defined(RAWCAN_SUPPORTED)
	case CORE_DTYPE_U64:
		len = 8u;
		break;
#endif /* defined(J1939_SUPPORTED) || defined(RAWCAN_SUPPORTED) */

	default:
		len = 1u;
		break;
	}

	return(len);
}


/***************************************************************************/
/**
* \internal
*
* \brief icoGetNumericDatatype - get numeric datatype for given len information
*
* This function returns the datatype for the given variable len
*
* \return CORE_DATA_TYPE_T
*
*/
CORE_DATA_TYPE_T icoGetNumericDatatype(
        UNSIGNED32 size			/* variable len */
    )
{
CORE_DATA_TYPE_T dtype;

	switch (size) {
	case 1u:
		dtype = CORE_DTYPE_U8;
		break;
	case 2u:
		dtype = CORE_DTYPE_U16;
		break;
	case 3u:
	case 4u:
		dtype = CORE_DTYPE_U32;
		break;
#if defined(J1939_SUPPORTED) || defined(RAWCAN_SUPPORTED)
	case 5u:
	case 6u:
	case 7u:
	case 8u:
			dtype = CORE_DTYPE_U64;
		break;
#endif /* defined(J1939_SUPPORTED) || defined(RAWCAN_SUPPORTED) */
	default:
		dtype = CORE_DTYPE_U8;
		break;
	}

	return(dtype);
}


/***************************************************************************/
/**
* \internal
*
* \brief coreMemcmp - memcmp with numeric dependencies
*
* This function provide an internal memcmp.
* If the parameter numeric is unequal 0,
* then the data are interpreted as numeric value,
*
* \return none
*
*/
INTEGER16 coreMemcmp(
		CO_CONST void	*pDest,		/* destination address */
		CO_CONST void	*pSrc,		/* source address */
		UNSIGNED32	size,			/* size in bytes */
		UNSIGNED16	numeric			/* numeric copy */
	)
{
UNSIGNED32	corrSize = size;
INTEGER16	changed;

#ifdef CO_CPU_DSP
	if (numeric != 0u)  {
		corrSize = size / CO_CPU_DSP_BYTESIZE;
		if (corrSize == 0u)  {
			corrSize = 1u;
		}
	}
#else /* CO_CPU_DSP */
	(void)numeric;
#endif /* CO_CPU_DSP */

	changed = (INTEGER16)MEMCMP(pDest, pSrc, (size_t)corrSize);

	return(changed);
}


/***************************************************************************/
/**
*
* \brief coreMemDataExchange32 - exchange U32 data
*
* This function exchanges the data of a given U32 value
* and returns an U32 value with all data are exchanged
* (big to little endian converting)
*
* \return U32
*
*/
UNSIGNED32 coreMemDataExchange32(
		UNSIGNED32 src
	)
{
UNSIGNED32	dst = 0ul;

	dst |= (src >> 24) & 0x000000ffu;
	dst |= (src >> 8) & 0x0000ff00u;
	dst |= (src << 8)  & 0x00ff0000u;
	dst |= (src << 24) & 0xff000000u;

	return(dst);
}


/***************************************************************************/
/**
*
* \brief coreMemDataExchange16 - exchange U16 data
*
* This function exchanges the data of a given U16 value
* and returns an U16 value with all data are exchanged
* (big to little endian converting)
*
* \return U16
*
*/
UNSIGNED16 coreMemDataExchange16(
		UNSIGNED16 src
	)
{
UNSIGNED16	dst = 0u;

	dst |= (src >> 8)  & 0x00ffu;
	dst |= (src << 8)  & 0xff00u;

	return(dst);
}
