/*
 * =====================================================================================
 *
 *       Filename:  hdm_utils.c
 *
 *    Description:  HDM utils functions
 *
 *        Version:  1.0
 *        Created:  09/16/2019 15:26:11 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *        Company:  Samsung Electronics
 *        Copyright (c) 2015 by Samsung Electronics, All rights reserved.
 *
 * =====================================================================================
 */

/** Includes */
#include "hdm_utils.h"

/**
 * HDM Global JWS
 */
tz_hdm_header_t    header;
tz_hdm_payload_t   payload;
tz_hdm_signature_t signature;

/**
 * HDM Global variable
 */
uint32_t current_service_index;

/**
 * Possible values:
 * - HDM_DEVICE_OK
 * - HDM_DEVICE_COMPROMISED
 * - HDM_APPLY_DEFAULT_POLICY
 */
uint32_t device_status;

/**
 * Logging
 */
char hdm_log_msg[LOG_MSG_SIZE];


/**
 * @brief
 * hex2int
 * Converts hexadecimal to integer
 *
 * @param[in] *hex - hexadecimal number
 * @return Integer
 */
int64_t hex2int(uint8_t *hex) {
        int64_t val = 0;

        for(int i = 0; i < 8; i++) {
                uint8_t byte = hex[i];

                if (byte >= '0' && byte <= '9') byte = byte - '0';
                else if (byte >= 'a' && byte <='f') byte = byte - 'a' + 10;
                else if (byte >= 'A' && byte <='F') byte = byte - 'A' + 10;
                else if (byte == '"') return val;
                else {
                    HDM_LOG("Invalid character received at the hex string");
                    return -1;
                }

                val = (val << 4) | (byte & 0xF);
        }

        return val;
}

/**
 * @brief
 * check_policy_version
 * Check if policy version if greater than RPMB policy version
 *
 * @param[in] payload     - policy payload
 * @param[in] *rpmb_data  - rpmb data
 * @return HDM status code
 */
hdm_return_code_t check_policy_version(tz_hdm_payload_t payload, tz_hdm_rpmb_t *rpmb_data) {
        HDM_LOG_DEBUG("check_policy_version()");
        hdm_return_code_t ret = HDM_STATUS_SUCCESS;

        HDM_LOG_DEBUG("payload.policy_version = %d",   payload.policy_version);
        HDM_LOG_DEBUG("rpmb_data->policy_version = %d",   rpmb_data->data[current_service_index].policy_version);
        HDM_LOG_DEBUG("rpmb_data->magic = %02x", rpmb_data->magic);

        if (rpmb_data->magic != HDM_MAGIC) {
                // RPMB is not initialized, no check is needed
                ret = HDM_STATUS_SUCCESS;
        } else {
                ret = (payload.policy_version > rpmb_data->data[current_service_index].policy_version) ? HDM_STATUS_SUCCESS : HDM_WRONG_POLICY_VERSION;
        }

        return ret;
}

/**
 * @brief
 * check_hdm_magic
 * Check if HDM is activated, or not
 *
 * @param[in] *rpmb_data - RPMB data
 * @return HDM status code
 */
hdm_return_code_t check_hdm_magic(tz_hdm_rpmb_t *rpmb_data) {
        HDM_LOG_DEBUG("check_hdm_magic()");
        return ((rpmb_data->magic == HDM_MAGIC) ? HDM_STATUS_SUCCESS : HDM_RPMB_MAGIC_FAIL);
}


/**
 * @brief
 * check_device_id
 * Check if device id in payload matches device id in RPMB
 *
 * @param[in] payload     - policy payload
 * @param[in] *rpmb_data  - rpmb data
 * @return HDM status code
 */
