//**************************************************************************************
/*! \copyright:     2020-2021 Thales Alenia Space Deutschland GmbH
* \project:         multiMIND
* \file:            (name of source file: hdlc.c)
* \date:            (09.02.2022)
* \author:          (Stelios Filippopoulos)
* \brief:           (hdlc functions)
* \language:        (C)
**************************************************************************************
*/

#include "tas/hdlc.h"
#include "tas/crc.h"

#include <stdint.h>

void hdlc_add_byte(uint8_t ch, uint8_t *buff, size_t *pos)
{
	size_t templen = *pos;

	if ((ch == 0x7E) ||
	    (ch == 0x7D) ||
		(ch == 0x7C))
	{
		buff[templen++] = 0x7D;
		ch ^= 0x20;
	}
	buff[templen++] = ch;

	*pos = templen;
}

void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen)
{
	size_t tlen = 0;
	uint16_t ii;
	uint16_t crc16;
	uint8_t  bt;

	// calc crc16
	crc16 = calc_crc16_buff_reflected( src, slen );

	dst[tlen++] = 0x7E;
	for (ii = 0; ii < slen; ii++)
	{
		bt = *src++;
		hdlc_add_byte(bt, dst, &tlen);
	}

	// hdlc crc16 is in little endian format
	// WARNING: This is not portable code! Bytes need to be swapped on a big
	// endian system
	// TODO: Fix
	hdlc_add_byte((uint8_t) (crc16 & 0xFF), dst, &tlen);
	hdlc_add_byte((uint8_t) ((crc16 >> 8) & 0xFF), dst, &tlen);

	dst[tlen++] = 0x7C;
	*dlen = tlen;
}

int hdlc_remove_framing_with_crc_check(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen)
{
	uint16_t tlen = 0;
	uint16_t ii;
	uint8_t  bt;

	*dlen = 0;
	if (slen < 4) return -1;
	if ((src[tlen] != 0x7E) && (src[slen-1] != 0x7C)) return -2;
	src++;
	for (ii = 1; ii < slen-1; ii++)
	{
		bt = *src++;

		if (bt == 0x7D)
		{
			bt = *src++ ^ 0x20;
			ii++;
		}
		dst[tlen++] = bt;
	}
	// calc crc16
	// TODO: Warning: This does not work because the CRC16 is little endian
	if(calc_crc16_buff_reflected( dst,  tlen ) != 0) {
		return 1;
	}
	*dlen = tlen - 2;
	return 0;
}