/*
* co_mapping.c - contains routines for co mapping
*
* Copyright (c) 2022-2024 emotas embedded communication GmbH
*-------------------------------------------------------------------
* $Id: co_mapping.c 54343 2024-06-14 07:54:23Z boe $
*
*
*-------------------------------------------------------------------
*
*
*/

/********************************************************************/
/**
* \brief co mapping routines
*
* \file co_mapping.c
* contains routines for co mapping
*
*/

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

/* header of project specific types
---------------------------------------------------------------------------*/
#include <gen_define.h>
#include <co_datatype.h>
#include <co_memcpy.h>
#include "ico_mapping.h"

/* constant definitions
---------------------------------------------------------------------------*/

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

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

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

/* list of local defined functions
---------------------------------------------------------------------------*/
static UNSIGNED32 startMapEntry(UNSIGNED8 buf[], UNSIGNED16	bufLen,
		UNSIGNED32 mapStart, UNSIGNED32 bufStart, UNSIGNED32 bufEnd,
		const void *pVar, CORE_DATA_TYPE_T dType, UNSIGNED32 mapEntryLen);

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

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

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


/***************************************************************************/
/**
* \internal
*
* \brief ico_transmitMap - create transmit mapping
*
* This function has to be called for each mapping entry
* until the mapping is done (returns with RET_OK).
* If the current mapping entry is not inside the current buffer
* the function returns RET_SERVICE_BUSY
*
*
*/
RET_T ico_transmitMap(
		UNSIGNED16	mapNr,			/* mapping entry number */
		const void	*pVar,			/* pointer to mapping var */
		CORE_DATA_TYPE_T	dType,	/* mapping data type */
		UNSIGNED32	mapEntryLen,	/* mapping len in bits */
		UNSIGNED8	buf[],			/* mapping buffer */
		UNSIGNED16	bufLen,			/* buffer len inside buf[] in bytes */
		UNSIGNED32	bufOffs			/* already transmitted offset in bytes (p.e. nr. of message*7)  */
	)
{
UNSIGNED32	bufStart;			/* buffer start in bits */
UNSIGNED32	bufEnd;				/* buffer end in bits */
static UNSIGNED32	mappedBits;	/* already mapped bits */

	if (mapNr == 0u)  {
		/* erase buffer */
		MEMSET(buf, 0, (size_t)bufLen);
		mappedBits = 0u;
	}

	/* calculate buffer start/end in bits */
	bufStart = bufOffs << 3;
	bufEnd = (bufOffs + bufLen) << 3;

	/* complete mapping entry before bufstart? */
	if ((mappedBits + mapEntryLen) < bufStart)  {
		mappedBits += mapEntryLen;

		return(RET_SERVICE_BUSY);
	} else {
		/* no - mapping starts before bufend ?  */
		if (mappedBits < bufEnd)  {
			/* start mapping */
			UNSIGNED32 offs = startMapEntry(buf, bufLen, mappedBits, bufStart, bufEnd,
						pVar, dType, mapEntryLen); /*lint !e960 LINT_ico_transmitMap_1 */
			/* Note: pointer arithmetic other than array indexing used */
			mappedBits += offs;
			if (mappedBits >= bufEnd)  {
				return(RET_OK);
			}
		}
	}

	return(RET_SERVICE_BUSY);
}