hdm_return_code_t check_device_id(tz_hdm_payload_t payload, tz_hdm_rpmb_t *rpmb_data) {
        HDM_LOG_DEBUG("check_device_id()");
        hdm_return_code_t ret = HDM_STATUS_SUCCESS;
        HDM_LOG_DEBUG("rpmb_data->magic = %02x", rpmb_data->magic);

        if (rpmb_data->magic != HDM_MAGIC) {
                // RPMB is not initialized, no check is needed
                ret = HDM_STATUS_SUCCESS;
        } else {
                if (TEE_MemCompare(payload.device_id, rpmb_data->device_id, JWS_PAYLOAD_DEVICE_ID_LEN) != 0) {
                        ret = HDM_DEVICE_ID_CHECK_FAIL;
                }
        }

        return ret;
}

/**
 * @brief
 * get_curr_service_index
 * Get current service index in RPMB
 * This API should called after hdm_rpmb_read is called
 *
 * @param[in] rpmb_data - RPMB data
 * @param[in] *name - service name
 * @param[out] index - index of service name
 * @return HDM status code
 */
hdm_return_code_t get_curr_service_index(tz_hdm_rpmb_t rpmb_data, uint8_t *name) {
        HDM_LOG_DEBUG("get_curr_service_index()");

        hdm_return_code_t ret = HDM_RPMB_IS_FULL;
        int i = 0;
        int empty_index = -1;
        current_service_index = MAX_SERVICE_NUM;
        HDM_LOG_DEBUG(" input name : %s", (char *)name);

        for (i=0; i<MAX_SERVICE_NUM; i++) {
                if (rpmb_data.data[i].isActivated == 1) {
                    if (TEE_MemCompare(name, rpmb_data.data[i].service_name, JWS_HEADER_SERVICE_NAME_LEN) == 0) {
                        HDM_LOG_DEBUG("service index : %d", i);
                        current_service_index = i;
                        ret = HDM_STATUS_SUCCESS;
                        goto exit;
                    }
                }
                else if (empty_index < 0) {
                    empty_index = i;
                }
                HDM_LOG_DEBUG("%d[%d]: name : %s, policy : 0x%x", i, rpmb_data.data[i].isActivated, (char *)rpmb_data.data[i].service_name, rpmb_data.data[i].device_block);
        }

        if (empty_index >= 0) {
            HDM_LOG_DEBUG("Index %d is empty", empty_index);
            ret = HDM_RPMB_SERVICE_NAME_MISMATCH;
            current_service_index = empty_index;
        }
exit:
        return ret;
}

/**
 * @brief
 * get_rpmb_version
 * return rpmb data version for data migration
 *
 * @param[in] *rpmb_data - RPMB data
 * @return HDM rpmb version
 */
uint32_t get_rpmb_version(tz_hdm_rpmb_t *rpmb_data) {

    if (rpmb_data->version == HDM_MAGIC) {
        HDM_LOG_DEBUG("Current RPMB version is Single Policy ");
        return HDM_SINGLE_POLICY_VERSION;
    }
    HDM_LOG_DEBUG("Current RPMB version : 0x%x", rpmb_data->version);

    return rpmb_data->version;
}


#ifdef CONFIG_QSEE
/**
 * @brief
 * hdm_get_status
 * return rpmb data
 *
 * @param[in] rpmb_data - RPMB data
 * @param[out] *buffer - return buffer
 * @param[in] buff_size - return buffer size
 * @return return length
 */
uint32_t hdm_get_status(tz_hdm_rpmb_t rpmb_data, uint8_t *buffer, uint32_t buff_size) {
    uint32_t index = 0, offset = 0;
    uint32_t policy = 0;
    uint32_t len = 0;

    for (index = 0; index < MAX_SERVICE_NUM && offset < buff_size; index++) {
        if (rpmb_data.data[index].isActivated == 1) {
            if (offset != 0) {
                memcpy(buffer+offset, "|", 1);
                offset += 1;
            }

            len = strlen((char *)(rpmb_data.data[index].service_name));
            if (len > 3)
                len = 3;

            TEE_MemMove((char*)buffer+offset, rpmb_data.data[index].service_name, len);
            offset += len;
            memcpy(buffer+offset, "&", 1);
            offset += 1;
            if (device_status == HDM_DEVICE_OK)
                snprintf((char*)buffer+offset, 3, "%02x", rpmb_data.data[index].device_block);
            else
                snprintf((char*)buffer+offset, 3, "%02x", rpmb_data.data[index].compromise_block);
            offset += 2;
            HDM_LOG_DEBUG("hdm_status : %s(%d)", buffer, offset);
        }
    }

    if (offset == 0) {
        snprintf((char*)buffer, 5, "NONE");
        offset = 4;
    }

    return offset;
}
#endif

