/**
 * \file TLV.c
 * \brief TLV buffer API.
 * \author Dmytro Podgornyi (d.podgornyi@samsung.com)
 * \version 0.1
 * \date Created Nov 04, 2013
 * \par In Samsung Ukraine R&D Center (SURC) under a contract between
 * \par LLC "Samsung Electronics Ukraine Company" (Kiev, Ukraine) and
 * \par "Samsung Elecrtronics Co", Ltd (Seoul, Republic of Korea)
 * \par Copyright: (c) Samsung Electronics Co, Ltd 2012. All rights reserved.
 */

#include <string.h>

#include "CommLayerData.h"
#include "TLV.h"
#include "log.h"

#define ALIGN_PTR(ADDR) if((ADDR) % 4 == 0){ (ADDR) += 0; } \
                        else if((ADDR) % 4 == 1){ (ADDR) += 3; } \
                        else if((ADDR) % 4 == 2){ (ADDR) += 2; } \
                        else if((ADDR) % 4 == 3){ (ADDR) += 1; }

int tlvInit(uint8_t *tlv, uint32_t tlvLen)
{
	if (tlvLen < TAGLENGTH_FIELD_SIZE) {
		LOGE("Allocated TLV buffer too small\n");
		return WRONG_DATA;
	}

	memset(tlv, 0, tlvLen);
	tlv[0] = TLV_START;

	return NO_ERROR;
}

int tlvAdd(uint8_t *tlv, uint32_t tlvLen, TlvTag_t tag, const void *value, uint32_t valueLen)
{
	uint32_t len = 0;
	uint32_t ptr = 0;

	if ((NULL == value) && (0 != valueLen)) {
		LOGE("tlvAdd: Non-zero size of NULL pointer to TLV value\n");
		return WRONG_DATA;
	}

	if (tlvLen < TAGLENGTH_FIELD_SIZE) {
		LOGE("tlvAdd: TLV len is smaller than min size\n");
		return WRONG_DATA;
	}

	if (valueLen >= MAX_TRANSFER_SIZE - TAGLENGTH_FIELD_SIZE) {
		LOGE("tlvAdd: TLV len exceeds max transfer size\n");
		return WRONG_DATA;
	}

	if (tlv[0] != TLV_START) {
		LOGE("tlvAdd: Not valid TLV start tag\n");
		return WRONG_DATA;
	}

	len = GET_UINT16T_LE(1, tlv);
	if (len + 2 * TAGLENGTH_FIELD_SIZE + valueLen > tlvLen) {
		LOGE("tlvAdd: TLV len is smaller that data requested to add\n");
		return WRONG_DATA;
	}
	/* Set tag and length of requested data to add */
	tlv[len + TAGLENGTH_FIELD_SIZE] = tag;
	tlv[len + TAGLENGTH_FIELD_SIZE + 1] = (uint8_t)(valueLen & 0xff);
	tlv[len + TAGLENGTH_FIELD_SIZE + 2] = (uint8_t)((valueLen >> 8) & 0xff);
	ptr = len + TAGLENGTH_FIELD_SIZE + 3;

	ALIGN_PTR(ptr);
	len += TAGLENGTH_FIELD_SIZE + valueLen + (&(tlv[ptr]) - &tlv[len + 2 * TAGLENGTH_FIELD_SIZE]);
	if (len >= MAX_TRANSFER_SIZE - TAGLENGTH_FIELD_SIZE) {
		LOGE("tlvAdd: updated TLV len exceeds max transfer size\n");
		return PLATFORM_INTERNAL_ERROR;
	}
	
	if (0 != valueLen) {
		memcpy(&(tlv[ptr]), value, valueLen);
	}

	/* Update total TLV length */
	tlv[1] = (uint8_t)(len & 0xff);
	tlv[2] = (uint8_t)((len >> 8) & 0xff);

	return NO_ERROR;
}

int tlvSize(const uint8_t *tlv, uint32_t tlvLen)
{
	uint32_t len = 0;

	if (tlvLen < TAGLENGTH_FIELD_SIZE) {
		return WRONG_DATA;
	}

	len = GET_UINT16T_LE(1, tlv);
	if (len + TAGLENGTH_FIELD_SIZE > tlvLen) {
		return WRONG_DATA;
	}

	return (int)(len + TAGLENGTH_FIELD_SIZE);
}

int tlvGet(const uint8_t *tlv, uint32_t tlvLen, TlvTag_t tag, uint32_t* length, uint8_t** value)
{
	uint32_t len = 0;
	uint32_t ptr = 0;

	if (tlvLen < TAGLENGTH_FIELD_SIZE) {
		LOGE("tlvLen < TAGLENGTH_FIELD_SIZE\n");
		return WRONG_DATA;
	}

	if (tlv[0] != TLV_START) {
		LOGE("tlv[0] != TLV_START\n");
		return WRONG_DATA;
	}

	len = GET_UINT16T_LE(1, tlv);
	if (len != tlvLen - TAGLENGTH_FIELD_SIZE) {
		LOGE("len [%d] != tlvLen - TAGLENGTH_FIELD_SIZE [%d]\n", len, tlvLen - TAGLENGTH_FIELD_SIZE);
		return WRONG_DATA;
	}

	ptr = TAGLENGTH_FIELD_SIZE;
	while (tag != (TlvTag_t)tlv[ptr]) {
		len = GET_UINT16T_LE(1, (&(tlv[ptr])));
		ptr += TAGLENGTH_FIELD_SIZE;
		ALIGN_PTR(ptr);
		ptr += len;

		if (ptr >= tlvLen) {
			LOGE("No more TLV's, wrong tag %d\n", tag);
			return WRONG_TAG;
		}
	}

	*length = GET_UINT16T_LE(1, (&(tlv[ptr])));
	if (*length == 0) {
		*value = NULL;
	} else {
		ptr += TAGLENGTH_FIELD_SIZE;
		ALIGN_PTR(ptr);
		*value = &(tlv[ptr]);
		if (*length > MAX_TRANSFER_SIZE || (ptr + *length) > tlvLen) {
			LOGE(" length [%d] exceeds tlv data\n", *length);
			return WRONG_DATA;
		}
	}

	return NO_ERROR;
}