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

#include "sem_TA.h"

#ifdef USE_TEEGRIS 
#include <tees_extension.h>
#endif

TEE_Result TA_CreateEntryPoint(void) {
    return TEE_SUCCESS;
}

void TA_DestroyEntryPoint(void) {
}

TEE_Result TA_OpenSessionEntryPoint(uint32_t paramTypes, TEE_Param params[4], void **sessionContext) {
    (void) paramTypes;
    (void) params;
    (void) sessionContext;

    return TEE_SUCCESS;
}

void TA_CloseSessionEntryPoint(void *sessionContext) {
    (void) sessionContext;
    return;
}

TEE_Result TA_InvokeCommandEntryPoint(void *sessionContext, uint32_t commandId, uint32_t paramTypes, TEE_Param params[4]) {
    TEE_Result ret = TEE_SUCCESS;
    p_cmd_t    pCmd;
    p_rsp_t    pRsp;

    cmd_t cmd_buf;
    rsp_t rsp_buf;

    (void) sessionContext;

    /* param[0]/param[1]: memref, param[2]/param[3]: none */
    if (paramTypes != TEE_PARAM_TYPES(TEE_PARAM_TYPE_MEMREF_INPUT, TEE_PARAM_TYPE_MEMREF_OUTPUT, TEE_PARAM_TYPE_NONE, TEE_PARAM_TYPE_NONE)) {
        /* Bad parameter types */
        return TEE_ERROR_BAD_PARAMETERS;
    }

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

#ifdef USE_TEEGRIS 
    /* param[0]: INPUT */
    if (TEES_IsREESharedMemory (TEE_MEMORY_ACCESS_READ, 
        params[0].memref.buffer, params[0].memref.size) != TEE_SUCCESS) {
        printf("Memory access check error\n");
        return TEE_ERROR_ACCESS_DENIED;
    }

    /* param[1]: OUT */
    if (TEES_IsREESharedMemory (TEE_MEMORY_ACCESS_WRITE, 
        params[1].memref.buffer, params[1].memref.size) != TEE_SUCCESS) {
        printf("Memory access check error\n");
        return TEE_ERROR_ACCESS_DENIED;
    }
#endif

    pCmd = (p_cmd_t) params[0].memref.buffer;
    pRsp = (p_rsp_t) params[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) ||
        (params[0].memref.size != sizeof(cmd_t)) || 
        (params[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(commandId, pCmd, pRsp);

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

    return ret;
}