#include "kg_hdm_cmd.h"

#define HDM_STATUS_RESP_LEN 60

uint32_t format_return_hdmPolicy(char *hdm_status_resp, uint32_t hdm_ret, tz_hdm_kg_status_t *read_hdm_policy)
{
    KG_LOG("format_return_hdmPolicy");
    switch (hdm_ret)
    {
        case HDM_TATA_SUCCESS:
            KG_LOG("HDM_TATA_SUCCESS");
            if(HDM_STATUS_RESP_LEN > strlen("<HDM_TATA_SUCCESS>:<0x00000000>:<0x00000000>")+1) {
                snprintf(hdm_status_resp, 
                    strlen("<HDM_TATA_SUCCESS>:<0x00000000>:<0x00000000>") + 1,
                    "<HDM_TATA_SUCCESS>:<0x%08X>:<0x%08X>",
                    read_hdm_policy->device_block,
                    read_hdm_policy->compromise_block);
            }
            break;
        case HDM_KG_NOT_ACTIVATED:
            KG_LOG("HDM_KG_NOT_ACTIVATED");
            if(HDM_STATUS_RESP_LEN > strlen("<HDM_KG_NOT_ACTIVATED>:<0x00000000>:<0x00000000>")+1) {
                snprintf(hdm_status_resp,
                    strlen("<HDM_KG_NOT_ACTIVATED>:<0x00000000>:<0x00000000>") + 1,
                    "<HDM_KG_NOT_ACTIVATED>:<0x00000000>:<0x00000000>");
            }
            break;
        default:
            KG_LOG("HDM_TATA_ERROR");
            if(HDM_STATUS_RESP_LEN > strlen("<HDM_TATA_ERROR>:<0x00000000>:<0x00000000>")+1) {
                snprintf(hdm_status_resp,
                    strlen("<HDM_TATA_ERROR>:<0x00000000>:<0x00000000>") + 1,
                    "<HDM_TATA_ERROR>:<0x00000000>:<0x00000000>");
            }
            break;
    }
    
    if(hdm_status_resp[0] == '\0') {
        KG_LOG("return empty");
        return KG_HDM_STATUS_FAIL;
    }
    if(strlen(hdm_status_resp) > KG_MAX_PAYLOAD_LEN) {
        KG_LOG("resp buffer is not enough");
        KG_LOG_DBG("strlen(hdm_status_resp) > KG_MAX_PAYLOAD_LEN");
        return KG_HDM_STATUS_FAIL;
    }
    return KG_SUCCESS;
}

uint32_t KG_get_hdm_policy(tz_common_payload_t *sendmsg, tz_common_payload_t *respmsg)
{
    KG_LOG("KG_get_hdm_policy");
    uint32_t ret = KG_SUCCESS;
    tz_hdm_kg_status_t read_hdm_policy;
    char hdm_status_resp[HDM_STATUS_RESP_LEN] = {0,};

    ret = TA_Communication_kg_get_status(&read_hdm_policy);
    ret = format_return_hdmPolicy(hdm_status_resp, ret, &read_hdm_policy);
    if(ret != KG_SUCCESS){
        KG_LOG_DBG("format_return_hdmPolicy ret = %d", ret);
        return ret;
    }
    KG_LOG_DBG("KG_get_hdm_policy - hdm_status_resp = %s", hdm_status_resp);
    respmsg->payload.resp.data_len = strlen(hdm_status_resp);
    TEE_MemMove(respmsg->payload.resp.data_buf, hdm_status_resp, respmsg->payload.resp.data_len);

    return ret;   

}
uint32_t format_return_errorNonce(char *hdm_status_resp, uint32_t hdm_nonce)
{
    KG_LOG("hdm_error_nonce_fillRes");
    switch (hdm_nonce)
    {
        case HDM_TATA_SUCCESS:
            KG_LOG("case: HDM_TATA_SUCCESS");
            if(HDM_STATUS_RESP_LEN > strlen("<0>:<0>")+1) {
                snprintf(hdm_status_resp,
                    strlen("<0>:<0>") + 1,
                    "<0>:<0>");
            }
            break;
        case HDM_KG_NOT_ACTIVATED:
            KG_LOG("case: HDM_KG_NOT_ACTIVATED");
            if(HDM_STATUS_RESP_LEN > strlen("<4>:<0>")+1) {
                snprintf(hdm_status_resp,
                    strlen("<4>:<0>") + 1,
                    "<4>:<0>");
            }
            break;
        case KG_RPMB_READ_FAIL:
            KG_LOG("case: KG_RPMB_READ_FAIL");
            if(HDM_STATUS_RESP_LEN > strlen("<514>:<0>")+1) {
                snprintf(hdm_status_resp,
                    strlen("<514>:<0>") + 1,
                    "<514>:<0>");
            }
            break;
        case KG_RPMB_UNAVAILABLE:
            KG_LOG("case: KG_RPMB_UNAVAILABLE");
            if(HDM_STATUS_RESP_LEN > strlen("<513>:<0>")+1) {
                snprintf(hdm_status_resp,
                    strlen("<513>:<0>") + 1,
                    "<513>:<0>");
            }
            break;
        case KG_RPMB_CHECK_FAIL:
            KG_LOG("case: KG_RPMB_CHECK_FAIL");
            if(HDM_STATUS_RESP_LEN > strlen("<2053>:<0>")+1) {
                snprintf(hdm_status_resp,
                    strlen("<2053>:<0>") + 1,
                    "<2053>:<0>");
            }
            break;
        case KG_RPMB_MAGIC_FAIL:
            KG_LOG("case: KG_RPMB_MAGIC_FAIL");
            if(HDM_STATUS_RESP_LEN > strlen("<516>:<0>")+1) {
                snprintf(hdm_status_resp,
                    strlen("<516>:<0>") + 1,
                    "<516>:<0>");
            }
            break;
        case KG_ALLOC_BUFFER_FAIL:
            KG_LOG("case: KG_ALLOC_BUFFER_FAIL");
            if(HDM_STATUS_RESP_LEN > strlen("<258>:<0>")+1) {
                snprintf(hdm_status_resp,
                    strlen("<258>:<0>") + 1,
                    "<258>:<0>");
            }
            break;
        case HDM_TATA_ERROR:
            KG_LOG("case: HDM_TATA_ERROR");
            if(HDM_STATUS_RESP_LEN > strlen("<-1>:<0>")+1) {
                snprintf(hdm_status_resp,
                    strlen("<-1>:<0>") + 1,
                    "<-1>:<0>");
            }
            break;
        default: // It is a QC Nonce
            KG_LOG("default case");
            KG_LOG_DBG("QC NONCE");
            snprintf(hdm_status_resp, 
                HDM_STATUS_RESP_LEN,
                "<0>:<%u>",
                hdm_nonce);
            
            break;
    }
    if(hdm_status_resp[0] == '\0') {
        KG_LOG("nonce empty");
        KG_LOG_DBG("there is no nonce to return");
        return KG_HDM_STATUS_FAIL;
    }
    if(strlen(hdm_status_resp) > KG_MAX_PAYLOAD_LEN) {
        KG_LOG("resp buffer is not enough");
        KG_LOG_DBG("strlen(hdm_error_nonce_resp) > KG_MAX_PAYLOAD_LEN");
        return KG_HDM_STATUS_FAIL;
    }
    return KG_SUCCESS;
}