/**
 * @brief
 * hdm_is_need_reboot
 * When some subsystem is unlocked, reboot is necessary
 * changed policy = bl policy ^ applied policy
 * unlocked policy = changed policy & bl policy
 * reboot policy = unlocked policy & HDM_SUBSYS_NEED_REBOOT
 *
 * @param[in]  applied_policy - applied policy
 *
 * @return if reboot is necessary or not
 */
uint32_t hdm_is_need_reboot(uint32_t applied_policy)
{
    uint32_t ret = 0;
    uint32_t bl_policy;

    HDM_LOG_DEBUG("hdm_is_need_reboot");

    if (hdm_ICCC_read(&bl_policy)) {
        HDM_LOG("Failed to read bl_policy in ICCC!");
        return 0;
    }
    HDM_LOG_DEBUG("bl_policy : 0x%x, applied policy : 0x%x", bl_policy, applied_policy);

    if (bl_policy == 0xffffffff)
        return ret;

    if((bl_policy ^ applied_policy) & bl_policy & HDM_SUBSYS_NEED_REBOOT)
        ret = 1;

    HDM_LOG_DEBUG("return %d", (int) ret);
    return ret;
}

/**
 * @brief
 * hdm_check_jws_string_field
 * Check if is empty
 * Check length
 *
 * @param [in] string_field - String field
 * @return HDM status code
 */
hdm_return_code_t hdm_check_jws_string_field(uint8_t *string_field, uint8_t *type) {
        hdm_return_code_t ret = HDM_STATUS_FAIL;
        uint32_t string_field_len = strlen((char *) string_field);

        if (string_field_len < 1) {
                HDM_LOG_DEBUG("The string field is empty");
                goto exit;
        }

        if (TEE_MemCompare(type, JWS_HEADER_SERVICE_NAME, strlen((char *) JWS_HEADER_SERVICE_NAME)) == 0) {
                if(string_field_len >= MIN_STRING_FIELD_LEN && string_field_len < JWS_HEADER_SERVICE_NAME_LEN) {
                        ret = HDM_STATUS_SUCCESS;
                }
        }
        else if (TEE_MemCompare(type, JWS_HEADER_REQ_ID, strlen((char *) JWS_HEADER_REQ_ID)) == 0) {
                if(string_field_len >= MIN_STRING_FIELD_LEN && string_field_len < JWS_HEADER_REQ_ID_LEN) {
                        ret = HDM_STATUS_SUCCESS;
                }
        }
        else {
                HDM_LOG_DEBUG("Field type not found: %s", type);
        }

exit:
        return ret;
}

/**
 * @brief
 * get_error_string
 * Get error string from error code
 *
 * @param[in] error_code - Error code
 *
 * @return The error string
 */
