#include "kg_hotp.h"

uint32_t hotp_passcode(uint8_t *key, uint32_t key_len, uint8_t *current_pass,
    uint32_t current_pass_len, uint8_t *update_pass, uint32_t *update_pass_len)
{
    if (NULL == key || key_len != 32) {
        KG_LOG("empty key buffer or invalid key length to generate hotp passcode\n");
        return KG_BUFFER_SIZE_FAIL;
    }

    if (NULL == current_pass || current_pass_len == 0) {
        KG_LOG("empty seed buffer or invalid key length to generate hotp passcode\n");
        return KG_BUFFER_SIZE_FAIL;
    }

    if (hmac_sha256(key, key_len, current_pass, current_pass_len, update_pass, update_pass_len) != KG_SUCCESS) {
        KG_LOG_DBG("hmac error when generating hotp passcode\n");
        return KG_CRYPTO_HMAC_FAIL;
    }

    KG_DUMP_DBG("KG HOTP DEBUG updatePass", update_pass, *update_pass_len);

    uint32_t offset = 0;
    int32_t binary = ((update_pass[offset] & 0x7f) << 24)
                   | ((update_pass[offset + 1] & 0xff) << 16)
                   | ((update_pass[offset + 2] & 0xff) << 8)
                   | (update_pass[offset + 3] & 0xff);

    KG_LOG_DBG("converted integer is %d\n", binary);

    uint64_t m = 10;
    uint32_t n;
    for (n = 1; n < KG_OTP_LEN; n++) {
        m *= 10;
    }

    binary = binary % m;

    KG_LOG_DBG("integer after modules is %d\n", binary);

    char buf[KG_OTP_LEN + 1];
    TEE_MemFill(buf, 0, KG_OTP_LEN + 1); 
    
    uint32_t i = 0;

    while (binary != 0) {
        int rem = binary % 10;
        buf[i++] = rem + '0';
        binary = binary / 10;
    }

    if (reverse(buf, i) != KG_SUCCESS)
        return KG_UTIL_FAIL;

    KG_LOG_DBG("hotp passcode is %s\n", (char *)buf);

    // In the case of leading 0, we need to keep the 0 instead of discard it
    TEE_MemFill(update_pass, 0x30, *update_pass_len);
    
    *update_pass_len = 8;
    TEE_MemMove(update_pass + 8 - strlen((char *)buf), (uint8_t *)buf, *update_pass_len);

    return KG_SUCCESS;
}

uint32_t hotp_challenge(
    uint8_t *key,
    uint32_t key_len,
    uint8_t *current_pass,
    uint32_t current_pass_len,
    uint8_t *challenge,
    uint32_t *challenge_len)
{
    if (NULL == key || key_len != 32) {
        KG_LOG("empty key buffer or invalid key length to generate hotp challenge\n");
        return KG_BUFFER_SIZE_FAIL;
    }

    if (NULL == current_pass || current_pass_len == 0) {
        KG_LOG("empty passcode buffer or invalid passcode length to generate hotp challenge\n");
        return KG_BUFFER_SIZE_FAIL;
    }

    if (hmac_sha256(key, key_len, current_pass, current_pass_len, challenge, challenge_len) != KG_SUCCESS) {
        KG_LOG("hmac error when generating hotp challenge\n");
        return KG_CRYPTO_HMAC_FAIL;
    }

    KG_DUMP_DBG("KG HOTP DEBUG updatePass", challenge, *challenge_len);

    uint32_t offset = SHA256_DIGEST_LENGTH - 4;
    int32_t binary = ((challenge[offset] & 0x7f) << 24)
                   | ((challenge[offset + 1] & 0xff) << 16)
                   | ((challenge[offset + 2] & 0xff) << 8)
                   | (challenge[offset + 3] & 0xff);

    KG_LOG_DBG("converted integer is %d\n", binary);

    uint64_t m = 10;
    uint32_t n;
    for (n = 1; n < KG_OTP_LEN; n++) {
        m *= 10;
    }

    binary = binary % m;

    KG_LOG_DBG("integer after modules is %d\n", binary);

    char buf[KG_OTP_LEN + 1];
    TEE_MemFill(buf, 0, KG_OTP_LEN + 1); 

    uint32_t i = 0;

    while (binary != 0) {
        int rem = binary % 10;
        buf[i++] = rem + '0';
        binary = binary / 10;
    }

    if (reverse(buf, i) != KG_SUCCESS)
        return KG_UTIL_FAIL;

    KG_LOG_DBG("hotp challenger is %s\n", (char *)buf);

    // In the case of leading 0, we need to keep the 0 in string instead of discard it
    TEE_MemFill(challenge, 0x30, *challenge_len);

    *challenge_len = 8;
    TEE_MemMove(challenge + 8 - strlen((char *)buf), (uint8_t *)buf, *challenge_len);

    return KG_SUCCESS;
}