/*
 *
 * (c) Copyright 2018 Samsung Research America, Inc.
 *                  All rights reserved
 *
 *                  MCL-B2B Lab
 *
 *
 * File: utils.c
 * Author: r.kadir@samsung.com
 * Creation Date: Nov, 2018
 *
 */
/**
* Copyright (C) 2018 Samsung Electronics Co., Ltd. All rights reserved.
*
* Mobile Platform & Solutions Lab (MPS Lab),
* Samsung Electronics Co., Ltd.
*
* This software and its documentation are confidential and proprietary
* information of Samsung Electronics Co., Ltd.  No part of the software and
* documents may be copied, reproduced, transmitted, translated, or reduced to
* any electronic medium or machine-readable form without the prior written
* consent of Samsung Electronics.
*
* Samsung Electronics makes no representations with respect to the contents,
* and assumes no responsibility for any errors that might appear in the
* software and documents. This publication and the contents hereof are subject
* to change without notice.
*/
#include "DDAR_TZ_Vendor_tl.h"
#include "dualdar_api.h"
#include "utils.h"
#include "dualdar_globals.h"
#include "base64.h"

#define FIRST_LEN_SIZE 							1
#define MAX_SECOND_LEN_SIZE 					4
char ddar_log_msg[LOG_MSG_SIZE];
static uint8_t hexMapLower[] =
    { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', 'c', 'd', 'e',
	'f'
};

static uint8_t hexMapUpper[] =
    { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E',
	'F'
};

static int32_t hexMapLen = sizeof(hexMapLower);

static uint8_t tohexchar(
	uint8_t nibble,
	uint8_t isUpper
);

static uint8_t fromhexchar(
	uint8_t c
);

static const char *getErrorString(
	uint32_t errorCode
);

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

void tobytes(
	uint8_t * hexstr,
	uint32_t hexstrlen,
	uint8_t * bytes
)
{
	if (hexstr == 0)
		return;

	int32_t i = 0;
	for (i = 0; i < hexstrlen; i += 2) {
		bytes[i / 2] =
		    (fromhexchar(hexstr[i]) << 4) | fromhexchar(hexstr[i + 1]);
	}

	if (hexstrlen % 2) {
		bytes[i / 2] =
		    (fromhexchar(hexstr[i]) << 4) | fromhexchar(hexstr[i + 1]);
	}
}

void tohexstring(
	uint8_t * bytes,
	int32_t byteslen,
	uint8_t * hexstring,
	uint8_t isUpper
)
{
	int32_t i = 0;
	for (i = 0; i < byteslen; i++) {
		hexstring[i * 2] = tohexchar((bytes[i] & 0xf0) >> 4, isUpper);
		hexstring[i * 2 + 1] = tohexchar(bytes[i] & 0x0f, isUpper);
	}
}

uint32_t add_data_to_buf(
	uint8_t * data,
	uint32_t datalen,
	uint8_t * buf,
	uint32_t maxbuflen,
	uint32_t * offset
)
{
	uint32_t ret = DUALDAR_OK;
	if ((*offset + datalen) >= maxbuflen)
		return DUALDAR_UNKNOWN_FAILURE;

	memcpy(buf + *offset, data, datalen);
	*offset += datalen;

	return ret;
}

/////////////////////////////////////////////////////////
// Internal Utilities
//////////////////////////////////////////////////////////

uint32_t convertBytes2UInt32(
	uint8_t * bytes,
	int32_t numbytes
)
{
	int32_t i;
	uint32_t outint = 0;

	for (i = 0; i < numbytes; i++) {
		outint = (outint << 8) | bytes[i];
	}

	return outint;
}

void convertUInt16ToBytes(
	uint16_t i,
	uint8_t * bytes
)
{
	bytes[0] = (i >> 8) & 0xff;
	bytes[1] = i & 0xff;
}

void convertUInt32ToBytes(
	uint32_t i,
	uint8_t * bytes
)
{
	bytes[0] = (i >> 24) & 0xff;
	bytes[1] = (i >> 16) & 0xff;
	bytes[2] = (i >> 8) & 0xff;
	bytes[3] = i & 0xff;
}

int32_t convertIntToVarBytes(
	uint32_t tlvlen,
	uint8_t * firstbyte,
	uint8_t * nextbytes
)
{
	int32_t nextbyteslen = 0;
	if (tlvlen <= 0x7f) {
		*firstbyte = (uint8_t) tlvlen;
		return 0;
	}

	if (tlvlen > 0xffffff) {
		nextbyteslen = 4;
		nextbytes[0] = (tlvlen & 0xff000000) >> 24;
		nextbytes[1] = (tlvlen & 0x00ff0000) >> 16;
		nextbytes[2] = (tlvlen & 0x0000ff00) >> 8;
		nextbytes[3] = tlvlen & 0x000000ff;
	} else if (tlvlen > 0xffff) {
		nextbyteslen = 3;
		nextbytes[0] = (tlvlen & 0x00ff0000) >> 16;
		nextbytes[1] = (tlvlen & 0x0000ff00) >> 8;
		nextbytes[2] = tlvlen & 0x000000ff;
	} else if (tlvlen > 0xff) {
		nextbyteslen = 2;
		nextbytes[0] = (tlvlen & 0x0000ff00) >> 8;
		nextbytes[1] = tlvlen & 0x000000ff;
	} else if (tlvlen > 0x7f) {
		nextbyteslen = 1;
		nextbytes[0] = tlvlen & 0x000000ff;
	}

	*firstbyte = 0x80 | nextbyteslen;

	return nextbyteslen;
}