uint8_t *get_error_string(uint32_t error_code) {
        switch (error_code) {
                case HDM_STATUS_SUCCESS:
                        return (uint8_t *) "SUCCESS";
                case HDM_STATUS_FAIL:
                        return (uint8_t *) "FAIL";
                case HDM_INVALID_JWS:
                        return (uint8_t *) "INVALID JWS";
                case HDM_JWS_INVALID_FORMAT:
                        return (uint8_t *) "INVALID JWS FORMAT";
                case HDM_JWS_NULL:
                        return (uint8_t *) "NULL JWS";
                case HDM_JWS_INVALID_LENGTH:
                        return (uint8_t *) "INVALID JWS LENGTH";
                case HDM_INVALID_SIGNATURE:
                        return (uint8_t *) "INVALID SIGNATURE";
                case HDM_INVALID_CERTCHAIN:
                        return (uint8_t *) "INVALID CERTIFICATE CHAIN";
                case HDM_INVALID_CA:
                        return (uint8_t *) "INVALID CERTIFICATE AUTHORITY";
                case HDM_VERIFY_POLICY_FAIL:
                        return (uint8_t *) "POLICY VERIFICATION FAILED";
                case HDM_APPLY_POLICY_FAIL:
                        return (uint8_t *) "APPLY POLICY FAILED";
                case HDM_LOAD_POLICY_FAIL:
                        return (uint8_t *) "LOAD POLICY FAILED";
                case HDM_COMPUTE_HASH_FAIL:
                        return (uint8_t *) "COMPUTE HASH FAILED";
                case HDM_RPMB_FAIL:
                        return (uint8_t *) "FAIL IN RPMB";
                case HDM_GEN_RPMB_HASH_FAIL:
                        return (uint8_t *) "RPMB HASH GENERATION FAILED";
                case HDM_RPMB_INTEGRITY_FAIL:
                        return (uint8_t *) "RPMB INTEGRIY CHECK FAILED";
                case HDM_GEN_DEVICE_ID_FAIL:
                        return (uint8_t *) "DEVICE ID GENERATION FAILED";
                case HDM_KEY_ERROR:
                        return (uint8_t *) "KEY ERROR";
                case HDM_SIGNATURE_ERROR:
                        return (uint8_t *) "SIGNATURE ERROR";
                case HDM_CERT_DRK_FAIL:
                        return (uint8_t *) "DRK CERTIFICATE FAILED";
                case HDM_ALLOC_ERROR:
                        return (uint8_t *) "MEMORY ALLOCATION FAILED";
                case HDM_GEN_RESPONSE_FAIL:
                        return (uint8_t *) "GENERATE RESPONSE FAILED";
                case HDM_BUF_SIZE_ERROR:
                        return (uint8_t *) "BUFFER SIZE ERROR";
                case HDM_CONVERT_DER_CERT_FAIL:
                        return (uint8_t *) "CONVERT DER FAILED";
                case HDM_DEVICE_ID_CHECK_FAIL:
                        return (uint8_t *) "DEVICE ID CHECK FAILED";
                case HDM_DEVICE_OK:
                        return (uint8_t *) "DEVICE INTEGRITY OK";
                case HDM_DEVICE_COMPROMISED:
                        return (uint8_t *) "DEVICE IS COMPROMISED";
                case HDM_WRONG_POLICY_VERSION:
                        return (uint8_t *) "WRONG POLICY VERSION";
                case HDM_UNWRAP_APP_NAME_FAILED:
                        return (uint8_t *) "APP NAME FAILED IN UNWRAP";
                case HDM_UNWRAP_FAILED_WRAP:
                        return (uint8_t *) "WRAP FAILED AFTER UNWRAPPING";
                case HDM_UNWRAP_FAILED_NO_KEY:
                        return (uint8_t *) "UNWRAP FAILED NO KEY";
                case HDM_UNWRAP_FAILED:
                        return (uint8_t *) "UNWRAP GENERIC FAIL";
                case HDM_UNWRAP_VERSION_MISMATCH:
                        return (uint8_t *) "UNWRAP VERSION MISMATCH";
                case HDM_UNWRAP_UNKNOWN:
                        return (uint8_t *) "UNWRAP UNKNOWN ERROR";
                case HDM_UNWRAP_LOAD_KEY_ERROR:
                        return (uint8_t *) "UNWRAP LOAD KEY ERROR";
                case HDM_UNWRAP_KEY_LENGTH_FAILED:
                        return (uint8_t *) "UNWRAP KEY LENGTH FAILED";
                case HDM_RPMB_MAGIC_FAIL:
                        return (uint8_t *) "RPMB MAGIC FAILED";
                case HDM_APPLY_DEFAULT_POLICY:
                        return (uint8_t *) "APPLY DEFAULT POLICY";
                case HDM_JSON_FAIL:
                        return (uint8_t *) "JSON MANIPULATION FAIL";
                case HDM_RPMB_IS_FULL:
                        return (uint8_t *) "RPMB IS FULL";
                case HDM_RPMB_SERVICE_NAME_MISMATCH:
                        return (uint8_t *) "SERVICE NAME IS MISMATCHED";
                case HDM_INVALID_SERVICE_NAME:
                        return (uint8_t *) "INVALID SERVICE NAME";
                case HDM_READ_EM_STATUS_FAIL:
                        return (uint8_t *) "EM STATUS FAIL";
                case HDM_INVALID_CN_CHECK:
                        return (uint8_t *) "INVALID CN CHECK";
                case HDM_INVALID_REQUEST_LEN:
                        return (uint8_t *) "INVALID REQUEST LENGTH";
                case HDM_INVALID_REQ_ID:
                        return (uint8_t *) "INVALID REQUEST ID";
                case HDM_SERVICE_NAME_NOT_FOUND:
                        return (uint8_t *) "SERVICE NAME NOT FOUND";

        }
        return (uint8_t *) "UNKNOWN";
}


