/*
 * app_main.c
 */

#include <stdio.h>
#include <string.h>
#include <unistd.h>

#include <tee_internal_api.h>
#include <tees_extension.h>

#include "app_property.h"

#include "app_main.h"

#include "icccOperations_v4.h"

static tciMessage_t sendMsgCopy;
static tciMessage_t rspMsgCopy;

#if defined(TEST1)
#define TA_TAG "TA_TEST:"
#elif defined(TEST2)
#define TA_TAG "TA_TEST2:"
#elif defined(TEST3)
#define TA_TAG "TA_TEST3:"
#else
#define TA_TAG "NA: "
#endif

TEE_Result TA_CreateEntryPoint(void)
{
    printf("%s TA_CreateEntryPoint \n", TA_TAG);
    return TEE_SUCCESS;
}

void TA_DestroyEntryPoint(void)
{
    printf("%s TA_DestroyEntryPoint \n", TA_TAG);
}

TEE_Result TA_OpenSessionEntryPoint(uint32_t paramTypes, TEE_Param params[4], void **sessionContext)
{
    printf("%s TA_OpenSessionEntryPoint \n", TA_TAG);
    (void) paramTypes;
    (void) params;
    (void) sessionContext;

    return TEE_SUCCESS;
}

void TA_CloseSessionEntryPoint(void *sessionContext)
{
    printf("%s TA_CloseSessionEntryPoint \n", TA_TAG);
    (void) sessionContext;
}

uint32_t test_saveData(tz_test_generic_payload_t *sendmsg, tz_test_generic_payload_t *respmsg)
{
    uint32_t ret = ICCC_SUCCESS;
    uint32_t type = sendmsg->content.iccc_req.type;
    uint32_t temp = 0xcafebabe;
    uint8_t verified_boot_hash[32] = {0x52, 0x94, 0x31, 0xc2, 0x09, 0xce, 0x01, 0x2a, 0xda, 0xda, 0x58, 0xc4, 0xc9, 0x7f, 0xf2, 0xb4,
                                      0xa2, 0x23, 0x68, 0x90, 0xf7, 0x27, 0x39, 0xb1, 0x8e, 0xd0, 0xda, 0xe3, 0xcb, 0x48, 0x98, 0x2c };
    kern_secure_info_t kern_secure_info;
    memcpy(&kern_secure_info.verified_boot_hash, verified_boot_hash, sizeof(verified_boot_hash));

    switch (type) {
        case SECTIMER_STATUS:
            printf("%s SECTIMER_STATUS value %d \n", TA_TAG, temp);
            if (ICCC_SUCCESS == Iccc_SaveData_TA(SECTIMER_STATUS, temp)) {
                printf("%s ICCC write success for SECTIMER_STATUS \n", TA_TAG);
            } else {
                printf("%s ICCC write failure for SECTIMER_STATUS \n", TA_TAG);
                ret = ICCC_ERROR_WRITE_FAILED;
            }
            break;
        case DMV_STATUS:
            printf("%s DMV_STATUS value %d \n", TA_TAG, temp);
            if (ICCC_SUCCESS == Iccc_SaveData_TA(DMV_STATUS, temp)) {
                printf("%s ICCC write success for DMV_STATUS \n", TA_TAG);
            } else {
                printf("%s ICCC write failure for DMV_STATUS \n", TA_TAG);
                ret = ICCC_ERROR_WRITE_FAILED;
            }
            break;
        case TRUSTBOOT_FLAG:
            printf("%s TRUSTBOOT_FLAG value %d \n", TA_TAG, temp);
            if (ICCC_SUCCESS == Iccc_SaveData_TA(TRUSTBOOT_FLAG, temp)) {
                printf("%s ICCC write success for TRUSTBOOT_FLAG \n", TA_TAG);
            } else {
                printf("%s ICCC write failure for TRUSTBOOT_FLAG \n", TA_TAG);
                ret = ICCC_ERROR_WRITE_FAILED;
            }
            break;
        case VERIFIEDBOOT_HASH:
            printf("%s VERIFIEDBOOT_HASH value \n", TA_TAG);
            for (int cnt = 0; cnt < 8; cnt++) {
                printf("%s verified_boot_hash[%d] %#x", TA_TAG, cnt, kern_secure_info.verified_boot_hash[cnt]);
            }
            if (ICCC_SUCCESS == Iccc_SaveData_TA(VERIFIEDBOOT_HASH, 0)) { // only uint32_t values can be supported in TEEGRIS
                printf("%s ICCC write success for VERIFIEDBOOT_HASH \n", TA_TAG);
            } else {
                printf("%s ICCC write failure for VERIFIEDBOOT_HASH \n", TA_TAG);
                ret = ICCC_ERROR_WRITE_FAILED;
            }
            break;
        default:
            printf("%s Iccc_SaveData_TA failure with unknown type \n", TA_TAG);
            ret = ICCC_UNKNOWN_TYPE;
            break;
    }

    respmsg->content.iccc_rsp.ret = ret;
    return ret;
}