uint32_t rot_check(void){
    kg_rpmb_info_t *info = NULL;
    uint32_t ret = KG_SUCCESS;

    KG_LOG("rot_check");
    
    if (KG_SUCCESS != kg_rpmb_init()) {
        KG_LOG("KG TA accessing RPMB failed\n");
        ret = KG_RPMB_UNAVAILABLE;
        goto exit;
    }

    ret = read_info_object(&info);
    if(KG_SUCCESS != ret) {
        KG_LOG("Failed to read info object\n");
        goto exit;
    }
    
    if (info->magic != KG_MAGIC) {
        KG_LOG("KG TA rpmb data magic check failed\n");
        ret = KG_RPMB_MAGIC_FAIL;
        goto exit;
    }

exit:
    if (info != NULL) {
        TEE_Free(info);
        info = NULL;
    }
    return ret;
}

uint32_t KG_rot_process(tz_common_payload_t *sendmsg, tz_common_payload_t *respmsg)
{
    KG_LOG("KG_release_hdm_policy");
    uint32_t ret = KG_SUCCESS;
    tz_hdm_kg_status_t read_hdm_policy;
    char hdm_error_nonce_resp[HDM_STATUS_RESP_LEN] = {0,};

    if (sendmsg->payload.cmd.data_len > KG_MAX_PAYLOAD_LEN) {
        KG_LOG("Received invalid input buffer when rot process\n");
        return KG_BUFFER_SIZE_FAIL;
    }

    //rot checks
    //check rpmb read/write
    ret = rot_check();
    if(ret !=  KG_SUCCESS) {
        KG_LOG("rot_check fail");
    } else { 
        KG_LOG("rot_check success");
#ifdef KG_CHECK_HDM_SUPPORTED
        KG_LOG_DBG("KG_CHECK_HDM_SUPPORTED");
        ret = TA_Communication_kg_get_status(&read_hdm_policy);
        if(ret == KG_SUCCESS){
            ret = TA_Communication_kg_release();
        }
        else {
            KG_LOG_DBG("TA_Communication_kg_get_status ret = %d", ret);
        }
#endif
    }
    
    ret = format_return_errorNonce(hdm_error_nonce_resp, ret);

    if(ret != KG_SUCCESS){
        KG_LOG_DBG("format_return_errorNonce ret = %d", ret);
        return ret;
    }

    KG_LOG_DBG("KG_release_hdm_policy - ret = %d", ret);
    KG_LOG_DBG("KG_release_hdm_policy - hdm_error_nonce_resp = %s", hdm_error_nonce_resp);
    respmsg->payload.resp.data_len = strlen(hdm_error_nonce_resp);
    TEE_MemMove(respmsg->payload.resp.data_buf, hdm_error_nonce_resp, respmsg->payload.resp.data_len);

    return ret;   

}