/**
 * @file se050_scp03.c
 * @brief SE050 Platform SCP03 Secure Channel
 * 
 * Clean-room implementation of SCP03 secure channel protocol.
 * 
 * License: MIT (Clean-room implementation)
 */

#define _GNU_SOURCE  /* For MADV_DONTDUMP, MADV_WIPEONFORK */
#define _POSIX_C_SOURCE 200809L

#include "se050_i2c_hal.h"
#include "se050_session_internal.h"
#include "se050_scp03.h"
#include "se050_mem_pool.h"
#include "se050_wireguard.h"
#include "se050_crypto_utils.h"
#include "se050_mem_protect.h"
#include <stdio.h>
#include <string.h>

/* SCP03 constants */
#define SCP03_KEY_SIZE          16
#define SCP03_IV_SIZE           16
#define SCP03_CMAC_SIZE         8
#define SCP03_MAX_CMD_LEN       255
#define SCP03_MAX_RSP_LEN       255

/* SCP03 APDU status codes */
#define SCP03_SW_SUCCESS        0x9000
#define SCP03_SW_FAIL           0x6F00

/* ============================================================================
 * Helper Functions
 * ============================================================================ */

/**
 * @brief Calculate CMAC for SCP03
 * 
 * This is a placeholder implementation. In production, use a proper
 * AES-CMAC implementation (e.g., from OpenSSL or libgcrypt).
 */
static int scp03_calc_cmac(const uint8_t *key, size_t key_len,
                           const uint8_t *data, size_t data_len,
                           uint8_t *mac, size_t *mac_len)
{
    (void)key; (void)key_len; (void)data; (void)data_len; (void)mac; (void)mac_len;
    /* Placeholder: In production, use OpenSSL AES-CMAC */
    return 0;
#if defined(__linux__) && defined(OPENSSL_AVAILABLE)
    /* Use OpenSSL for AES-CMAC */
    #include <openssl/cmac.h>
    CMAC_CTX *ctx;
    size_t len;
    
    ctx = CMAC_CTX_new();
    if (!ctx) {
        return -1;
    }
    
    if (CMAC_Init(ctx, key, key_len, EVP_aes_128(), NULL) != 1) {
        CMAC_CTX_free(ctx);
        return -1;
    }
    
    if (CMAC_Update(ctx, data, data_len) != 1) {
        CMAC_CTX_free(ctx);
        return -1;
    }
    
    if (CMAC_Final(ctx, mac, &len) != 1) {
        CMAC_CTX_free(ctx);
        return -1;
    }
    
    *mac_len = len;
    CMAC_CTX_free(ctx);
    
    return 0;
#else
    /* Fallback: Simple XOR-based MAC (NOT SECURE - for testing only) */
    size_t i, j;
    
    memset(mac, 0, *mac_len);
    
    for (i = 0; i < data_len; i++) {
        for (j = 0; j < *mac_len && j < SCP03_KEY_SIZE; j++) {
            mac[j % *mac_len] ^= data[i] ^ key[j];
        }
    }
    
    return 0;
#endif
}

/**
 * @brief AES-CBC encryption for SCP03
 */