uint32_t test_readData(tz_test_generic_payload_t *sendmsg, tz_test_generic_payload_t *respmsg)
{
    uint32_t ret = ICCC_SUCCESS;
    uint32_t type = sendmsg->content.iccc_req.type;
    uint32_t flag = 1;

    bl_secure_info_t bl_secure_info;
    ta_secure_info_t ta_secure_info;
    kern_secure_info_t kern_secure_info;
    sys_secure_info_t sys_secure_info;
    rot_secure_info_t rot_secure_info;
    uint8_t attestation_blob_hash[32] = {0, };
    uint8_t verified_boot_hash[32] = {0, };   
    uint8_t verified_boot_key[32] = {0, };
    rot_bl_struct_t rot_bl_struct;

    switch (type) {
        case WARRANTY_BIT:
            if (ICCC_SUCCESS == Iccc_ReadData_TA(WARRANTY_BIT, &flag)) {
                printf("%s ICCC read success for WARRANTY_BIT \n", TA_TAG);
                if (flag == 0) {
                    printf("%s Warranty_bit valid - (%d) \n", TA_TAG, flag);
                } else {
                    printf("%s Warranty_bit invalid - (%d) \n", TA_TAG, flag);
                }
                respmsg->content.iccc_rsp.value = flag;
            } else {
                printf("%s ICCC read failure for WARRANTY_BIT \n", TA_TAG);
                ret = ICCC_ERROR_READ_FAILED;
            }
            break;
        case TRUSTBOOT_FLAG:
            if (ICCC_SUCCESS == Iccc_ReadData_TA(TRUSTBOOT_FLAG, &flag)) {
                printf("%s ICCC read success for TRUSTBOOT_FLAG \n", TA_TAG);
                if (flag == 0) {
                    printf("%s SVB measurements match (svb1.0) - (%d) \n", TA_TAG, flag);
                } else {
                    printf("%s SVB measurements mismatch (svb1.0) - (%d) \n", TA_TAG, flag);
                }
                respmsg->content.iccc_rsp.value = flag;
            } else {
                printf("%s ICCC read failure for TRUSTBOOT_FLAG \n", TA_TAG);
                ret = ICCC_ERROR_READ_FAILED;
            }
            break;
        case ATN_BLOB_HASH:
            memset(attestation_blob_hash, 0x00, sizeof(attestation_blob_hash));

            if (ICCC_SUCCESS == Iccc_ReadData_TA(ATN_BLOB_HASH, (uint32_t *) attestation_blob_hash)) {
                printf("%s ICCC read success for ATN_BLOB_HASH \n", TA_TAG);
                memcpy(&ta_secure_info.atn_blob_hash, attestation_blob_hash, sizeof(attestation_blob_hash));
                for (int cnt = 0; cnt < 8; cnt++) {
                    printf("%s atn_blob_hash[%d] %#x \n", TA_TAG, cnt, ta_secure_info.atn_blob_hash[cnt]);
                }
                memcpy(&respmsg->content.iccc_rsp.ta_secure_info, &ta_secure_info, sizeof(ta_secure_info));
            } else {
                printf("%s ICCC read failure for ATN_BLOB_HASH \n", TA_TAG);
                ret = ICCC_ERROR_READ_FAILED;
            }
            break;
        case VERIFIEDBOOT_HASH:
            memset(verified_boot_hash, 0x00, sizeof(verified_boot_hash));

            if (ICCC_SUCCESS == Iccc_ReadData_TA(VERIFIEDBOOT_HASH, (uint32_t *) verified_boot_hash)) {
                printf("%s ICCC read success for VERIFIEDBOOT_HASH \n", TA_TAG);
                memcpy(&kern_secure_info.verified_boot_hash, verified_boot_hash, sizeof(verified_boot_hash));
                for (int cnt = 0; cnt < 8; cnt++) {
                    printf("%s verified_boot_hash[%d] %#x \n", TA_TAG, cnt, kern_secure_info.verified_boot_hash[cnt]);
                }
                memcpy(&respmsg->content.iccc_rsp.kern_secure_info, &kern_secure_info, sizeof(kern_secure_info));
            } else {
                printf("%s ICCC read failure for VERIFIEDBOOT_HASH \n", TA_TAG);
                ret = ICCC_ERROR_READ_FAILED;
            }
            break;
        case ROT_STRUCT:
            memset(&rot_bl_struct, 0x00, sizeof(rot_bl_struct));

            if (ICCC_SUCCESS == Iccc_ReadData_TA(ROT_STRUCT, (uint32_t *) &rot_bl_struct)) {
                printf("%s ICCC read success for ROT_STRUCT \n", TA_TAG);
                printf("%s verified_boot_state %#x \n", TA_TAG, rot_bl_struct.verified_boot_state);
                printf("%s device_locked %#x \n", TA_TAG, rot_bl_struct.device_locked);
                printf("%s os_version %#x \n", TA_TAG, rot_bl_struct.os_version);
                printf("%s patch_month_year %#x \n", TA_TAG, rot_bl_struct.patch_month_year);
                for (int cnt = 0; cnt < 8; cnt++) {
                    printf("%s verified_boot_key[%d] %#x \n", TA_TAG, cnt, rot_bl_struct.verified_boot_key[cnt]);
                }
                printf("%s boot_patch_level %#x \n", TA_TAG, rot_bl_struct.boot_patch_level);
                printf("%s vendor_patch_level %#x \n", TA_TAG, rot_bl_struct.vendor_patch_level);
                memcpy(&respmsg->content.iccc_rsp.rot_secure_info, &rot_bl_struct, sizeof(rot_bl_struct));
            } else {
                printf("%s ICCC read failure for ROT_STRUCT \n", TA_TAG);
                ret = ICCC_ERROR_READ_FAILED;
            }
            break;
        case 0xFFFFFFFF: // All Flags
            memset(&bl_secure_info, 0x00, sizeof(bl_secure_info));
            memset(&ta_secure_info, 0x00, sizeof(ta_secure_info));
            memset(&kern_secure_info, 0x00, sizeof(kern_secure_info));
            memset(&sys_secure_info, 0x00, sizeof(sys_secure_info));
            memset(&rot_secure_info, 0x00, sizeof(rot_secure_info));
            memset(attestation_blob_hash, 0x00, sizeof(attestation_blob_hash));
            memset(verified_boot_hash, 0x00, sizeof(verified_boot_hash));
            memset(verified_boot_key, 0x00, sizeof(verified_boot_key));

            Iccc_ReadData_TA(RP_VER, &bl_secure_info.rp_ver);
            printf("%s rp_ver %#x \n", TA_TAG, bl_secure_info.rp_ver);
            Iccc_ReadData_TA(KERNEL_RP, &bl_secure_info.kernel_rp);
            printf("%s kernel_rp %#x \n", TA_TAG, bl_secure_info.kernel_rp);
            Iccc_ReadData_TA(SYSTEM_RP, &bl_secure_info.system_rp);
            printf("%s system_rp %#x \n", TA_TAG, bl_secure_info.system_rp);
            Iccc_ReadData_TA(TEST_BIT, &bl_secure_info.test_bit);
            printf("%s test_bit %#x \n", TA_TAG, bl_secure_info.test_bit);
            Iccc_ReadData_TA(SEC_BOOT, &bl_secure_info.sec_boot);
            printf("%s sec_boot %#x \n", TA_TAG, bl_secure_info.sec_boot);
            Iccc_ReadData_TA(REACT_LOCK, &bl_secure_info.react_lock);
            printf("%s react_lock %#x \n", TA_TAG, bl_secure_info.react_lock);
            Iccc_ReadData_TA(KIWI_LOCK, &bl_secure_info.kiwi_lock);
            printf("%s kiwi_lock %#x \n", TA_TAG, bl_secure_info.kiwi_lock);
            Iccc_ReadData_TA(FRP_LOCK, &bl_secure_info.frp_lock);
            printf("%s frp_lock %#x \n", TA_TAG, bl_secure_info.frp_lock);
            Iccc_ReadData_TA(CC_MODE, &bl_secure_info.cc_mode);
            printf("%s cc_mode %#x \n", TA_TAG, bl_secure_info.cc_mode);
            Iccc_ReadData_TA(MDM_MODE, &bl_secure_info.mdm_mode);
            printf("%s mdm_mode %#x \n", TA_TAG, bl_secure_info.mdm_mode);
            Iccc_ReadData_TA(CURR_BIN_STATUS, &bl_secure_info.curr_bin_status);
            printf("%s curr_bin_status %#x \n", TA_TAG, bl_secure_info.curr_bin_status);
            Iccc_ReadData_TA(AFW_VALUE, &bl_secure_info.afw_value);
            printf("%s afw_value %#x \n", TA_TAG, bl_secure_info.afw_value);
            Iccc_ReadData_TA(WARRANTY_BIT, &bl_secure_info.warranty_bit);
            printf("%s warranty_bit %#x \n", TA_TAG, bl_secure_info.warranty_bit);
            Iccc_ReadData_TA(KAP_STATUS, &bl_secure_info.kap_status);
            printf("%s kap_status %#x \n", TA_TAG, bl_secure_info.kap_status);
            for (int cnt = 0; cnt < 12; cnt++) {
                Iccc_ReadData_TA(IMAGE_STATUS1+cnt, &bl_secure_info.image_status[cnt]);
                printf("%s image_status[%d] %#x \n", TA_TAG, cnt, bl_secure_info.image_status[cnt]);
            }
            Iccc_ReadData_TA(IMAGE_STATUS_BL, &bl_secure_info.image_status_bl);
            printf("%s image_status_bl %#x \n", TA_TAG, bl_secure_info.image_status_bl);
            Iccc_ReadData_TA(WB_HISTORY, &bl_secure_info.WbHistory);
            printf("%s WbHistory %#x \n", TA_TAG, bl_secure_info.WbHistory);
            Iccc_ReadData_TA(AP_SERIAL_0, &bl_secure_info.ap_serial_0);
            printf("%s ap_serial_0 %#x \n", TA_TAG, bl_secure_info.ap_serial_0);
            Iccc_ReadData_TA(AP_SERIAL_1, &bl_secure_info.ap_serial_1);
            printf("%s ap_serial_1 %#x \n", TA_TAG, bl_secure_info.ap_serial_1);
            Iccc_ReadData_TA(EM_STATUS, &bl_secure_info.em_status);
            printf("%s em_status %#x \n", TA_TAG, bl_secure_info.em_status);
            Iccc_ReadData_TA(EM_TOKEN, &bl_secure_info.em_token);
            printf("%s em_token %#x \n", TA_TAG, bl_secure_info.em_token);
            Iccc_ReadData_TA(SECTIMER_BASE, &ta_secure_info.sectimer_base);
            printf("%s sectimer_base %#x \n", TA_TAG, ta_secure_info.sectimer_base);
            Iccc_ReadData_TA(SECTIMER_FLAG, &ta_secure_info.sectimer_flag);
            printf("%s sectimer_flag %#x \n", TA_TAG, ta_secure_info.sectimer_flag);
            Iccc_ReadData_TA(SECTIMER_STATUS, &ta_secure_info.sectimer_status);
            printf("%s sectimer_status %#x \n", TA_TAG, ta_secure_info.sectimer_status);
            Iccc_ReadData_TA(ATN_BLOB_HASH, (uint32_t *) attestation_blob_hash);
            memcpy(&ta_secure_info.atn_blob_hash, attestation_blob_hash, sizeof(attestation_blob_hash));
            for (int cnt = 0; cnt < 8; cnt++) {
                printf("%s atn_blob_hash[%d] %#x \n", TA_TAG, cnt, ta_secure_info.atn_blob_hash[cnt]);
            }
            Iccc_ReadData_TA(DMV_STATUS, &kern_secure_info.dmv_status);
            printf("%s dmv_status %#x \n", TA_TAG, kern_secure_info.dmv_status);
            Iccc_ReadData_TA(VERIFIEDBOOT_HASH, (uint32_t *) verified_boot_hash);
            memcpy(&kern_secure_info.verified_boot_hash, verified_boot_hash, sizeof(verified_boot_hash));
            for (int cnt = 0; cnt < 8; cnt++) {
                printf("%s verified_boot_hash[%d] %#x \n", TA_TAG, cnt, kern_secure_info.verified_boot_hash[cnt]);
            }
            Iccc_ReadData_TA(TRUSTBOOT_FLAG, &sys_secure_info.trustboot_flag);
            printf("%s trustboot_flag %#x \n", TA_TAG, sys_secure_info.trustboot_flag);
            Iccc_ReadData_TA(TIMA_VERSION_FLAG, &sys_secure_info.tima_version_flag);
            printf("%s tima_version_flag %#x \n", TA_TAG, sys_secure_info.tima_version_flag);
            Iccc_ReadData_TA(VERIFIEDBOOTSTATE_FLAG, &rot_secure_info.verified_boot_state);
            printf("%s verified_boot_state %#x \n", TA_TAG, rot_secure_info.verified_boot_state);
            Iccc_ReadData_TA(DEVICELOCK_FLAG, &rot_secure_info.device_locked);
            printf("%s device_locked %#x \n", TA_TAG, rot_secure_info.device_locked);
            Iccc_ReadData_TA(OSVERSION_FLAG, &rot_secure_info.os_version);
            printf("%s os_version %#x \n", TA_TAG, rot_secure_info.os_version);
            Iccc_ReadData_TA(PATCHMONTHYEAR_FLAG, &rot_secure_info.patch_month_year);
            printf("%s patch_month_year %#x \n", TA_TAG, rot_secure_info.patch_month_year);
            Iccc_ReadData_TA(VERIFIEDBOOTKEY_FLAG, (uint32_t *) verified_boot_key);
            memcpy(&rot_secure_info.verified_boot_key, verified_boot_key, sizeof(verified_boot_key));
            for (int cnt = 0; cnt < 8; cnt++) {
                printf("%s verified_boot_key[%d] %#x \n", TA_TAG, cnt, rot_secure_info.verified_boot_key[cnt]);
            }
            Iccc_ReadData_TA(BOOTPATCHLEVEL_FLAG, &rot_secure_info.boot_patch_level);
            printf("%s boot_patch_level %#x \n", TA_TAG, rot_secure_info.boot_patch_level);
            Iccc_ReadData_TA(VENDORPATCHLEVEL_FLAG, &rot_secure_info.vendor_patch_level);
            printf("%s vendor_patch_level %#x \n", TA_TAG, rot_secure_info.vendor_patch_level);

            memcpy(&respmsg->content.iccc_rsp.bl_secure_info, &bl_secure_info, sizeof(bl_secure_info));
            memcpy(&respmsg->content.iccc_rsp.ta_secure_info, &ta_secure_info, sizeof(ta_secure_info));
            memcpy(&respmsg->content.iccc_rsp.kern_secure_info, &kern_secure_info, sizeof(kern_secure_info));
            memcpy(&respmsg->content.iccc_rsp.sys_secure_info, &sys_secure_info, sizeof(sys_secure_info));
            memcpy(&respmsg->content.iccc_rsp.rot_secure_info, &rot_secure_info, sizeof(rot_secure_info));

            break;
        default:
            printf("%s Iccc_ReadData_TA failure with unknown type \n", TA_TAG);
            ret = ICCC_UNKNOWN_TYPE;
            break;
    }

    respmsg->content.iccc_rsp.ret = ret;
    return ret;
}

