#include "kg_test.h"

#ifdef KG_ENG_MODE
void kg_change_rpmb(uint32_t flag) {

    KG_LOG_DBG("Calling kg_change_rpmb");
    uint32_t ret = KG_SUCCESS;
    kg_rpmb_data_t *krd = NULL;
    uint8_t hash[SHA256_DIGEST_LENGTH] = {0};

    krd = TEE_Malloc(sizeof(kg_rpmb_data_t), 0);
    if (NULL == krd) {
        KG_LOG("Failed to alloc buffer for kg rpmb data\n");
        ret = KG_ALLOC_BUFFER_FAIL;
        goto exit;
    }

    if (KG_SUCCESS != kg_rpmb_init()) {
        KG_LOG("RPMB is not available when initing KG secure data\n");
        ret = KG_RPMB_UNAVAILABLE;
        goto exit;
    }

    if (KG_SUCCESS != kg_rpmb_read((uint8_t *)krd, sizeof(kg_rpmb_data_t))) {
        KG_LOG("RPMB read failed when retriving data for kg init\n");
        ret = KG_RPMB_READ_FAIL;
        goto exit;
    }
    if(flag == CHANGE_RPMB_MAGIC)
        krd->magic = 0;

    if (KG_SUCCESS != kg_rpmb_write((uint8_t *)krd, sizeof(kg_rpmb_data_t))) {
        KG_LOG("RPMB write failed when initing KG TA secure data\n");
        ret = KG_RPMB_WRITE_FAIL;
        goto exit;
    }
exit:
    if (krd != NULL) {
        TEE_MemFill(krd, 0, sizeof(*krd));
        TEE_Free(krd);
        krd = NULL;
    }
    return ret;
}

void kg_init_for_test(void) {
    KG_LOG_DBG("Calling kg_init_for_test");
    uint32_t ret = KG_SUCCESS;
    kg_rpmb_data_t *krd = NULL;
    kg_secure_data_t *ksd = NULL;
    uint8_t *wrap_data = NULL;
    uint32_t wrap_data_len = KG_SECURE_DATA_LEN;
    uint8_t hash[SHA256_DIGEST_LENGTH] = {0};

    krd = TEE_Malloc(sizeof(kg_rpmb_data_t), 0);
    if (NULL == krd) {
        KG_LOG("Failed to alloc buffer for kg rpmb data\n");
        ret = KG_ALLOC_BUFFER_FAIL;
        goto exit;
    }

    ksd = TEE_Malloc(sizeof(kg_secure_data_t), 0);
    if (NULL == ksd) {
        KG_LOG("Failed to alloc buffer for kg secure data\n");
        ret = KG_ALLOC_BUFFER_FAIL;
        goto exit;
    }

    wrap_data = TEE_Malloc(wrap_data_len, 0);
    if (NULL == wrap_data) {
        KG_LOG("Failed to alloc buffer for kg secure wrap data\n");
        ret = KG_ALLOC_BUFFER_FAIL;
        goto exit;
    }

    if (KG_SUCCESS != kg_rpmb_init()) {
        KG_LOG("RPMB is not available when initing KG secure data\n");
        ret = KG_RPMB_UNAVAILABLE;
        goto exit;
    }

    if (KG_SUCCESS != kg_rpmb_read((uint8_t *)krd, sizeof(kg_rpmb_data_t))) {
        KG_LOG("RPMB read failed when retriving data for kg init\n");
        ret = KG_RPMB_READ_FAIL;
        goto exit;
    }

    krd->magic = KG_MAGIC;
    krd->version = KG_SECURE_DATA_VERSION;

    if (TZ_API_OK != TZ_wrap_data_with_derived_key((uint8_t *)KG_NAME, strlen(KG_NAME), (uint8_t *)ksd, sizeof(*ksd), wrap_data, &wrap_data_len)) {
        KG_LOG("Wraping secure data during kg init failed\n");
        ret = KG_TZ_API_FAIL;
        goto exit;
    }

    if (wrap_data_len > KG_SECURE_DATA_LEN) {
        KG_LOG("Wrap data size overflow\n");
        ret = KG_BUFFER_SIZE_FAIL;
    }

    krd->kg_wrap_data_len = wrap_data_len;
    TEE_MemMove(krd->kg_wrap_data, wrap_data, wrap_data_len);


    if (KG_SUCCESS != kg_rpmb_write((uint8_t *)krd, sizeof(kg_rpmb_data_t))) {
        KG_LOG("RPMB write failed when initing KG TA secure data\n");
        ret = KG_RPMB_WRITE_FAIL;
        goto exit;
    }

exit:
    if (krd != NULL) {
        TEE_MemFill(krd, 0, sizeof(*krd));
        TEE_Free(krd);
        krd = NULL;
    }
    if (ksd != NULL) {
        TEE_MemFill(ksd, 0, sizeof(*ksd));
        TEE_Free(ksd);
        ksd = NULL;
    }
    if (wrap_data != NULL) {
        TEE_Free(wrap_data);
        wrap_data = NULL;
    }
    return ret;
}

#endif