/**
 * @brief
 * get_numeric_rep
 * Get decimal value for given HEX digit
 *
 * @param[in] byte - HEX digit in byte format
 *
 * @return The decimal representation, -1 if the
 *         digit is invalid
 */
static int get_numeric_rep(int byte) {
        if (byte >= '0' && byte <= '9')
                byte = byte - '0';
        else if (byte >= 'a' && byte <='f')
                byte = byte - 'a' + 10;
        else if (byte >= 'A' && byte <='F')
                byte = byte - 'A' + 10;
        else
                byte = -1;

        return byte;
}

/**
 * @brief
 * hex_to_byte_stream
 * Transforms given HEX input string into a
 * byte formatted stream.
 * If the input string has odd length, this
 * method fills the last byte with a zeroed
 * nibble on the least significant bits.
 *
 * @param[in]  hex_string - representation of HEX number
 * @param[in]  str_len    - length of the HEX string
 * @param[out] buff       - output pointer to the byte stream
 *
 * @return nothing
 */
void hex_to_byte_stream(uint8_t hex_string[], uint32_t str_len,
                        uint8_t **buff) {
        uint8_t *out = NULL;
        uint32_t size = 0;
        uint32_t pos = 0;

        if (str_len == 0){
                goto exit;
        }

        if (str_len % 2 == 0) {
                size = str_len/2;
        } else {
                size = str_len/2 + 1;
        }

        out = (uint8_t*)TEE_Malloc(size*sizeof(uint8_t), 0);

        if (out == NULL) { // SI-16876
            HDM_LOG("TEE_Malloc failed");
            goto exit;
        }

        for (int i=0; i<str_len && pos < size; i+=2, pos++) {
                int8_t nibble_1;
                int8_t nibble_0;

                nibble_1 = get_numeric_rep(hex_string[i]);
                if (nibble_1 < 0) {
                        goto error;
                }

                if (i+2 > str_len) {
                        nibble_0 = 0;
                } else {
                        nibble_0 = get_numeric_rep(hex_string[i+1]);
                        if (nibble_0 < 0) {
                                goto error;
                        }
                }

                out[pos] = nibble_1;
                out[pos] = out[pos] << 4;
                out[pos] = out[pos] | nibble_0;
        }
exit:
        *buff = out;
        return;

error:
        TEE_Free(out);
        out = NULL;
        *buff = out;
}

/**
 * @brief
 * change string "\x5cn" to "\n".
 *
 * @param[in/out]  certificate - pointer to certificate
 *
 * @return nothing
 */
