/*
* co_sdoblockclient.c - contains sdo block routines for client
*
* Copyright (c) 2013-2023 emotas embedded communication GmbH
*-------------------------------------------------------------------
* $Id: co_sdoblockclient.c 51912 2024-01-19 13:30:38Z boe $
*
*
*-------------------------------------------------------------------
*
*
*/

/********************************************************************/
/**
* \brief sdo block routines
*
* \file co_sdoblockclient.c
* contains sdo block transfer routines for client
*
*/


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

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

#ifdef CO_SDO_CLIENT_CNT
# ifdef CO_SDO_BLOCK
#include <co_datatype.h>
#include <co_memcpy.h>
#include <co_timer.h>
#include <co_drv.h>
#include <co_sdo.h>
#include <co_odaccess.h>
#include <co_nmt.h>
#include "ico_cobhandler.h"
#include "ico_queue.h"
#include "ico_event.h"
#include "ico_odaccess.h"
#include "ico_indication.h"
#ifdef CO_SDO_NETWORKING
# include "ico_sdoserver.h"
#endif /* CO_SDO_NETWORKING */
#include "ico_nmt.h"
#include "ico_sdo.h"
#include "ico_sdoclient.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 void sdoClientReadBlockconfirmation(CO_SDO_CLIENT_T *pSdo);
static void sdoClientWriteBlockEnd(CO_SDO_CLIENT_T *pSdo);
static void	sdoClientWriteBlock(void *pData);
static void sdoClientWriteBlockCont(CO_SDO_CLIENT_T *pSdo);

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

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

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


/***************************************************************************/
/**
* \internal
*
* \brief  icoSdoClientReadBlockInit
*
*
* \return none
*
*/
void icoSdoClientReadBlockInit(
		CO_SDO_CLIENT_T	*pSdo,				/* pointer to SDO */
		const CO_CAN_REC_MSG_T	*pRecData	/* pointer to receive data */
	)
{
UNSIGNED8	trData[CO_CAN_MAX_DATA_LEN];	/* data */
UNSIGNED32	len = 0u;

	if (pSdo->state != CO_SDO_CLIENT_STATE_BLK_UL_INIT)  {
		/* ignore message */
		return;
	}

	/* delete timer */
	(void)coTimerStop(&pSdo->timer);

#ifdef CO_SDO_BLOCK_CRC
	/* use CRC ? */
	if ((pRecData->data[0] & CO_SDO_SCS_BLOCK_UL_CRC) != 0u)  {
		pSdo->blockCrcUsed = CO_TRUE;
		pSdo->blockCrc = 0u;
	} else {
		pSdo->blockCrcUsed = CO_FALSE;
	}
#endif /* CO_SDO_BLOCK_CRC */

	/* size indicated ? */
	if ((pRecData->data[0] & CO_SDO_SCS_BLOCK_UL_SIZE) != 0u)  {
		/* segmented transfer, len in byte 4..7 */
		(void)coreMemcpyPack(&len, &pRecData->data[4], 4u, 0u, CORE_DTYPE_U32, 0u);
	} else {
		/* unspecified data length */
		len = pSdo->para.size;
	}

	/* received len > as internal buffer size */
	if (len > pSdo->para.size)  {
		/* send abort */
		icoSdoClientAbort(pSdo, RET_DATA_TYPE_MISMATCH);
		return;
	}

	trData[0] = CO_SDO_CCS_BLOCK_UPLOAD | CO_SDO_CCS_BLOCK_SC_UL_BLK;
	MEMSET(&trData[1], 0, 7u);

	/* transmit request */
	(void)icoTransmitMessage(pSdo->trCob, &trData[0], 0u);

	/* start timer */
	(void)coTimerStart(&pSdo->timer, pSdo->timeOutVal,
			icoSdoClientTimeOut, pSdo, CO_TIMER_ATTR_ROUNDUP); /*lint !e960 */
	/* Derogation MisraC2004 R.16.9 function identifier used without '&'
	 * or parenthesized parameter */

	pSdo->para.size = len;
	pSdo->restSize = len;
	pSdo->seqNr = 1u;
	pSdo->state = CO_SDO_CLIENT_STATE_BLK_UL_BLK;
}