/////////////////////////////////////////////////////////
// Dump Functions
//////////////////////////////////////////////////////////
#define MAX_COLS 64
void print_str(
	uint8_t * label,
	uint8_t * buf,
	uint32_t buf_len
)
{
	DUALDAR_LOG("Dump called for %s with ptr = %p and len = %d: ", label, buf,
		 buf_len);

	uint8_t line[MAX_COLS + 1] = { 0 };
	uint32_t numlines = buf_len / MAX_COLS;
	uint32_t numcolslastline = buf_len % MAX_COLS;

	uint32_t lineidx = 0;

	for (lineidx = 0; lineidx < numlines; lineidx++) {
		memcpy(line, buf + lineidx * MAX_COLS, MAX_COLS);
		DUALDAR_LOG("%s", line);
	}

	if (numcolslastline > 0) {
		memset(line, 0, MAX_COLS + 1);
		memcpy(line, buf + lineidx * MAX_COLS, numcolslastline);
		DUALDAR_LOG("%s", line);
	}
}

void fill_output_buf(
	uint8_t * buf,
	uint32_t buf_len,
	uint32_t val
)
{
	int32_t i = 0;
	DUALDAR_LOG("fill_output_buf : %p, %d", buf, buf_len);
	for (i = 0; i < buf_len; i++) {
		buf[i] = val;
	}
}

void print_int(
	uint8_t * label,
	uint32_t i
)
{
	DUALDAR_LOG("Dump called for %s value = %d: ", label, i);
}

void print_buf(
	uint8_t * label,
	uint8_t * buf,
	uint32_t buf_len
)
{
	int i = 0;
	DUALDAR_LOG("Dump called for %s with ptr = %p and len = %d: ", label, buf,
		 buf_len);
	for (i = 0; i < buf_len; i += 8) {
		DUALDAR_LOG
		    ("%02X %02X %02X %02X %02X %02X %02X %02X",
		     buf[i], buf[i + 1], buf[i + 2], buf[i + 3], buf[i + 4],
		     buf[i + 5], buf[i + 6], buf[i + 7]);
	}
}

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

static uint8_t tohexchar(
	uint8_t nibble,
	uint8_t isUpper
)
{
	if (nibble < hexMapLen) {
		if (isUpper)
			return hexMapLower[nibble];
		else
			return hexMapUpper[nibble];
	}
	return '*';
}

static uint8_t fromhexchar(
	uint8_t c
)
{
	if (c >= '0' && c <= '9')
		return c - '0';
	if (c >= 'a' && c <= 'f')
		return c - 'a' + 10;
	if (c >= 'A' && c <= 'F')
		return c - 'A' + 10;
	return 255;
}

uint32_t convertStringToInteger(
	uint8_t * s
)
{
	uint32_t num = 0;
	uint32_t i = 0;
	uint8_t digit;

	while (s[i] != 0) {
		if (s[i] < '0' && s[i] > '9')
			return 0;

		digit = s[i] - '0';
		num = (num * 10) + digit;
		i++;
	}
	return num;
}

void set_error_string(
	uint32_t errcode,
	uint8_t * destbuf
)
{
	const char *errstr = getErrorString(errcode);
	uint32_t errstrlen = strlen(errstr);
	if (errstrlen > DUALDAR_MAX_ERROR_STR_LEN) {
		errstrlen = DUALDAR_MAX_ERROR_STR_LEN;
	}
	_strncpy((char *)destbuf, errstr, errstrlen);
}


static const char *getErrorString(
	uint32_t errorCode
)
{

	char *errorString = 0;
	switch (errorCode) {
	case DUALDAR_OK:
		errorString = DUALDAR_OK_STR;
		break;
	case DUALDAR_INVALID_INPUT_PARAM:
		errorString = DUALDAR_INVALID_INPUT_PARAM_STR;
		break;
	case DUALDAR_UNKNOWN_FAILURE:
	default:
		errorString = DUALDAR_UNKNOWN_FAILURE_STR;
		break;
	}

	return (const char *)errorString;
}

char *
_strncpy(char *dst, const char *src, size_t n)
{
	if (n != 0) {
		char *d = dst;
		const char *s = src;

		do {
			if ((*d++ = *s++) == 0) {
				/* NUL pad the remaining n-1 bytes */
				while (--n != 0)
					*d++ = 0;
				break;
			}
		} while (--n != 0);
	}
	return (dst);
}


void dbg_dump(
	char * label,
	uint8_t * data,
	uint32_t data_len
)
{
	uint32_t i;
	char buf[50] = { 0 };
	DUALDAR_LOG(">>DBG_DUMP: %s(%d)", label, data_len);

	for (i = 0; i < data_len; i++) {
	    int offset = 3 * (i % 16);
		snprintf(buf + offset, 50-offset, "%02X ", *(data + i));
		if (i % 16 == 15) {
			DUALDAR_LOG("%s", buf);
			memset(buf, 0, sizeof(buf));
		}
	}
	DUALDAR_LOG("%s", buf);
}