#include <stdint.h>
#include <tee_ta_api.h>
#include <tee_api.h>

#include "tz_platform.h"
#include "cmd_handler.h"

static TEE_Result ta_entry_panic(uint32_t param_types, TEE_Param params[4]);
static TEE_Result ta_entry_wait(uint32_t param_types, TEE_Param params[4]);
static TEE_Result ta_entry_params(uint32_t param_types, TEE_Param params[4]);
/*
 * Trusted Application Entry Points
 */

/* Called each time a new instance is created */
TEE_Result TA_CreateEntryPoint(void)
{
	return TEE_SUCCESS;
}

/* Called each time an instance is destroyed */
void TA_DestroyEntryPoint(void)
{
}

/* Called each time a session is opened */
TEE_Result TA_OpenSessionEntryPoint(uint32_t nParamTypes,
				    TEE_Param pParams[4],
				    void **ppSessionContext)
{
	(void)nParamTypes;
	(void)pParams;
	(void)ppSessionContext;
	return TEE_SUCCESS;
}

/* Called each time a session is closed */
void TA_CloseSessionEntryPoint(void *pSessionContext)
{
	(void)pSessionContext;
}

/* Called when a command is invoked */
TEE_Result TA_InvokeCommandEntryPoint(void *pSessionContext,
				      uint32_t nCommandID, uint32_t nParamTypes,
				      TEE_Param pParams[4])
{
    TEE_Result ret = TEE_SUCCESS;
    p_cmd_t    pCmd;
    p_rsp_t    pRsp;

    cmd_t cmd_buf;
    rsp_t rsp_buf;

    (void)pSessionContext;

    /* param[0]/param[1]: memref, param[2]/param[3]: none */
    if (nParamTypes != TEE_PARAM_TYPES(TEE_PARAM_TYPE_MEMREF_INPUT, TEE_PARAM_TYPE_MEMREF_OUTPUT, TEE_PARAM_TYPE_NONE, TEE_PARAM_TYPE_NONE)) {
        /* Bad parameter types */
        LOGE("ParamTypes is not matched");
        return TEE_ERROR_BAD_PARAMETERS;
    }

    if (pParams[0].memref.buffer == NULL || pParams[1].memref.buffer == NULL) {
        LOGE("Invalid TEE_Param");
        return ret;
    }

    pCmd = (p_cmd_t) pParams[0].memref.buffer;
    pRsp = (p_rsp_t) pParams[1].memref.buffer;

    /* Validate buffers. Both rsp & cmd should be allocated despite of further usage */
    if ((pCmd == NULL) || (pRsp == NULL) || 
        (pCmd->dataLen > MAX_DATA_SIZE) ||
        (pParams[0].memref.size != sizeof(cmd_t)) || 
        (pParams[1].memref.size != sizeof(rsp_t))) {
        LOGE("Invalid cmd or rsp pointers or size");
        return TEE_ERROR_BAD_PARAMETERS;
    }

    /* Replicate buffer to avoid of shared memory usage */
    TEE_MemMove(&cmd_buf, pCmd, sizeof(cmd_t));
    TEE_MemMove(&rsp_buf, pRsp, sizeof(rsp_t));
    pCmd = &cmd_buf;
    pRsp = &rsp_buf;

    cmdHandler(nCommandID, pCmd, pRsp);

    /* Copy back rsp buffer to shared memory */
    pRsp = (p_rsp_t) pParams[1].memref.buffer;
    TEE_MemMove(pRsp, &rsp_buf, sizeof(rsp_t));

    return TEE_SUCCESS;
}

static TEE_Result ta_entry_panic(uint32_t param_types, TEE_Param params[4])
{
	volatile bool mytrue = true;
	(void)param_types;
	(void)params;

	/*
	 * Somewhat clumsy way of avoiding compile errors if TEE_Panic() has
	 * the __noreturn attribute.
	 */
	if (mytrue)
		TEE_Panic(0xbeef);

	/*
	 * Should not be reached, but if it is the testsuite can detect that
	 * TEE_Panic() returned instead of panicking the TA.
	 */
	return TEE_SUCCESS;
}

static TEE_Result ta_entry_wait(uint32_t param_types, TEE_Param params[4])
{
	TEE_Result res = TEE_SUCCESS;
	(void)param_types;

	/* Wait */
	res = TEE_Wait(params[0].value.a);

	return res;
}

//buffer add ->buffer
static TEE_Result ta_entry_params(uint32_t param_types, TEE_Param params[4])
{
	size_t n = 0, i = 0, t = 0,size;
	char addend, operator, *pin, *pout;

	if (param_types != TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
					   TEE_PARAM_TYPE_VALUE_OUTPUT,
					   TEE_PARAM_TYPE_MEMREF_INPUT,
					   TEE_PARAM_TYPE_MEMREF_OUTPUT))
	return TEE_ERROR_BAD_PARAMETERS;

	if (!params[2].memref.buffer || !params[3].memref.buffer)
			return TEE_ERROR_BAD_PARAMETERS;

	if (params[2].memref.size != params[3].memref.size)
			return TEE_ERROR_BAD_PARAMETERS;

	addend = params[0].value.a;
	operator = params[0].value.b; //1:add 0:sub

	size = params[2].memref.size;
	pin = (char*)params[2].memref.buffer;
	pout = (char*)params[3].memref.buffer;

	if(operator) {
		for(i = 0; i < size; i++) {
			pout[i] = pin[i] + addend;
		}
	} else {
		for(i = 0; i < size; i++) {
			if(pin[i] < addend) {
				pout[i] = addend - pin[i];
				t++;
			} else {
				pout[i] = pin[i] - addend;
			}
		}
	}

	params[1].value.a = t;

	return TEE_SUCCESS;
}