static int scp03_aes_cbc_encrypt(const uint8_t *key, size_t key_len,
                                  const uint8_t *iv, size_t iv_len,
                                  const uint8_t *plaintext, size_t plaintext_len,
                                  uint8_t *ciphertext, size_t *ciphertext_len)
{
#if defined(__linux__) && defined(OPENSSL_AVAILABLE)
    /* Use OpenSSL for AES-CBC */
    #include <openssl/aes.h>
    #include <openssl/evp.h>
    
    EVP_CIPHER_CTX *ctx;
    int len;
    int ciphertext_len_tmp;
    
    ctx = EVP_CIPHER_CTX_new();
    if (!ctx) {
        return -1;
    }
    
    if (EVP_EncryptInit_ex(ctx, EVP_aes_128_cbc(), NULL, key, iv) != 1) {
        EVP_CIPHER_CTX_free(ctx);
        return -1;
    }
    
    if (EVP_EncryptUpdate(ctx, ciphertext, &len, plaintext, (int)plaintext_len) != 1) {
        EVP_CIPHER_CTX_free(ctx);
        return -1;
    }
    
    ciphertext_len_tmp = len;
    
    if (EVP_EncryptFinal_ex(ctx, ciphertext + len, &len) != 1) {
        EVP_CIPHER_CTX_free(ctx);
        return -1;
    }
    
    ciphertext_len_tmp += len;
    *ciphertext_len = (size_t)ciphertext_len_tmp;
    
    EVP_CIPHER_CTX_free(ctx);
    
    return 0;
#else
    /* Fallback: Simple XOR-based encryption (NOT SECURE - for testing only) */
    size_t i;
    size_t block_size = 16;
    
    /* Pad to block size */
    size_t padded_len = ((plaintext_len + block_size - 1) / block_size) * block_size;
    memset(ciphertext, 0, padded_len);
    memcpy(ciphertext, plaintext, plaintext_len);
    
    /* XOR with key and IV */
    for (i = 0; i < padded_len; i++) {
        ciphertext[i] ^= key[i % key_len] ^ iv[i % iv_len];
    }
    
    *ciphertext_len = padded_len;
    return 0;
#endif
}

/**
 * @brief AES-CBC decryption for SCP03
 */
static int scp03_aes_cbc_decrypt(const uint8_t *key, size_t key_len,
                                  const uint8_t *iv, size_t iv_len,
                                  const uint8_t *ciphertext, size_t ciphertext_len,
                                  uint8_t *plaintext, size_t *plaintext_len)
{
#if defined(__linux__) && defined(OPENSSL_AVAILABLE)
    /* Use OpenSSL for AES-CBC */
    #include <openssl/aes.h>
    #include <openssl/evp.h>
    
    EVP_CIPHER_CTX *ctx;
    int len;
    int plaintext_len_tmp;
    
    ctx = EVP_CIPHER_CTX_new();
    if (!ctx) {
        return -1;
    }
    
    if (EVP_DecryptInit_ex(ctx, EVP_aes_128_cbc(), NULL, key, iv) != 1) {
        EVP_CIPHER_CTX_free(ctx);
        return -1;
    }
    
    if (EVP_DecryptUpdate(ctx, plaintext, &len, ciphertext, (int)ciphertext_len) != 1) {
        EVP_CIPHER_CTX_free(ctx);
        return -1;
    }
    
    plaintext_len_tmp = len;
    
    if (EVP_DecryptFinal_ex(ctx, plaintext + len, &len) != 1) {
        EVP_CIPHER_CTX_free(ctx);
        return -1;
    }
    
    plaintext_len_tmp += len;
    *plaintext_len = (size_t)plaintext_len_tmp;
    
    EVP_CIPHER_CTX_free(ctx);
    
    return 0;
#else
    /* Fallback: Simple XOR-based decryption (NOT SECURE - for testing only) */
    size_t i;
    
    /* XOR with key and IV */
    for (i = 0; i < ciphertext_len; i++) {
        plaintext[i] = ciphertext[i] ^ key[i % key_len] ^ iv[i % iv_len];
    }
    
    *plaintext_len = ciphertext_len;
    return 0;
#endif
}

/* ============================================================================
 * SCP03 Management
 * ============================================================================ */

/**
 * @brief Derive session keys from DEK using SCP03 key derivation
 * 
 * SCP03 uses the DEK key to derive session-specific encryption and MAC keys.
 * This is done through a key derivation function using AES.
 */