void settle_certificate_to_openssl(uint8_t *certificate)
{
        char *psResult = (char *)certificate;

        while((psResult = strstr(psResult, "\x5cn") )!= NULL)
        {
                strncpy(psResult,"\r\n",2);
                psResult= &psResult[2];

        }
        return ;
}

/**
 * Debug print functions
 */
#ifdef DEBUG_HDM
void dump_jws_header(tz_hdm_header_t *header) {
        HDM_LOG_DEBUG("#####################################################");
        HDM_LOG_DEBUG("#              header jws                           #");
        HDM_LOG_DEBUG("#####################################################");

        HDM_LOG_DEBUG("service name:  %s", header->service_name);
        HDM_LOG_DEBUG("alg:           %s", header->alg);
        HDM_LOG_DEBUG("version:       %s", header->protocol_version);
        HDM_LOG_DEBUG("x5c.count:     %d", header->x5c_count);

//        for(int i = 0; i < header->x5c_count; i++) {
//                printf("certificate %d:\n",i);
//
//                for(int j=0; j < header->x5c[i].certificate_len; j++) {
//                        printf("%c", header->x5c[i].certificate[j]);
//                }
//                printf("\n");
//        }

        HDM_LOG_DEBUG("#####################################################");
}

void dump_jws_payload(tz_hdm_payload_t *payload) {
        HDM_LOG_DEBUG("#####################################################");
        HDM_LOG_DEBUG("#              payload jws                          #");
        HDM_LOG_DEBUG("#####################################################");

        HDM_LOG_DEBUG("device_id:         %s", payload->device_id);
        HDM_LOG_DEBUG("device_block:      0x%08x", payload->device_block);
        HDM_LOG_DEBUG("compromise_block:  0x%08x", payload->compromise_block);
        HDM_LOG_DEBUG("generic_cmd_cnt:   %d", payload->policy_version);
        HDM_LOG_DEBUG("server_nonce:      %s", payload->server_nonce);

        HDM_LOG_DEBUG("#####################################################");
}

void dump_jws_sinature(tz_hdm_signature_t *signature) {
        HDM_LOG_DEBUG("#####################################################");
        HDM_LOG_DEBUG("#              signature jws                        #");
        HDM_LOG_DEBUG("#####################################################");

        HDM_LOG_DEBUG("signature: %s", signature->signature);

        HDM_LOG_DEBUG("#####################################################");
}

void dump_rpmb_data(tz_hdm_rpmb_t *rpmb_data) {
        HDM_LOG_DEBUG("#####################################################");
        HDM_LOG_DEBUG("#                    RPMB DATA                      #");
        HDM_LOG_DEBUG("#####################################################");

        HDM_LOG_DEBUG("version:           0x%x", rpmb_data->version);
        HDM_LOG_DEBUG("magic:             0x%x", rpmb_data->magic);
        HDM_LOG_DEBUG("device_id:         %s", rpmb_data->device_id);
        HDM_LOG_DEBUG("hash_value:        %s", rpmb_data->hash_value);

        for(int i = 0; i < MAX_SERVICE_NUM; i++) {
                HDM_LOG_DEBUG("service num:       %d", i);
		HDM_LOG_DEBUG("  isActivated:       %d", rpmb_data->data[i].isActivated);
		HDM_LOG_DEBUG("  service_name:      %s", rpmb_data->data[i].service_name);
		HDM_LOG_DEBUG("  device_block:      0x%08x", rpmb_data->data[i].device_block);
		HDM_LOG_DEBUG("  compromise_block:  0x%08x", rpmb_data->data[i].compromise_block);
		HDM_LOG_DEBUG("  policy_version:    %d", rpmb_data->data[i].policy_version);
        }

        HDM_LOG_DEBUG("#####################################################");
}

void printRange(uint8_t *buff, int len, char *tag) {
        uint8_t str[len+1];
        TEE_MemFill(str, 0, len);
        TEE_MemMove(str, buff, len);
        str[len] = '\0';
        HDM_LOG("[%s] %s", tag, str);
}

#endif