/***************************************************************************/
/**
* \internal
*
* \brief  icoSdoClientReadBlock
*
*
* \return none
*
*/
void icoSdoClientReadBlock(
		CO_SDO_CLIENT_T	*pSdo,				/* pointer to SDO */
		const CO_CAN_REC_MSG_T	*pRecData	/* pointer to receive data */
	)
{
UNSIGNED32	len;
UNSIGNED32	packOffset = 0u;
CORE_DATA_TYPE_T dType;

	/* delete timer */
	(void)coTimerStop(&pSdo->timer);

	/* check for next correct block number */
	if ((pRecData->data[0] & (UNSIGNED8)~(UNSIGNED8)CO_SDO_SCS_BLOCK_UL_LAST)
				== pSdo->seqNr)  {

		/* block cnt reached ? */
		if (pSdo->seqNr <= pSdo->blockSize)  {
			pSdo->seqNr++;
		}

		/* last message ? */
		if ((pRecData->data[0] & CO_SDO_SCS_BLOCK_UL_LAST) != 0u)  {
			/* yes */
			/* save data temporary until block end was received */
			MEMCPY(&pSdo->saveBlockData[0], &pRecData->data[1], 7u);

			/* send response to server */
			sdoClientReadBlockconfirmation(pSdo);

			/* continue with block end sequence */
			pSdo->state = CO_SDO_CLIENT_STATE_BLK_UL_END;

			/* start timer */
			(void)coTimerStart(&pSdo->timer, pSdo->timeOutVal,
					icoSdoClientTimeOut, pSdo,
					CO_TIMER_ATTR_ROUNDUP); /*lint !e960 */
			/* Derogation MisraC2004 R.16.9 function identifier used without '&'
			 * or parenthesized parameter */

			return;
		}

		/* save data */
		if (pSdo->restSize > 7u)  {
			len = 7u;
		} else {
			len = pSdo->restSize;
		}

		/* received len > as internal buffer size */
		if (len > pSdo->restSize)  {
			/* send abort */
			icoSdoClientAbort(pSdo, RET_DATA_TYPE_MISMATCH);
			return;
		}

#ifdef CO_CPU_DSP
		if (pSdo->domain == CO_TRUE)  {
			packOffset = pSdo->para.size % CO_CPU_DSP_BYTESIZE;
		}
#endif /* CO_CPU_DSP */

		if (pSdo->numeric != 0u)  {
			dType = CORE_DTYPE_U8;
		} else {
			dType = CORE_DTYPE_NON_NUMERIC;
		}
		(void)coreMemcpyPack(pSdo->pData, &pRecData->data[1], len, packOffset,
			dType, 0u);
		pSdo->pData += len;
		pSdo->restSize -= len;

#ifdef CO_SDO_BLOCK_CRC
		if (pSdo->blockCrcUsed == CO_TRUE)  {
			pSdo->blockCrc = icoSdoCrcCalc(pSdo->blockCrc, &pRecData->data[1], len);
		}
#endif /* CO_SDO_BLOCK_CRC */
	}

	/* block cnt reached ? */
	if ((pRecData->data[0] & (UNSIGNED8)~(UNSIGNED8)CO_SDO_SCS_BLOCK_UL_LAST)
			== pSdo->blockSize)  {
		/* send response to server */
		sdoClientReadBlockconfirmation(pSdo);
	}

	/* start timer */
	(void)coTimerStart(&pSdo->timer, pSdo->timeOutVal,
			icoSdoClientTimeOut, pSdo,
			CO_TIMER_ATTR_ROUNDUP); /*lint !e960 */
	/* Derogation MisraC2004 R.16.9 function identifier used without '&'
	 * or parenthesized parameter */

}