uint32_t test_getDeviceStatus(tz_test_generic_payload_t *sendmsg, tz_test_generic_payload_t *respmsg)
{
    uint32_t ret = ICCC_SUCCESS;
    uint32_t type = sendmsg->content.iccc_req.type;
    uint8_t resp_msg_buf[ICCC_STATUS_MAX_RESULT_MESSAGE] = {0, };
    uint32_t resp_msg_len = 0;
    uint32_t result_code = 1;

    switch (type) {
        case ICCC_STATUS_COMP_TYPE_SOFT_INTEGRITY:
            printf("%s ICCC_STATUS_COMP_TYPE_SOFT_INTEGRITY \n", TA_TAG);
            if (ICCC_SUCCESS == Iccc_DeviceStatus_TA(type, resp_msg_buf, sizeof(resp_msg_buf), &resp_msg_len, &result_code)) {
                if (result_code == 0) {
                    printf("%s Device integrity check (%d) is success - (%s - %d), (0x%08x) \n", TA_TAG, type, resp_msg_buf, resp_msg_len, result_code);
                } else {
                    printf("%s Device integrity check (%d) is failed - (%s - %d), (0x%08x) \n", TA_TAG, type, resp_msg_buf, resp_msg_len, result_code);
                }
                respmsg->content.iccc_rsp.result_code = result_code;
            } else {
                printf("%s ICCC integrity check failure with Iccc_DeviceStatus_TA \n", TA_TAG);
                ret = ICCC_ERROR_DEVICE_STATUS_FAILED;
            }
            break;
        case ICCC_STATUS_COMP_TYPE_HARD_INTEGRITY:
            printf("%s ICCC_STATUS_COMP_TYPE_HARD_INTEGRITY \n", TA_TAG);
            if (ICCC_SUCCESS == Iccc_DeviceStatus_TA(type, resp_msg_buf, sizeof(resp_msg_buf), &resp_msg_len, &result_code)) {
                if (result_code == 0) {
                    printf("%s Device integrity check (%d) is success - (%s - %d), (0x%08x) \n", TA_TAG, type, resp_msg_buf, resp_msg_len, result_code);
                } else {
                    printf("%s Device integrity check (%d) is failed - (%s - %d), (0x%08x) \n", TA_TAG, type, resp_msg_buf, resp_msg_len, result_code);
                }
                respmsg->content.iccc_rsp.result_code = result_code;
            } else {
                printf("%s ICCC integrity check failure with Iccc_DeviceStatus_TA \n", TA_TAG);
                ret = ICCC_ERROR_DEVICE_STATUS_FAILED;
            }
            break;
        default:
            printf("%s Iccc_DeviceStatus_TA failure with unknown type \n", TA_TAG);
            ret = ICCC_UNKNOWN_TYPE;
            break;
    }

    respmsg->content.iccc_rsp.ret = ret;
    return ret;
}