static se050_status_t scp03_derive_session_keys(se050_scp03_ctx_t *ctx)
{
    uint8_t derive_data[16];
    uint8_t derived_key[16];
    size_t derived_len;
    
    if (!ctx) {
        return SE050_ERR_INVALID_ARG;
    }
    
    /* 
     * SCP03 Key Derivation:
     * - Use DEK to encrypt a derivation constant
     * - Result is used as session key material
     * 
     * Note: This is a simplified derivation. Real SCP03 uses more complex
     * derivation with counters and nonces.
     */
    
    /* Derivation constant for ENC key */
    memset(derive_data, 0, sizeof(derive_data));
    derive_data[0] = 0x01;  /* ENC key derivation */
    
    if (scp03_aes_cbc_encrypt(ctx->dek_key, SCP03_KEY_SIZE,
                               derive_data, SCP03_IV_SIZE,
                               derive_data, sizeof(derive_data),
                               derived_key, &derived_len) != 0) {
        return SE050_ERR_SCP03;
    }
    
    /* Update ENC key with derived key */
    for (size_t i = 0; i < SCP03_KEY_SIZE; i++) {
        ctx->enc_key[i] ^= derived_key[i];
    }
    
    /* Derivation constant for MAC key */
    memset(derive_data, 0, sizeof(derive_data));
    derive_data[0] = 0x02;  /* MAC key derivation */
    
    if (scp03_aes_cbc_encrypt(ctx->dek_key, SCP03_KEY_SIZE,
                               derive_data, SCP03_IV_SIZE,
                               derive_data, sizeof(derive_data),
                               derived_key, &derived_len) != 0) {
        return SE050_ERR_SCP03;
    }
    
    /* Update MAC key with derived key */
    for (size_t i = 0; i < SCP03_KEY_SIZE; i++) {
        ctx->mac_key[i] ^= derived_key[i];
    }
    
    /* Securely zeroize */
    memzero_explicit(derive_data, sizeof(derive_data));
    memzero_explicit(derived_key, sizeof(derived_key));
    
    return SE050_OK;
}

se050_status_t se050_scp03_init(se050_scp03_ctx_t **ctx, se050_session_ctx_t *session)
{
    se050_scp03_ctx_t *scp03;
    
    if (!ctx || !session) {
        return SE050_ERR_INVALID_ARG;
    }
    
    /* Allocate SCP03 context from static pool */
    scp03 = se050_scp03_alloc_pool();
    if (!scp03) {
        return SE050_ERR_FAIL;
    }
    
    scp03->session = session;
    scp03->cmd_counter = 0;
    scp03->rsp_counter = 0;
    scp03->initialized = 0;
    
    /* Zeroize keys */
    memzero_explicit(scp03->enc_key, sizeof(scp03->enc_key));
    memzero_explicit(scp03->mac_key, sizeof(scp03->mac_key));
    memzero_explicit(scp03->dek_key, sizeof(scp03->dek_key));
    memzero_explicit(scp03->cmd_icv, sizeof(scp03->cmd_icv));
    memzero_explicit(scp03->rsp_icv, sizeof(scp03->rsp_icv));
    
    *ctx = scp03;
    return SE050_OK;
}

void se050_scp03_free(se050_scp03_ctx_t *ctx)
{
    if (!ctx) {
        return;
    }
    
    /* Securely zeroize all sensitive data */
    if (ctx->initialized) {
        memzero_explicit(ctx->enc_key, sizeof(ctx->enc_key));
        memzero_explicit(ctx->mac_key, sizeof(ctx->mac_key));
        memzero_explicit(ctx->dek_key, sizeof(ctx->dek_key));
        memzero_explicit(ctx->cmd_icv, sizeof(ctx->cmd_icv));
        memzero_explicit(ctx->rsp_icv, sizeof(ctx->rsp_icv));
    }
    
    /* Free SCP03 context to static pool */
    se050_scp03_free_pool(ctx);
}