/***************************************************************************/
/**
* \internal
*
* \brief  sdoClientReadBlockConfirmation
*
*
* \return none
*
*/
static void sdoClientReadBlockconfirmation(
		CO_SDO_CLIENT_T	*pSdo
	)
{
UNSIGNED8	trData[CO_CAN_MAX_DATA_LEN];	/* data */

	trData[0] = CO_SDO_CCS_BLOCK_UPLOAD | CO_SDO_CCS_BLOCK_SC_UL_CON;
	trData[1] = pSdo->seqNr - 1u;
	trData[2] = pSdo->blockSize;
	MEMSET(&trData[3], 0, 5u);

	/* transmit request */
	(void)icoTransmitMessage(pSdo->trCob, &trData[0], 0u);

	if ((pSdo->seqNr - 1u) == pSdo->blockSize)  {
		pSdo->seqNr = 1u;
	}
}


/***************************************************************************/
/**
* \internal
*
* \brief  icoSdoClientReadBlockEnd
*
*
* \return none
*
*/
void icoSdoClientReadBlockEnd(
		CO_SDO_CLIENT_T	*pSdo,				/* pointer to SDO */
		const CO_CAN_REC_MSG_T	*pRecData	/* pointer to receive data */
	)
{
UNSIGNED8	trData[CO_CAN_MAX_DATA_LEN];	/* data */
UNSIGNED32	packOffset = 0u;
UNSIGNED8	len;
#ifdef CO_SDO_BLOCK_CRC
UNSIGNED16	crc;
#endif /* CO_SDO_BLOCK_CRC */
CORE_DATA_TYPE_T dType;

	/* delete timer */
	(void)coTimerStop(&pSdo->timer);

	/* valid data from last transfer */
	len = 7u - ((pRecData->data[0] >> 2) & 0x7u);

#ifdef CO_CPU_DSP
	if (pSdo->domain == CO_TRUE)  {
		packOffset = pSdo->para.size % CO_CPU_DSP_BYTESIZE;
	}
#endif /* CO_DSP32 */

	if (pSdo->numeric != 0u)  {
		dType = CORE_DTYPE_U8;
	} else {
		dType = CORE_DTYPE_NON_NUMERIC;
	}
	(void)coreMemcpyPack(pSdo->pData, &pSdo->saveBlockData[0], (UNSIGNED32)len,
		packOffset, dType, 0u);

#ifdef CO_SDO_BLOCK_CRC
	if (pSdo->blockCrcUsed == CO_TRUE)  {
		pSdo->blockCrc = icoSdoCrcCalc(pSdo->blockCrc, &pSdo->saveBlockData[0],
			(UNSIGNED32)len);
		crc = pRecData->data[1] | (UNSIGNED16)((UNSIGNED16)pRecData->data[2] << 8u);
		/* printf("crc %x %x\n", pSdo->blockCrc, crc); */
		if (pSdo->blockCrc != crc)  {
			/* wrong CRC, abort */
			icoSdoClientAbort(pSdo, RET_SDO_CRC_ERROR);
			return;
		}
	}
#endif /* CO_SDO_BLOCK_CRC */

	trData[0] = CO_SDO_CCS_BLOCK_UPLOAD | CO_SDO_CCS_BLOCK_SC_UL_END;
	MEMSET(&trData[1], 0, 7u);

	/* transmit request */
	(void)icoTransmitMessage(pSdo->trCob, &trData[0], 0u);

	pSdo->state = CO_SDO_CLIENT_STATE_FREE;

	icoSdoClientUserInd(pSdo, CO_SDO_CLIENT_STATE_BLK_UL_END, 0u);
}