/***************************************************************************/
/**
* \internal
*
* \brief startMapEntry - map one entry
*
* is only called from ij1939_transmitMap()
*
*
*/
static UNSIGNED32 startMapEntry(
		UNSIGNED8		buf[],
		UNSIGNED16		bufLen,		/* transmit buffer len (in bytes) */
		UNSIGNED32		mapStart,	/* start bit for mapping entry inside complete msg */
		UNSIGNED32		bufStart,	/* start bit inside actual buffer */
		UNSIGNED32		bufEnd,		/* end bit inside actual buffer */
		const void		*pVar,		/* pointer to variable */
		CORE_DATA_TYPE_T dType,
		UNSIGNED32		mapEntryLen	/* mapping entry len in bits */
	)
{
UNSIGNED32 bits;		/* zu versendende bits von diesem mapping entry */
UNSIGNED8	tmpU8;
UNSIGNED16	tmpU16;
UNSIGNED32	retBits;
UNSIGNED32 	transmitted;	/* schon versendete bits von diesem map entry */
UNSIGNED32	bufIdx;
UNSIGNED32	bufOffs;
UNSIGNED32	usedBits;

	/* start inside transmit buffer ? */
	if (mapStart > bufStart)  {
		/* mapping starts inside transmit buffer ? */
		transmitted = 0u;
		bufOffs = mapStart - bufStart;
		bufIdx = bufOffs >> 3;
	} else {
		/* mapping starts before transmit buffer */
		transmitted = bufStart - mapStart;	/* schon versendete bits von diesem map entry */
		bufIdx = 0u;
		bufOffs = 0u;
	}

	bits = mapEntryLen - transmitted;
	retBits = mapEntryLen;
	/* mapentry doesnt fit into this buffer */
	if (bits > ((bufEnd - bufStart) - bufOffs))  {
		bits = (bufEnd - bufStart) - bufOffs;
		retBits = bits + transmitted;
	}


	while (bits != 0u)  {
		/* copy from real variable */
		/* lock OD */
		CO_OS_LOCK_OD

		coreMemcpyUnpack(&tmpU8, pVar, 1u, (transmitted >> 3),
						dType, 0u);
		/* unlock OD */
		CO_OS_UNLOCK_OD

		/* shifte nach rechts bis zur ersten 8-bit Grenze */
		usedBits = 8u - (transmitted % 8u);
		if (usedBits > bits)  {
			usedBits = bits;
		}
		tmpU8 = tmpU8 >> (transmitted % 8u);

		/* muss geshiftet werden? */
		if (((bufOffs % 8u) == 0u) && ((transmitted % 8u) == 0u))  {
			/* nein */
			buf[bufIdx] = tmpU8 & 0xffu;
		} else {
			/* left shift for transmit buffer */
			tmpU16 = tmpU8;
			tmpU16 = (UNSIGNED16)(tmpU16 << (bufOffs % 8u));

			buf[bufIdx] |= (UNSIGNED8)(tmpU16 & 0xffu);
			if ((bufIdx + 1u) < bufLen)  {
				buf[bufIdx + 1u] |= (UNSIGNED8)((tmpU16 >> 8) & 0xffu);
			}
		}

		if (usedBits == 8u)  {
			bufIdx++;
		}
		transmitted += usedBits;
		bufOffs += usedBits;
		bits -= usedBits;
	}

	return(retBits);
}