uint32_t test_negative(tz_test_generic_payload_t *sendmsg, tz_test_generic_payload_t *respmsg)
{
    uint32_t ret = ICCC_SUCCESS;
    uint32_t ng_flag = 0;

    if (ICCC_SUCCESS == Iccc_ReadData_TA(0xFFFFFFFF, &ng_flag)) {
        printf("%s ICCC read success with unsupported flag \n", TA_TAG);
    } else {
        printf("%s ICCC read failure with unsupported flag \n", TA_TAG);
        ret = ICCC_ERROR_READ_FAILED;
    }

    if (ICCC_SUCCESS == Iccc_SaveData_TA(0xFFFFFFFF, 0)) {
        printf("%s ICCC write success for unsupported flag \n", TA_TAG);
    } else {
        printf("%s ICCC write failure for unsupported flag \n", TA_TAG);
        ret = ICCC_ERROR_WRITE_FAILED;
    }

    uint32_t type = ICCC_STATUS_COMP_TYPE_DIR_INTEGRITY;;
    uint8_t resp_msg_buf[ICCC_STATUS_MAX_RESULT_MESSAGE] = {0, };
    uint32_t resp_msg_len = 0;
    uint32_t result_code = 1;
    if (ICCC_SUCCESS == Iccc_DeviceStatus_TA(type, resp_msg_buf, sizeof(resp_msg_buf), &resp_msg_len, &result_code)) {
        printf("%s ICCC integrity check success with unsupported flag \n", TA_TAG);
    } else {
        printf("%s ICCC integrity check failure with unsupported flag \n", TA_TAG);
        ret = ICCC_ERROR_DEVICE_STATUS_FAILED;
    }

    respmsg->content.iccc_rsp.ret = ret;
    return ret;
}