/***************************************************************************/
/**
* \internal
*
* \brief  icoSdoClientWriteBlockInit
*
*
* \return none
*
*/
void icoSdoClientWriteBlockInit(
		CO_SDO_CLIENT_T		*pSdo,			/* pointer to SDO */
		const CO_CAN_REC_MSG_T	*pRecData	/* pointer to receive data */
	)
{
	if (pSdo->state != CO_SDO_CLIENT_STATE_BLK_DL_INIT)  {
		/* ignore message */
		return;
	}

	/* delete timer */
	(void)coTimerStop(&pSdo->timer);

#ifdef CO_SDO_BLOCK_CRC
	/* use CRC ? */
	if ((pRecData->data[0] & CO_SDO_SCS_BLOCK_DL_CRC) != 0u)  {
		pSdo->blockCrcUsed = CO_TRUE;
#ifdef CO_EVENT_CSDO_DOMAIN_WRITE
		/* calculate CRC for domain indication only for first block */
		if ((pSdo->split == CO_TRUE) && (pSdo->pFunction != NULL))  {
			pSdo->blockCrc = icoSdoCrcCalc(0u, pSdo->pData, pSdo->msgCnt * 7);
		} else 
#endif /* CO_EVENT_CSDO_DOMAIN_WRITE */
		{
			pSdo->blockCrc = icoSdoCrcCalc(0u, pSdo->pData, pSdo->para.size);
		}
	} else {
		pSdo->blockCrcUsed = CO_FALSE;
	}
#endif /* CO_SDO_BLOCK_CRC */

	pSdo->blockSize = pRecData->data[4];
	if ((pSdo->blockSize == 0u) || (pSdo->blockSize > 127u))  {
		icoSdoClientAbort(pSdo, RET_SDO_WRONG_BLOCKSIZE);
		return;
	}

	pSdo->seqNr = 1u;

	sdoClientWriteBlock(pSdo);
}


/***************************************************************************/
/**
* \internal
*
* \brief  sdoClientWriteBlock
*
*
* \return none
*
*/
static void sdoClientWriteBlock(
		void			*pData
	)
{
CO_SDO_CLIENT_T	*pSdo = pData;
UNSIGNED8	trData[CO_CAN_MAX_DATA_LEN];	/* data */
UNSIGNED32	size;
UNSIGNED32	packOffset = 0u;
RET_T		retVal;
CORE_DATA_TYPE_T dType;

	trData[0] = pSdo->seqNr;
	MEMSET(&trData[1], 0, 7u);

	/* last transfer ? */
	if ((pSdo->para.size - pSdo->restSize) > 7u)  {
		size = 7u;
	} else {
		/* last transfer */
		size = pSdo->para.size - pSdo->restSize;
		trData[0] |= CO_SDO_SCS_BLOCK_DL_LAST;

		pSdo->state = CO_SDO_CLIENT_STATE_BLK_DL_ACQ;
	}

# ifdef CO_CPU_DSP
	if (pSdo->domain == CO_TRUE)  {
		packOffset = pSdo->restSize % CO_CPU_DSP_BYTESIZE;
	}
# endif /* CO_CPU_DSP */

	/* get data */
	if (pSdo->numeric != 0u)  {
		dType = CORE_DTYPE_U32;
	} else {
		dType = CORE_DTYPE_NON_NUMERIC;
	}
	coreMemcpyUnpack(&trData[1], pSdo->pData, (UNSIGNED32)size,
		packOffset, dType, 0u);

	/* transmit message */
	retVal = icoTransmitMessage(pSdo->trCob, &trData[0], 0u);

	if (retVal == RET_DRV_TRANS_BUFFER_FULL)  {
		/* transmit next message */
		(void)icoEventStart(&pSdo->blockEvent, sdoClientWriteBlock,
			(void *)pSdo); /*lint !e960 */
		/* Derogation MisraC2004 R.16.9 function identifier used without '&'
		 * or parenthesized parameter */

		return;
	}

	/* apply size information */
	pSdo->restSize += size;
	pSdo->pData += size;

	/* block count reached or last block ? */
	if ((pSdo->seqNr >= pSdo->blockSize) || (pSdo->restSize == pSdo->para.size))  {
		/* await acq from server */
		pSdo->state = CO_SDO_CLIENT_STATE_BLK_DL_ACQ;

		/* start timer */
		(void)coTimerStart(&pSdo->timer, pSdo->timeOutVal,
				icoSdoClientTimeOut, pSdo, CO_TIMER_ATTR_ROUNDUP); /*lint !e960 */
		/* Derogation MisraC2004 R.16.9 function identifier used without '&'
		 * or parenthesized parameter */
	} else {
		sdoClientWriteBlockCont(pSdo);
	}
}


