#include <stdio.h>
#include <string.h>
#include <ctype.h>
#include <errno.h>
#include <unistd.h>
#include <gfaserial.h>
#include <gfamininetmst.h>
#include <byteswap.h>
#include "bl_commands.h"
#include "gfabootlmast.h"

/////////////////////////////////////////////////////////////////////////////

uint8_t GfaBLM_BU_DataCheckSum(const void *pData, size_t nCbData)
{
	uint8_t chk = 0;
	const uint8_t *pbData = (const uint8_t*)pData;
	while(nCbData--) {
		chk += *pbData++;
	}
	return chk;
}

/////////////////////////////////////////////////////////////////////////////

size_t GfaBLM_BuildCmdDataPacket(const char *pszCmd, const void *pCmdData, size_t nCbCmdData, void *pPacket, size_t nCbPacket, bool bAddLenAndCheck)
{
	size_t nLen = strlen(pszCmd);
	uint8_t *pbPacket = (uint8_t*)pPacket;
	
	if(nCbPacket < (nLen + nCbCmdData + ((pCmdData && nCbCmdData && bAddLenAndCheck) ? 2 : 0)))
		return 0;

	memcpy(pbPacket, pszCmd, nLen);
	pbPacket += nLen;

	if(pCmdData && nCbCmdData)
	{
		if(bAddLenAndCheck)
		{
			*pbPacket++ = nCbCmdData + 2;
			*pbPacket++ = GfaBLM_BU_DataCheckSum(pCmdData, nCbCmdData);
			nLen += 2;
		}
		memcpy(pbPacket, pCmdData, nCbCmdData);
		nLen += nCbCmdData;
	}

	return nLen;
}