se050_status_t se050_scp03_set_keys(se050_scp03_ctx_t *ctx,
                                     const uint8_t *enc_key,
                                     const uint8_t *mac_key,
                                     const uint8_t *dek_key)
{
    se050_status_t status;
    
    if (!ctx || !enc_key || !mac_key || !dek_key) {
        return SE050_ERR_INVALID_ARG;
    }
    
    /* Copy keys securely */
    secure_memcpy(ctx->enc_key, enc_key, SCP03_KEY_SIZE);
    secure_memcpy(ctx->mac_key, mac_key, SCP03_KEY_SIZE);
    secure_memcpy(ctx->dek_key, dek_key, SCP03_KEY_SIZE);
    
    /* Derive session keys from DEK */
    status = scp03_derive_session_keys(ctx);
    if (status != SE050_OK) {
        return status;
    }
    
    /* Initialize ICVs */
    memset(ctx->cmd_icv, 0, sizeof(ctx->cmd_icv));
    memset(ctx->rsp_icv, 0, sizeof(ctx->rsp_icv));
    
    /* Reset counters */
    ctx->cmd_counter = 0;
    ctx->rsp_counter = 0;
    
    ctx->initialized = 1;
    
    return SE050_OK;
}

se050_status_t se050_scp03_load_keys_from_file(se050_scp03_ctx_t *ctx, const char *file_path)
{
    FILE *fp;
    uint8_t keys[48];  /* ENC (16) + MAC (16) + DEK (16) */
    size_t bytes_read;
    
    if (!ctx || !file_path) {
        return SE050_ERR_INVALID_ARG;
    }
    
    /* Open key file */
    fp = fopen(file_path, "rb");
    if (!fp) {
        return SE050_ERR_FAIL;
    }
    
    /* Read all 48 bytes (ENC + MAC + DEK) */
    bytes_read = fread(keys, 1, sizeof(keys), fp);
    fclose(fp);
    
    if (bytes_read < 48) {
        return SE050_ERR_FAIL;
    }
    
    /* Set all three keys */
    se050_scp03_set_keys(ctx, keys, &keys[16], &keys[32]);
    
    /* Securely zeroize */
    memzero_explicit(keys, sizeof(keys));
    
    return SE050_OK;
}

/* ============================================================================
 * SCP03 Encryption/Decryption
 * ============================================================================ */

se050_status_t se050_scp03_encrypt_command(se050_scp03_ctx_t *ctx,
                                            uint8_t *cmd,
                                            size_t *cmd_len)
{
    uint8_t iv[SCP03_IV_SIZE];
    uint8_t mac[SCP03_CMAC_SIZE];
    size_t mac_len = SCP03_CMAC_SIZE;
    uint8_t padded_cmd[SCP03_MAX_CMD_LEN + 16];
    size_t padded_len;
    size_t encrypted_len;
    
    if (!ctx || !cmd || !cmd_len) {
        return SE050_ERR_INVALID_ARG;
    }
    
    if (!ctx->initialized) {
        return SE050_ERR_NOT_INIT;
    }
    
    if (*cmd_len > SCP03_MAX_CMD_LEN) {
        return SE050_ERR_INVALID_ARG;
    }
    
    /* Calculate IV from counter */
    memset(iv, 0, sizeof(iv));
    iv[0] = (uint8_t)((ctx->cmd_counter >> 56) & 0xFF);
    iv[1] = (uint8_t)((ctx->cmd_counter >> 48) & 0xFF);
    iv[2] = (uint8_t)((ctx->cmd_counter >> 40) & 0xFF);
    iv[3] = (uint8_t)((ctx->cmd_counter >> 32) & 0xFF);
    iv[4] = (uint8_t)((ctx->cmd_counter >> 24) & 0xFF);
    iv[5] = (uint8_t)((ctx->cmd_counter >> 16) & 0xFF);
    iv[6] = (uint8_t)((ctx->cmd_counter >> 8) & 0xFF);
    iv[7] = (uint8_t)(ctx->cmd_counter & 0xFF);
    
    /* Pad command (PKCS#7) */
    padded_len = *cmd_len + 1;
    memcpy(padded_cmd, cmd, *cmd_len);
    padded_cmd[*cmd_len] = 0x80;
    memset(padded_cmd + padded_len, 0, 15 - ((*cmd_len + 1) % 16));
    padded_len = ((padded_len + 15) / 16) * 16;
    
    /* Encrypt */
    if (scp03_aes_cbc_encrypt(ctx->enc_key, SCP03_KEY_SIZE,
                               iv, SCP03_IV_SIZE,
                               padded_cmd, padded_len,
                               cmd, &encrypted_len) != 0) {
        return SE050_ERR_SCP03;
    }
    
    /* Calculate ICV (CMAC) */
    if (scp03_calc_cmac(ctx->mac_key, SCP03_KEY_SIZE,
                        cmd, encrypted_len,
                        mac, &mac_len) != 0) {
        return SE050_ERR_SCP03;
    }
    
    /* Update ICV */
    memcpy(ctx->cmd_icv, mac, SCP03_CMAC_SIZE);
    
    /* Increment counter */
    ctx->cmd_counter++;
    
    *cmd_len = encrypted_len;
    
    return SE050_OK;
}