uint32_t process_cmd(uint32_t commandId, tciMessage_t *sendmsg, tciMessage_t *respmsg)
{
    uint32_t ret = ICCC_SUCCESS;

    /* Process command message */
    switch (commandId) {
        case CMD_ICCC_TEST_SAVEDATA:
            printf("%s CMD_ICCC_TEST_SAVEDATA \n", TA_TAG);
            ret = test_saveData(&sendmsg->payload.generic, &respmsg->payload.generic);
            break;
        case CMD_ICCC_TEST_READDATA:
            printf("%s CMD_ICCC_TEST_READDATA \n", TA_TAG);
            ret = test_readData(&sendmsg->payload.generic, &respmsg->payload.generic);
            break;
        case CMD_ICCC_TEST_GETDEVISESTAT:
            printf("%s CMD_ICCC_TEST_GETDEVISESTAT \n", TA_TAG);
            ret = test_getDeviceStatus(&sendmsg->payload.generic, &respmsg->payload.generic);
            break;
        case CMD_ICCC_TEST_NEGATIVE:
            printf("%s CMD_ICCC_TEST_NEGATIVE \n", TA_TAG);
            ret = test_negative(&sendmsg->payload.generic, &respmsg->payload.generic);
            break;
        default:
            printf("%s Unknown Command \n", TA_TAG);
            ret = CMD_ICCC_UNKNOWN;
            respmsg->payload.generic.content.iccc_rsp.ret = ret;
            break;
    }

    printf("%s process_cmd: ret: %d \n", TA_TAG, ret);
    return ret;
}