/***************************************************************************/
/**
* \internal
*
* \brief ico_receiveUnmap - unmap received data
*
*
* return changed
*/
BOOL_T ico_receiveUnmap(
		const UNSIGNED16	mapNr,		/* mapping entry number */
		void				*pVar,		/* optional destination pointer */
		const CORE_DATA_TYPE_T dType,	/* mapping data type */
		const UNSIGNED32	mapLen,		/* mapping len in bits */
		const UNSIGNED8		buf[],		/* pointer receive buffer */
		UNSIGNED32			bufStart,	/* startbyte at receive data */
		UNSIGNED32			bufEnd		/* endbyte at received data */
	)
{
static UNSIGNED32	mapCnt;		/* Mapping Counter over all mappings */
static UNSIGNED32	recBufIdx;	/* index at receive buffer */
static UNSIGNED32	recBufBits;	/* bits at receive buffer */
UNSIGNED32	mapEntryLen;		/* len of actual mapping entry */
UNSIGNED32	mapEntryOffs;		/* offset inside mapping entry */
UNSIGNED32	j;
UNSIGNED32	x;
UNSIGNED8	tmpU8;
UNSIGNED16	tmpU16;
UNSIGNED32	bits;
UNSIGNED32	offs;
UNSIGNED32	cnt;
BOOL_T		changed = CO_FALSE;

	/* calculate in bits */
	bufStart <<= 3;
	bufEnd <<= 3;
	mapEntryOffs = 0u;

	if (mapNr == 0u)  {
		mapCnt = 0u;
		recBufIdx = 0u;		/* index at receive buffer */
		recBufBits = 0u;	/* bits at receive buffer */
	}

	mapEntryLen = mapLen;

	/* mapping starts before actual buffer ? */
	if (mapCnt < bufStart)  {
		/* actual mapping end < bufStart */
		if ((mapCnt + mapEntryLen) < bufStart)  {
			mapCnt += mapEntryLen;
			mapEntryLen = 0u;
		} else {
			/* maybe above bufStart */
			if (dType >= CORE_DTYPE_NON_NUMERIC)  {
				/* end sign before bufStart ? */
				x = mapLen;
				if ((mapCnt + x) <= bufStart)  {
					/* correct mapEntry len */
					mapEntryLen = x;
					mapCnt += mapEntryLen;
					mapEntryLen = 0u;
				} else {
					mapEntryOffs = bufStart - mapCnt;
					mapEntryLen -= mapEntryOffs;
					mapCnt += mapEntryOffs;
				}
			} else {
				mapEntryOffs = bufStart - mapCnt;
				mapEntryLen -= mapEntryOffs;
				mapCnt += mapEntryOffs;
			}
		}
	}

	/* len > buffer available? */
	if (mapEntryLen > (bufEnd - bufStart))  {
		/* set max data len */
		mapEntryLen = bufEnd - bufStart;
	}

	/* mapping entry start inside received buffer? */
	if ((mapCnt >= bufStart) && (mapCnt < bufEnd))  {
		/* ok, then map */
		/* check for end sign at data buffer */
		if (dType == CORE_DTYPE_STRING_ASTERIKS)  {
			for (j = 0u; j < (mapEntryLen >> 3); j++)  {
				if (buf[recBufIdx + j] == (UNSIGNED8)'*')  {
					mapEntryLen = (j + 1u) << 3;
					break;
				}
			}
		}
		if (dType == CORE_DTYPE_STRING_NULL)  {
			for (j = 0u; j < (mapEntryLen >> 3); j++)  {
				if (buf[recBufIdx + j] == 0u)  {
					mapEntryLen = (j + 1u) << 3;
					break;
				}
			}
		}

		if (((mapLen % 8u) == 0u) && ((mapCnt % 8u) == 0u))  {

			/* correct buffer start */
			if ((mapCnt % 8u) != 0u)  {
				bits = 8u - (mapCnt % 8u);
				mapCnt += bits;
				recBufBits += bits;
				recBufIdx = (recBufBits >> 3);
			}
			/* lock OD */
			CO_OS_LOCK_OD

			changed = coreMemcpyPack(pVar, &buf[recBufIdx],
					mapEntryLen >> 3, mapEntryOffs >> 3,
					dType, 0u);

			/* unlock OD */
			CO_OS_UNLOCK_OD
		} else {

			/* bitwise mapping */

			offs = 0u;
			bits = mapEntryLen;
			while (bits != 0u)  {

				tmpU16 = 0u;
				if (bits > (8u - (mapCnt % 8u)))  {
					cnt = 2u;
				} else {
					cnt = 1u;
				}
				(void)coreMemcpyPack(&tmpU16, &buf[recBufIdx + offs], cnt,
					0u, CORE_DTYPE_U16, 0u);

				/* shift */
				tmpU16 >>= (mapCnt % 8u);
				/* mask */
				if (bits < 8u)  {
					tmpU16 &= (UNSIGNED16)((UNSIGNED16)0xffu >> (8u - bits));
				}

				tmpU8 = (UNSIGNED8)(tmpU16 & 0xffu);

				/* copy to real variable */
				/* lock OD */
				CO_OS_LOCK_OD

				changed = coreMemcpyPack((UNSIGNED8 *)pVar, &tmpU8, 1u,
					offs, dType, 0u);
				/* unlock OD */
				CO_OS_UNLOCK_OD

				if (bits > 8u)  {
					bits -= 8u;
					offs ++;
				} else {
					bits = 0u;
				}
			}
		}

		mapCnt += mapEntryLen;
		recBufBits += mapEntryLen;
		recBufIdx = (recBufBits >> 3);
	}

	return(changed);
}