uint16_t se050_scp03_decrypt_response(se050_scp03_ctx_t *ctx,
                                       size_t cmd_len,
                                       uint8_t *rsp,
                                       size_t *rsp_len)
{
    (void)cmd_len;  /* Used in production for MAC verification */
    uint8_t iv[SCP03_IV_SIZE];
    uint8_t mac[SCP03_CMAC_SIZE];
    size_t mac_len = SCP03_CMAC_SIZE;
    uint8_t decrypted[SCP03_MAX_RSP_LEN];
    size_t decrypted_len;
    uint16_t sw;
    int ret;
    
    if (!ctx || !rsp || !rsp_len) {
        return SCP03_SW_FAIL;
    }
    
    if (!ctx->initialized) {
        return SCP03_SW_FAIL;
    }
    
    if (*rsp_len < 2) {
        return SCP03_SW_FAIL;
    }
    
    /* Calculate IV from counter */
    memset(iv, 0, sizeof(iv));
    iv[0] = (uint8_t)((ctx->rsp_counter >> 56) & 0xFF);
    iv[1] = (uint8_t)((ctx->rsp_counter >> 48) & 0xFF);
    iv[2] = (uint8_t)((ctx->rsp_counter >> 40) & 0xFF);
    iv[3] = (uint8_t)((ctx->rsp_counter >> 32) & 0xFF);
    iv[4] = (uint8_t)((ctx->rsp_counter >> 24) & 0xFF);
    iv[5] = (uint8_t)((ctx->rsp_counter >> 16) & 0xFF);
    iv[6] = (uint8_t)((ctx->rsp_counter >> 8) & 0xFF);
    iv[7] = (uint8_t)(ctx->rsp_counter & 0xFF);
    
    /* Calculate expected ICV */
    ret = scp03_calc_cmac(ctx->mac_key, SCP03_KEY_SIZE,
                          rsp, *rsp_len,
                          mac, &mac_len);
    if (ret != 0) {
        return SCP03_SW_FAIL;
    }
    
    /* Verify ICV (constant-time comparison) */
    if (crypto_memneq(mac, ctx->rsp_icv, SCP03_CMAC_SIZE) != 0) {
        return SCP03_SW_FAIL;
    }
    
    /* Decrypt */
    ret = scp03_aes_cbc_decrypt(ctx->enc_key, SCP03_KEY_SIZE,
                                 iv, SCP03_IV_SIZE,
                                 rsp, *rsp_len,
                                 decrypted, &decrypted_len);
    if (ret != 0) {
        return SCP03_SW_FAIL;
    }
    
    /* Remove padding */
    if (decrypted_len > 0 && decrypted[decrypted_len - 1] == 0x80) {
        decrypted_len--;
    }
    
    /* Copy decrypted data */
    memcpy(rsp, decrypted, decrypted_len);
    *rsp_len = decrypted_len;
    
    /* Extract status word */
    if (decrypted_len >= 2) {
        sw = (uint16_t)((decrypted[decrypted_len - 2] << 8) | decrypted[decrypted_len - 1]);
    } else {
        sw = SCP03_SW_FAIL;
    }
    
    /* Update ICV */
    memcpy(ctx->rsp_icv, mac, SCP03_CMAC_SIZE);
    
    /* Increment counter */
    ctx->rsp_counter++;
    
    return sw;
}