/* Trustlet entry */
TEE_Result TA_InvokeCommandEntryPoint(void *sessionContext, uint32_t commandID, uint32_t paramTypes, TEE_Param params[4])
{
    printf("%s TA_InvokeCommandEntryPoint \n", TA_TAG);

    uint32_t ret = TEE_SUCCESS;
    uint32_t commandId;
    tciMessage_t* sendmsg = NULL;
    tciMessage_t* respmsg = NULL;

    printf("%s tz buildinfo: build %s, teegris %s, chipset %s \n", TA_TAG, TARGET_BUILD_VARIANT, TEEGRIS_VERSION, TEEGRIS_BUILD_MODEL);
    printf("%s paramTypes: %d, TEE_PARAM_TYPES: %d \n", TA_TAG,
                       paramTypes, TEE_PARAM_TYPES(TEE_PARAM_TYPE_MEMREF_INOUT, TEE_PARAM_TYPE_MEMREF_OUTPUT, TEE_PARAM_TYPE_NONE, TEE_PARAM_TYPE_NONE));
    if (paramTypes != (uint32_t)TEE_PARAM_TYPES(TEE_PARAM_TYPE_MEMREF_INOUT, TEE_PARAM_TYPE_MEMREF_OUTPUT, TEE_PARAM_TYPE_NONE, TEE_PARAM_TYPE_NONE)) {
        printf("%s Bad Parameters in TA_InvokeCommandEntryPoint \n", TA_TAG);
        return TEE_ERROR_BAD_PARAMETERS;
    }

    /* Entry if session was open from NW */
    if (params[0].memref.buffer == NULL || params[1].memref.buffer == NULL ||
        params[0].memref.size < sizeof(tciMessage_t) || params[1].memref.size < sizeof(tciMessage_t)) {
        printf("%s Shared memory buffer size check error \n", TA_TAG);
        return TEE_ERROR_BAD_PARAMETERS;
    }

    if (TEES_IsREESharedMemory (TEE_MEMORY_ACCESS_READ | TEE_MEMORY_ACCESS_WRITE, params[0].memref.buffer, params[0].memref.size) != TEE_SUCCESS ||
        TEES_IsREESharedMemory (TEE_MEMORY_ACCESS_WRITE, params[1].memref.buffer, params[1].memref.size) != TEE_SUCCESS) {
        printf("%s TEES_IsREESharedMemory access check error \n", TA_TAG);
        return TEE_ERROR_ACCESS_DENIED;
    }

    /* Local buffer to prevent Race Condition */
    TEE_MemFill(&sendMsgCopy, 0, sizeof(tciMessage_t));
    TEE_MemFill(&rspMsgCopy, 0, sizeof(tciMessage_t));
    TEE_MemMove(&sendMsgCopy, params[0].memref.buffer, sizeof(tciMessage_t));

    sendmsg = &sendMsgCopy;
    respmsg = &rspMsgCopy;

    ret = process_cmd(commandID, sendmsg, respmsg);

    respmsg->header.id = RSP_ID(commandID);
    respmsg->header.status = ret;

    TEE_MemMove(params[1].memref.buffer, &rspMsgCopy, sizeof(tciMessage_t));
    TEE_MemFill(&sendMsgCopy, 0, sizeof(tciMessage_t));
    TEE_MemFill(&rspMsgCopy, 0, sizeof(tciMessage_t));

    return TEE_SUCCESS;
}