/******************************************************************************/
/**
* \internal
*
* \brief sdoClientWriteBlockCont
*
*
*/
static void sdoClientWriteBlockCont(
		CO_SDO_CLIENT_T	*pSdo
	)
{
#ifdef CO_EVENT_CSDO_DOMAIN_WRITE
RET_T		retVal;

	/* splitted domain transfer ? */
	if (pSdo->split == CO_TRUE)  {
		/* size reached or last segment */
		if (((pSdo->restSize % (7u * pSdo->msgCnt)) == 0u)
			/*|| ((pRecData->data[0] & CO_SDO_CCS_CONT_BIT) != 0u)*/)  {
			/* domain indication */
			if (pSdo->pFunction != NULL)  {
				retVal = pSdo->pFunction(pSdo->para.sdoNr,
					pSdo->para.index, pSdo->para.subIndex, pSdo->restSize, pSdo->pFctData);

				/* set pData to start of buffer */
				pSdo->pData = pSdo->pDomainData;

# ifdef CO_SDO_BLOCK_CRC
				/* use CRC ? */
				if (pSdo->blockCrcUsed == CO_TRUE)  {
					/* calculate CRC for next block */
					UNSIGNED32	size;

					size = pSdo->para.size - pSdo->restSize;
					if (size > (pSdo->msgCnt * 7u))  {
						size = pSdo->msgCnt * 7u;
					}
					pSdo->blockCrc = icoSdoCrcCalc(pSdo->blockCrc, pSdo->pData, size);
				}
# endif /* CO_SDO_BLOCK_CRC */

# ifdef CO_SDO_CLIENT_DOMAIN_SPLIT_INDICATION
				/* split indication ? */
				if (retVal == RET_SDO_SPLIT_INDICATION)  {
					/* stop timeout timer */
					(void)coTimerStop(&pSdo->timer);
					pSdo->splitIndication = CO_TRUE;
					return;
				}

				pSdo->splitIndication = CO_FALSE;
# endif /* CO_SDO_CLIENT_DOMAIN_SPLIT_INDICATION */

				if (retVal != RET_OK)  {
					icoSdoClientAbort(pSdo, retVal);
					return;
				}
			}
		}
	}
#endif /* CO_EVENT_CSDO_DOMAIN_WRITE */

	pSdo->seqNr++;

	/* transmit next message */
	(void)icoEventStart(&pSdo->blockEvent, sdoClientWriteBlock,
		(void *)pSdo); /*lint !e960 */
	/* Derogation MisraC2004 R.16.9 function identifier used without '&'
	 * or parenthesized parameter */
}


/******************************************************************************/
/**
* \internal
*
* \brief icoSdoClientDomainWriteBlockIndCont - continue SDO Client Domain Write indication
*
*
*/
void icoSdoClientDomainWriteBlockIndCont(
		CO_SDO_CLIENT_T	*pSdo
	)
{
	pSdo->seqNr++;
	sdoClientWriteBlock(pSdo);
}


/***************************************************************************/
/**
* \internal
*
* \brief  icoSdoClientWriteBlockAcq
*
*
* \return none
*
*/
void icoSdoClientWriteBlockAcq(
		CO_SDO_CLIENT_T		*pSdo,
		const CO_CAN_REC_MSG_T	*pRecData
	)
{
UNSIGNED8	blk;
UNSIGNED32	size;

	if (pSdo->state != CO_SDO_CLIENT_STATE_BLK_DL_ACQ)  {
		/* ignore message */
		return;
	}

	/* delete timer */
	(void)coTimerStop(&pSdo->timer);

	/* set new blocksize */
	pSdo->blockSize = pRecData->data[2];
	if ((pSdo->blockSize < 1u) || (pSdo->blockSize > 127u))  {
		icoSdoClientAbort(pSdo, RET_SDO_WRONG_BLOCKSIZE);
		return;
	}

	/* check last seqNr */
	if (pRecData->data[1] == pSdo->seqNr)  {
		/* ok, start next block */

		/* all data transfered ? */
		if ((pSdo->para.size - pSdo->restSize) == 0u)  {
			/* send block end */
			sdoClientWriteBlockEnd(pSdo);
		} else {
			/* start next block */
			pSdo->seqNr = 0u;
			sdoClientWriteBlockCont(pSdo);
		}
	} else {
		/* wrong sequence number ? */
		if (pRecData->data[1] > 127u)  {
			icoSdoClientAbort(pSdo, RET_SDO_WRONG_SEQ_NR);
			return;
		}

		/* start with last correct received block + 1 */
		pSdo->seqNr = pRecData->data[1] + 1u;

		/* calculate blocks to be repeated */
		blk = pSdo->blockSize - pRecData->data[1];
		if (pSdo->para.size == pSdo->restSize)  {
			/* all blocks already sent - calculate last block byte cnt */
			/* size of last block */
			size = (pSdo->para.size % (pSdo->blockSize * 7ul))
					- (pRecData->data[1] * 7ul);
		} else {
			/* not all blocks sent */
			size = blk * 7ul;
		}

		pSdo->pData -= size;
		pSdo->restSize -= size;

		/* start next block */
		sdoClientWriteBlock(pSdo);
	}
}


/***************************************************************************/
/**
* \internal
*
* \brief  sdoClientWriteBlockEnd
*
*
* \return none
*
*/
static void sdoClientWriteBlockEnd(
		CO_SDO_CLIENT_T	*pSdo
	)
{
UNSIGNED8 size;
UNSIGNED8	trData[CO_CAN_MAX_DATA_LEN];	/* data */

	trData[0] = CO_SDO_CCS_BLOCK_DOWNLOAD | CO_SDO_CCS_BLOCK_CS_DL_END;
	MEMSET(&trData[1], 0, 7u);

# ifdef CO_SDO_BLOCK_CRC
	if (pSdo->blockCrcUsed == CO_TRUE)  {
		/* BLOCK_TEST D6 Start */
		/* pSdo->blockCrc = 0; */
		/* BLOCK_TEST D6 End */

		trData[1] = (UNSIGNED8)(pSdo->blockCrc & 0xffu);
		trData[2] = (UNSIGNED8)((pSdo->blockCrc >> 8) & 0xffu);
	}
# endif /* CO_SDO_BLOCK_CRC */

	/* calculate the unused byte in the last segment */
	size = (UNSIGNED8)(pSdo->para.size % 7u);

	if (size != 0u)  {
		trData[0] |= (UNSIGNED8)((7u - size) << 2);
	}

	/* transmit answer */
	(void)icoTransmitMessage(pSdo->trCob, &trData[0], 0u);

	pSdo->state = CO_SDO_CLIENT_STATE_BLK_DL_END;

	/* start timer */
	(void)coTimerStart(&pSdo->timer, pSdo->timeOutVal,
			icoSdoClientTimeOut, pSdo, CO_TIMER_ATTR_ROUNDUP); /*lint !e960 */
	/* Derogation MisraC2004 R.16.9 function identifier used without '&'
	 * or parenthesized parameter */
}

# endif /* CO_SDO_BLOCK_CLIENT */
#endif /* CO_SDO_CLIENT_CNT */