lib_its_security.c

/*!
 * \File      lib_its_security.c
 * \brief     Source file for Security external functions.
 * \author    FSCOM
 * \copyright FSCOM Copyright Notification
 *            No part may be reproduced except as authorized by written permission.
 *            The copyright and the foregoing restriction extend to reproduction in all media.
 *            All rights reserved.
 * \version   1.0
 */
#include "lib_its_security.h"

#include <math.h>
#include <assert.h>
#ifndef _Win64
#include <arpa/inet.h>
#else
#define WIN32_LEAN_AND_MEAN             // Exclude rarely-used stuff from Windows headers
 // Windows Header Files
#include <windows.h>
#include <Winsock2.h>
#endif

#include <openssl/bio.h>

/**
 * Internal functions
 */

void show_ec_key(const int8_t* p_prefix, lib_its_security_context_t* p_lib_its_security_context) {
    fprintf(stderr, "%s: ", p_prefix);
    BIGNUM* x = BN_new();
    BIGNUM* y = BN_new();
    const EC_POINT* keys = EC_KEY_get0_public_key(p_lib_its_security_context->ec_key);
    if (EC_POINT_get_affine_coordinates_GFp(p_lib_its_security_context->ec_group, keys, x, y, NULL)) {
        BN_print_fp(stderr, x);
        fprintf(stderr, "\n");
        BN_print_fp(stderr, y);
        fprintf(stderr, "\n");
    }
    BN_free(x);
    BN_free(y);
}

void show_ec_point(const int8_t* p_prefix, lib_its_security_context_t* p_lib_its_security_context, EC_POINT* p_ec_point) {
    fprintf(stderr, "%s: ", p_prefix);
    char* result = EC_POINT_point2hex(p_lib_its_security_context->ec_group, p_ec_point, POINT_CONVERSION_UNCOMPRESSED, p_lib_its_security_context->bn_ctx);
    if (result != NULL) {
        fprintf(stderr, "%s\n", result);
#ifdef _Win64
        free(result);
#endif
    }
    else {
        fprintf(stderr, "(null)\n");
    }
}

void show_hex(const int8_t* p_prefix, const void* p_buffer, size_t p_buffer_length) {
    fprintf(stderr, "%s: ", p_prefix);
    for (uint8_t* p = (unsigned char*)p_buffer; p_buffer_length; p_buffer_length--, p++) {
        fprintf(stderr, "%02x", *p);
    }
    putc('\n', stderr);
}

int8_t* bin_to_hex(const uint8_t* p_buffer, const size_t p_buffer_length) {
    int8_t* buf = NULL;
    size_t i = 0, j = 0;

    // Sanity check
    if (p_buffer_length == 0) {
        return NULL;
    }

    buf = (int8_t*)malloc(p_buffer_length << 1);
    do {
        *(buf + j) = "0123456789ABCDEF"[(*(p_buffer + i) >> 4) & 0x0F];
        *(buf + j + 1) = "0123456789ABCDEF"[*(p_buffer + i) & 0x0F];
        i += 1; j += 2;
    } while (i < p_buffer_length);

    return buf;
}

uint8_t* hex_to_bin(const int8_t* p_buffer, size_t* p_buffer_length) {
    int8_t a;
    size_t i, len;
    uint8_t* retval = NULL;

    // Sanity check
    if (p_buffer == NULL) {
        return NULL;
    }
    if ((len = strlen((const char*)p_buffer)) & 1) {
        return NULL;
    }

    retval = (uint8_t*)malloc(len >> 1);
    for (i = 0; i < len; i++) {
        a = toupper(*(p_buffer + i));
        if (!isxdigit(a)) {
            break;
        }
        if (isdigit(a)) {
            a -= '0';
        }
        else {
            a = a - 'A' + 0x0A;
        }
        if (i & 1) {
            retval[i >> 1] |= a;
        }
        else {
            retval[i >> 1] = a << 4;
        }
    } // End of 'for' statement
    if (i < len) {
        free(retval);
        retval = NULL;
    }
    *p_buffer_length = len >> 1;

    return retval;
}

int32_t sign(
    lib_its_security_context_t* p_lib_its_security_context,
    const uint8_t* p_data,
    const size_t p_data_length,
    uint8_t** p_sig_r,
    uint8_t** p_sig_s,
    size_t* p_sig_length
) {
    // Sanity checks
    if ((p_lib_its_security_context == NULL) || (p_data == NULL)) {
        return -1;
    }

    ECDSA_SIG* signature = ECDSA_do_sign(p_data, p_data_length, p_lib_its_security_context->ec_key);
    if (signature == NULL) {
        return -1;
    }

    if (ECDSA_do_verify(p_data, p_data_length, signature, p_lib_its_security_context->ec_key) != 1) {
        return -1;
    }

    const BIGNUM* r;
    const BIGNUM* s;
    ECDSA_SIG_get0(signature, &r, &s);
    *p_sig_length = BN_num_bytes(s);
    *p_sig_r = (uint8_t*)malloc(*p_sig_length);
    BN_bn2bin(r, (uint8_t*)(*p_sig_r));
    *p_sig_s = (uint8_t*)malloc(*p_sig_length);
    BN_bn2bin(s, (uint8_t*)(*p_sig_s));

    ECDSA_SIG_free(signature);

    return 0;
}

int32_t sign_verify(
    lib_its_security_context_t* p_lib_its_security_context,
    const uint8_t* p_data,
    const size_t p_data_length,
    const uint8_t* p_sig_r,
    const uint8_t* p_sig_s,
    const size_t p_sig_length
) {
    show_hex((const int8_t*)">>> sign_verify: p_data", p_data, p_data_length);
    show_hex((const int8_t*)">>> sign_verify: p_sig_r", p_sig_r, p_sig_length);
    show_hex((const int8_t*)">>> sign_verify: p_sig_s", p_sig_s, p_sig_length);

    // Sanity checks
    if ((p_lib_its_security_context == NULL) || (p_data == NULL) || (p_sig_r == NULL) || (p_sig_s == NULL)) {
        return -1;
    }

    // Build the signature
    BIGNUM* r = BN_bin2bn(p_sig_r, p_sig_length, NULL);
    BIGNUM* s = BN_bin2bn(p_sig_s, p_sig_length, NULL);
    ECDSA_SIG* signature = ECDSA_SIG_new();
    ECDSA_SIG_set0(signature, r, s);
    // Check the signature
    int32_t result = ECDSA_do_verify(p_data, p_data_length, signature, p_lib_its_security_context->ec_key);
    fprintf(stderr, "sign_verify: result=%d\n", result);
    ECDSA_SIG_free(signature);

    return (result == 1) ? 0 : -1;
}

int bin_to_ec_point(
    lib_its_security_context_t* p_lib_its_security_context,
    const uint8_t* p_public_key_x,
    const uint8_t* p_public_key_y, EC_POINT** p_ec_point
) {
    BIGNUM* pubk_bn = NULL;
    size_t l = 2 * p_lib_its_security_context->key_length + 1;
    uint8_t* v = (uint8_t*)malloc(l);
    *v = 0x04;
    memcpy((void*)(v + 1), (const void*)p_public_key_x, p_lib_its_security_context->key_length);
    memcpy((void*)(v + 1 + p_lib_its_security_context->key_length), (const void*)p_public_key_y, p_lib_its_security_context->key_length);
    pubk_bn = BN_bin2bn(v, l, NULL);
    *p_ec_point = EC_POINT_new(p_lib_its_security_context->ec_group);
    EC_POINT_bn2point(p_lib_its_security_context->ec_group, pubk_bn, *p_ec_point, p_lib_its_security_context->bn_ctx);
    BN_clear_free(pubk_bn);
    free(v);

    return 0;
}

int public_key_to_bin(
    lib_its_security_context_t* p_lib_its_security_context,
    uint8_t** p_bin_key
) {
    const EC_GROUP* ec_group = EC_KEY_get0_group(p_lib_its_security_context->ec_key);
    const EC_POINT* pub = EC_KEY_get0_public_key(p_lib_its_security_context->ec_key);
    BIGNUM* pub_bn = BN_new();

    EC_POINT_point2bn(ec_group, pub, POINT_CONVERSION_UNCOMPRESSED, pub_bn, p_lib_its_security_context->bn_ctx);
    *p_bin_key = (uint8_t*)malloc(BN_num_bytes(pub_bn));
    BN_bn2bin(pub_bn, *p_bin_key);

    BN_clear_free(pub_bn);

    return 0;
}

int kdf2_sha256(
    lib_its_security_context_t* p_lib_its_security_context,
    const uint8_t* p_salt,
    const int32_t p_salt_length,
    const int32_t p_key_length,
    uint8_t** p_digest, size_t* p_digest_length
) {
    // Sanity checks

    int sha256_blk_len = 32;
    int num_blk_out = (int)ceil(p_key_length / (float)sha256_blk_len);
    uint8_t* digest = (uint8_t*)malloc((num_blk_out + 1) * sha256_blk_len);
    int32_t digest_idx = 0;
    const size_t hash_input_length = p_lib_its_security_context->secret_key_length + sizeof(int32_t) + p_salt_length;
    uint8_t* hash_input = (uint8_t*)malloc(hash_input_length);
    int i_ntonl;
    for (int32_t i = 1; i < num_blk_out + 1; i++) {
        uint8_t* p = hash_input;
        memcpy((void*)p, (const void*)p_lib_its_security_context->secret_key, p_lib_its_security_context->secret_key_length);
        p += p_lib_its_security_context->secret_key_length;
        i_ntonl = htonl(i);
        memcpy((void*)p, (const void*)&i_ntonl, sizeof(int32_t));
        p += sizeof(int32_t);
        memcpy((void*)p, (const void*)p_salt, p_salt_length);
        //show_hex((const int8_t*)"hash_input", (const void*)hash_input, hash_input_length);
        uint8_t* h;
        hash_with_sha256(hash_input, hash_input_length, &h);
        //show_hex((const int8_t*)"h", (const void*)h, 32);
        memcpy((void*)(digest + digest_idx), (const void*)h, sha256_blk_len);
        //show_hex((const int8_t*)"digest", (const void*)digest, digest_idx + sha256_blk_len);
        digest_idx += sha256_blk_len;
        free(h);
    } // End of 'for' statement
    free(hash_input);
    if (digest_idx > p_key_length * 2) {
        digest_idx = p_key_length * 2;
    }
    *p_digest = (uint8_t*)malloc(digest_idx);
    memcpy((void*)(*p_digest), (const void*)digest, digest_idx);
    *p_digest_length = digest_idx;
    free(digest);

    return 0;
}

int32_t kdf2(
    lib_its_security_context_t* p_lib_its_security_context,
    const uint8_t* p_salt,
    const int32_t p_salt_length,
    const int32_t p_key_length,
    const unsigned char p_hash_algorithm,
    uint8_t** p_digest,
    size_t* p_digest_length
) {
    // Sanity checks

    int result = -1;
    switch (p_hash_algorithm) {
    case 0x00: // SHA 256
        result = kdf2_sha256(p_lib_its_security_context, p_salt, p_salt_length, p_key_length, p_digest, p_digest_length);
        break;
    } // End of 'switch' statement

    return result;
}

int32_t generate_and_derive_ephemeral_key_for_encryption(
    lib_its_security_context_t* p_ecdh_private_key,
    const encryption_algorithm_t p_enc_algorithm,
    lib_its_security_context_t* p_public_keys,
    const uint8_t* p_salt,
    const size_t p_salt_length
) {
    // Sanity checks
    if (p_public_keys->private_key != NULL) {
        return -1;
    }
    if ((p_public_keys->public_key_x == NULL) || (p_public_keys->public_key_y == NULL)) {
        return -1;
    }

    // Set buffers size
    p_ecdh_private_key->encryption_algorithm = p_enc_algorithm;
    switch (p_ecdh_private_key->encryption_algorithm) {
    case aes_128_ccm:
        // No break;
    case aes_128_gcm:
        p_ecdh_private_key->nonce_length = 12;
        p_ecdh_private_key->sym_key_length = 16;
        p_ecdh_private_key->tag_length = 16;
        break;
    default:
        return -1;
    } // End of 'switch' statement
    uint8_t k_enc;
    uint8_t k_mac;
    switch (p_ecdh_private_key->elliptic_curve) {
    case nist_p_256: // Use the ANSI X9.62 Prime 256v1 curve 
      // No break;
    case brainpool_p_256_r1:
        k_enc = 16;
        k_mac = 32;
        break;
    case brainpool_p_384_r1:
        k_enc = 24; // TODO To be checked
        k_mac = 48;
        break;
    default:
        return -1;
    } // End of 'switch' statement

    /* Convert the ephemeral public encryption keys to an EC point */
    EC_POINT* ec_point = NULL;
    bin_to_ec_point(p_public_keys, p_public_keys->public_key_x, p_public_keys->public_key_y, &ec_point);
    show_ec_point((const int8_t*)"ec_point", p_public_keys, ec_point);
    /* Generate the shared secret key (Key Agreement) */
    p_ecdh_private_key->secret_key_length = (EC_GROUP_get_degree(p_ecdh_private_key->ec_group) + 7) / 8;
    p_ecdh_private_key->secret_key = (uint8_t*)malloc(p_ecdh_private_key->secret_key_length);
    int32_t result = ECDH_compute_key(
        p_ecdh_private_key->secret_key,
        p_ecdh_private_key->secret_key_length,
        ec_point, // From recipient's public keys
        p_ecdh_private_key->ec_key,  // From ephemeral's private key
        NULL);
    if (result != p_ecdh_private_key->secret_key_length) {
        free(p_ecdh_private_key->secret_key);
        p_ecdh_private_key->secret_key = NULL;
        EC_POINT_free(ec_point);
        return -1;
    }
    EC_POINT_free(ec_point);
    show_hex((const int8_t*)"secret", p_ecdh_private_key->secret_key, p_ecdh_private_key->secret_key_length);

    /* Derive the shared secret key */
    uint8_t* digest;
    size_t digest_length;
    if (kdf2(p_ecdh_private_key, p_salt, p_salt_length, k_enc + k_mac, 0x00/*sha256*/, &digest, &digest_length) != 0) {
        free(p_ecdh_private_key->secret_key);
        p_ecdh_private_key->secret_key = NULL;
        return -1;
    }
    show_hex((const int8_t*)"digest", digest, digest_length);

    /* Extract K1 and generate encrypted symmetric key */
    uint8_t* k1 = (uint8_t*)malloc(k_enc);
    memcpy((void*)k1, (const void*)digest, k_enc);
    show_hex((const int8_t*)"k1", k1, k_enc);
    BIGNUM* r = BN_new();
    BN_pseudo_rand(r, k_enc * 8, -1, 0);
    p_ecdh_private_key->sym_key = (uint8_t*)malloc(k_enc);
    p_ecdh_private_key->sym_key_length = k_enc;
    BN_bn2bin(r, p_ecdh_private_key->sym_key);
    BN_free(r);
    show_hex((const int8_t*)"sym_key", p_ecdh_private_key->sym_key, p_ecdh_private_key->sym_key_length);
    p_ecdh_private_key->enc_sym_key = (uint8_t*)malloc(k_enc);
    for (int i = 0; i < k_enc; *(p_ecdh_private_key->enc_sym_key + i) = *(k1 + i) ^ *(p_ecdh_private_key->sym_key + i), i++);
    show_hex((const int8_t*)"enc_sym_key", p_ecdh_private_key->enc_sym_key, p_ecdh_private_key->sym_key_length);

    // Extract K2 and generate Tag vector
    int32_t k2_length = k_enc * 2;
    uint8_t* k2 = (uint8_t*)malloc(k2_length);
    memcpy((void*)k2, (const void*)(k_enc + digest), k2_length);
    show_hex((const int8_t*)"k2", k2, k2_length);
    hmac_sha256(k2, k2_length, p_ecdh_private_key->enc_sym_key, p_ecdh_private_key->sym_key_length, &p_ecdh_private_key->tag);
    show_hex((const int8_t*)"tag", p_ecdh_private_key->tag, p_ecdh_private_key->tag_length);

    /* Generate random IV (nonce) */
    r = BN_new();
    BN_pseudo_rand(r, p_ecdh_private_key->nonce_length * 8, -1, 0);
    p_ecdh_private_key->nonce = (uint8_t*)malloc(p_ecdh_private_key->nonce_length);
    BN_bn2bin(r, p_ecdh_private_key->nonce);
    BN_free(r);
    show_hex((const int8_t*)"nonce", p_ecdh_private_key->nonce, p_ecdh_private_key->nonce_length);

    free(digest);
    free(k1);
    free(k2);
    return 0;
}

int32_t generate_and_derive_ephemeral_key_for_decryption(
    lib_its_security_context_t* p_lib_its_security_context,
    const encryption_algorithm_t p_enc_algorithm,
    lib_its_security_context_t* p_ephemeral_keys,
    const uint8_t* p_enc_sym_key,
    const uint8_t* p_nonce,
    const uint8_t* p_authentication_vector,
    const uint8_t* p_salt,
    const size_t p_salt_length
) {
    /* Sanity checks */
    if ((p_lib_its_security_context->public_key_x == NULL) || (p_lib_its_security_context->public_key_y == NULL)) {
        return -1;
    }

    /* Set buffers size */
    p_lib_its_security_context->encryption_algorithm = p_enc_algorithm;
    switch (p_lib_its_security_context->encryption_algorithm) {
    case aes_128_ccm:
        // No break;
    case aes_128_gcm:
        p_lib_its_security_context->nonce_length = 12;
        p_lib_its_security_context->sym_key_length = 16;
        p_lib_its_security_context->tag_length = 16;
        break;
    default:
        return -1;
    } // End of 'switch' statement
    unsigned int k_enc;
    unsigned int k_mac;
    switch (p_lib_its_security_context->elliptic_curve) {
    case nist_p_256: // Use the ANSI X9.62 Prime 256v1 curve 
      // No break;
    case brainpool_p_256_r1:
        k_enc = 16;
        k_mac = 32;
        break;
    case brainpool_p_384_r1:
        break;
    default:
        return -1;
    } // End of 'switch' statement

    /* Fill context buffer */
    p_lib_its_security_context->nonce = (uint8_t*)malloc(p_lib_its_security_context->nonce_length);
    memcpy((void*)p_lib_its_security_context->nonce, (const void*)p_nonce, p_lib_its_security_context->nonce_length);
    p_lib_its_security_context->enc_sym_key = (uint8_t*)malloc(p_lib_its_security_context->sym_key_length);
    memcpy((void*)p_lib_its_security_context->enc_sym_key, (const void*)p_enc_sym_key, p_lib_its_security_context->sym_key_length);
    p_lib_its_security_context->tag = (uint8_t*)malloc(p_lib_its_security_context->tag_length);
    memcpy((void*)p_lib_its_security_context->tag, (const void*)p_authentication_vector, p_lib_its_security_context->tag_length);

    /* Convert the ephemeral key to an EC point */
    EC_POINT* ec_point = NULL;
    bin_to_ec_point(p_ephemeral_keys, p_ephemeral_keys->public_key_x, p_ephemeral_keys->public_key_y, &ec_point); // EC_POINT from ephemeral keys
    // Generate the shared secret key (Key Agreement)
    p_lib_its_security_context->secret_key_length = (EC_GROUP_get_degree(p_lib_its_security_context->ec_group) + 7) / 8;
    p_lib_its_security_context->secret_key = (uint8_t*)malloc(p_lib_its_security_context->secret_key_length);
    int32_t result = ECDH_compute_key(
        p_lib_its_security_context->secret_key,
        p_lib_its_security_context->secret_key_length,
        ec_point, // From ephemeral keys
        p_lib_its_security_context->ec_key,  // From recipient's private key
        NULL);
    if (result != p_lib_its_security_context->secret_key_length) {
        free(p_lib_its_security_context->secret_key);
        p_lib_its_security_context->secret_key = NULL;
        EC_POINT_free(ec_point);
        return -1;
    }
    EC_POINT_free(ec_point);
    show_hex((const int8_t*)"secret", p_lib_its_security_context->secret_key, p_lib_its_security_context->secret_key_length);

    /* Derive the shared secret key */
    uint8_t* digest;
    size_t digest_length;
    if (kdf2(p_lib_its_security_context, p_salt, p_salt_length, k_enc + k_mac, 0x00/*sha256*/, &digest, &digest_length) != 0) {
        free(p_lib_its_security_context->secret_key);
        p_lib_its_security_context->secret_key = NULL;
        return -1;
    }
    show_hex((const int8_t*)"digest", digest, digest_length);

    /* Extract K2 and generate Tag vector */
    int32_t k2_length = k_enc * 2;
    uint8_t* k2 = (uint8_t*)malloc(k2_length);
    memcpy((void*)k2, (const void*)(k_enc + digest), k2_length);
    show_hex((const int8_t*)"k2", k2, k2_length);
    hmac_sha256(k2, k2_length, p_lib_its_security_context->enc_sym_key, p_lib_its_security_context->sym_key_length, &p_lib_its_security_context->tag);
    show_hex((const int8_t*)"authentication vector", p_lib_its_security_context->tag, p_lib_its_security_context->tag_length);

    /* Extract K1 and generate encrypted symmetric key */
    uint8_t* k1 = (uint8_t*)malloc(k_enc);
    memcpy((void*)k1, (const void*)digest, k_enc);
    show_hex((const int8_t*)"k1", k1, k_enc);
    p_lib_its_security_context->sym_key = (uint8_t*)malloc(k_enc);
    for (unsigned int i = 0; i < k_enc; *(p_lib_its_security_context->sym_key + i) = *(k1 + i) ^ *(p_lib_its_security_context->enc_sym_key + i), i++);
    show_hex((const int8_t*)"sym_key", p_lib_its_security_context->sym_key, p_lib_its_security_context->sym_key_length);
    free(k1);
    free(k2);
    free(digest);

    return 0;
}

/**
 * Public functions
 */

int32_t initialize(
    const ecc_elliptic_curves_t p_elliptic_curve,
    lib_its_security_context_t** p_lib_its_security_context
) {
    // Sanity checks
    if (p_lib_its_security_context == NULL) {
        return -1;
    }

    OpenSSL_add_all_algorithms();
    ERR_load_crypto_strings();
    ERR_clear_error();

    *p_lib_its_security_context = (lib_its_security_context_t*)malloc(sizeof(lib_its_security_context_t));
    if (*p_lib_its_security_context == NULL) {
        return -1;
    }
    (*p_lib_its_security_context)->elliptic_curve = p_elliptic_curve;
    (*p_lib_its_security_context)->ec_key = NULL;
    (*p_lib_its_security_context)->ec_group = NULL;
    (*p_lib_its_security_context)->bn_ctx = NULL;
    (*p_lib_its_security_context)->key_length = -1;
    (*p_lib_its_security_context)->private_key = NULL;
    (*p_lib_its_security_context)->public_key_x = NULL;
    (*p_lib_its_security_context)->public_key_y = NULL;
    (*p_lib_its_security_context)->public_key_c = NULL;
    (*p_lib_its_security_context)->secret_key = NULL;
    (*p_lib_its_security_context)->sym_key = NULL;
    (*p_lib_its_security_context)->enc_sym_key = NULL;
    (*p_lib_its_security_context)->tag = NULL;
    (*p_lib_its_security_context)->nonce = NULL;
    (*p_lib_its_security_context)->secret_key_length = -1;
    (*p_lib_its_security_context)->sym_key_length = -1;
    (*p_lib_its_security_context)->nonce_length = -1;
    (*p_lib_its_security_context)->tag_length = -1;

    int32_t result = -1;
    switch (p_elliptic_curve) {
    case nist_p_256: // Use the ANSI X9.62 Prime 256v1 curve
        (*p_lib_its_security_context)->key_length = 32;
        result = OBJ_txt2nid("prime256v1");
        break;
    case brainpool_p_256_r1:
        (*p_lib_its_security_context)->key_length = 32;
        result = OBJ_txt2nid("brainpoolP256r1");
        break;
    case brainpool_p_384_r1:
        (*p_lib_its_security_context)->key_length = 48;
        result = OBJ_txt2nid("brainpoolP384r1");
        break;
    default:
        fprintf(stderr, "lib_its_security::initialize: Unsupported EC elliptic_curve\n");
    } // End of 'switch' statement
    if (result < 0) {
        return -1;
    }

    (*p_lib_its_security_context)->ec_key = EC_KEY_new_by_curve_name(result); /* Set the elliptic curve */
    EC_KEY_set_asn1_flag((*p_lib_its_security_context)->ec_key, OPENSSL_EC_NAMED_CURVE); /* Used to save and retrieve keys */
    (*p_lib_its_security_context)->ec_group = (EC_GROUP*)EC_KEY_get0_group((*p_lib_its_security_context)->ec_key); /* Get pointer to the EC_GROUP */
    (*p_lib_its_security_context)->bn_ctx = BN_CTX_new();

    return 0;
}

int32_t initialize_with_private_key(
    const ecc_elliptic_curves_t p_elliptic_curve,
    const uint8_t* p_private_key,
    lib_its_security_context_t** p_lib_its_security_context
) {
    // Sanity checks
    if ((p_lib_its_security_context == NULL) || (p_private_key == NULL)) {
        return -1;
    }

    if (initialize(p_elliptic_curve, p_lib_its_security_context) == -1) {
        return -1;
    }
    EC_KEY_set_conv_form((*p_lib_its_security_context)->ec_key, POINT_CONVERSION_UNCOMPRESSED);

    // Build private key
    BIGNUM* p = BN_new();
    BN_bin2bn(p_private_key, (*p_lib_its_security_context)->key_length, p);
    // Build public keys
    EC_POINT* ec_point = EC_POINT_new((*p_lib_its_security_context)->ec_group);
    EC_POINT_mul((*p_lib_its_security_context)->ec_group, ec_point, p, NULL, NULL, (*p_lib_its_security_context)->bn_ctx);
    // Set private key
    EC_KEY_set_private_key((*p_lib_its_security_context)->ec_key, p);
    if (EC_KEY_check_key((*p_lib_its_security_context)->ec_key) != 0) {
        EC_POINT_free(ec_point);
        BN_clear_free(p);
        return -1;
    }
    BN_clear_free(p);
    // Private key is correct, set public keys
    EC_KEY_set_public_key((*p_lib_its_security_context)->ec_key, ec_point);
    BIGNUM* xy = BN_new();
    EC_POINT_point2bn((*p_lib_its_security_context)->ec_group, ec_point, POINT_CONVERSION_UNCOMPRESSED, xy, (*p_lib_its_security_context)->bn_ctx);
    if (BN_num_bytes(xy) == 0) {
        EC_POINT_free(ec_point);
        BN_clear_free(xy);
        return -1;
    }

    int32_t v_length = BN_num_bytes(xy);
    uint8_t* vv = (uint8_t*)malloc(v_length);
    BN_bn2bin(xy, vv);
    if ((v_length % 2) != 0) {
        // Remove first byte
        //    v_length -= 1;
        //    memcpy((void*)vv, (const void*)(vv + 1), v_length - 1);
        uint8_t* v = (uint8_t*)malloc(v_length - 1);
        memcpy((void*)v, (const void*)(vv + 1), v_length - 1);
        free(vv);
        vv = v;
        v_length -= 1;
    }
    BN_clear_free(xy);
    const int l = v_length / 2;
    (*p_lib_its_security_context)->public_key_x = (uint8_t*)malloc(l);
    memcpy((void*)(*p_lib_its_security_context)->public_key_x, (const void*)vv, l);
    (*p_lib_its_security_context)->public_key_y = (uint8_t*)malloc(l);
    memcpy((void*)(*p_lib_its_security_context)->public_key_y, (const void*)(vv + l), l);
    // Compressed
    int len = EC_POINT_point2oct((*p_lib_its_security_context)->ec_group, ec_point, POINT_CONVERSION_COMPRESSED, NULL, 0, (*p_lib_its_security_context)->bn_ctx);
    if (len != 0) {
        (*p_lib_its_security_context)->public_key_c = (uint8_t*)malloc(len);
        if (EC_POINT_point2oct((*p_lib_its_security_context)->ec_group, ec_point, POINT_CONVERSION_COMPRESSED, (*p_lib_its_security_context)->public_key_c, len, (*p_lib_its_security_context)->bn_ctx) != 0) {
            (*p_lib_its_security_context)->compressed_mode = ((*vv & 0x01) == 0x00) ? compressed_y_0 : compressed_y_1;
            memmove((void*)(*p_lib_its_security_context)->public_key_c, (const void*)(1 + (*p_lib_its_security_context)->public_key_c), len - 1);
        }
    }
    free(vv);
    EC_POINT_free(ec_point);

    (*p_lib_its_security_context)->private_key = (uint8_t*)malloc((*p_lib_its_security_context)->key_length);
    memcpy((void*)(*p_lib_its_security_context)->private_key, (const void*)p_private_key, (*p_lib_its_security_context)->key_length);

    return 0;
}

int32_t initialize_with_public_key(
    const ecc_elliptic_curves_t p_elliptic_curve,
    const uint8_t* p_public_key,
    const ecc_compressed_mode_t p_compressed_mode,
    lib_its_security_context_t** p_lib_its_security_context
) {
    // Sanity checks
    if ((p_lib_its_security_context == NULL) || (p_public_key == NULL)) {
        return -1;
    }

    if (initialize(p_elliptic_curve, p_lib_its_security_context) == -1) {
        return -1;
    }
    EC_KEY_set_conv_form((*p_lib_its_security_context)->ec_key, POINT_CONVERSION_UNCOMPRESSED);

    // Set public key
    BIGNUM* compressed_key = BN_new();
    BN_bin2bn(p_public_key, (*p_lib_its_security_context)->key_length, compressed_key);
    EC_POINT* ec_point = EC_POINT_new((*p_lib_its_security_context)->ec_group);
    int32_t result = 0;
    switch (p_elliptic_curve) {
    case nist_p_256: // Use primary
      // No break;
    case brainpool_p_256_r1:
        // No break;
    case brainpool_p_384_r1:
        result = EC_POINT_set_compressed_coordinates_GFp((*p_lib_its_security_context)->ec_group, ec_point, compressed_key, (p_compressed_mode == compressed_y_1) ? 1 : 0, (*p_lib_its_security_context)->bn_ctx); // Use primary elliptic curve
        break;
    default: // Use Binary
        result = EC_POINT_set_compressed_coordinates_GF2m((*p_lib_its_security_context)->ec_group, ec_point, compressed_key, (p_compressed_mode == compressed_y_1) ? 1 : 0, (*p_lib_its_security_context)->bn_ctx);
    } // End of 'switch' statement
    BN_clear_free(compressed_key);

    if (result == 0) {
        EC_POINT_free(ec_point);
        return -1;
    }
    else if (EC_POINT_is_on_curve((*p_lib_its_security_context)->ec_group, ec_point, (*p_lib_its_security_context)->bn_ctx) == 0) {
        EC_POINT_free(ec_point);
        return -1;
    }

    // Set public keys
    BIGNUM* xy = BN_new();
    EC_POINT_point2bn((*p_lib_its_security_context)->ec_group, ec_point, POINT_CONVERSION_UNCOMPRESSED, xy, (*p_lib_its_security_context)->bn_ctx);
    if (BN_num_bytes(xy) == 0) {
        EC_POINT_free(ec_point);
        BN_clear_free(xy);
        return -1;
    }
    EC_KEY_set_public_key((*p_lib_its_security_context)->ec_key, ec_point);
    // Generate X, Y coordinates
    int32_t v_length = BN_num_bytes(xy);
    uint8_t* vv = (uint8_t*)malloc(v_length);
    BN_bn2bin(xy, vv);
    BN_clear_free(xy);
    if ((v_length % 2) != 0) { // TODO Check alse xy[0] == 0x04
      // Remove first byte
      //    v_length -= 1;
      //    memcpy((void*)vv, (const void*)(vv + 1), v_length - 1);
        uint8_t* v = (uint8_t*)malloc(v_length - 1);
        memcpy((void*)v, (const void*)(vv + 1), v_length - 1);
        free(vv);
        vv = v;
        v_length -= 1;
    }
    const int l = v_length / 2;
    (*p_lib_its_security_context)->public_key_x = (uint8_t*)malloc(l);
    memcpy((void*)(*p_lib_its_security_context)->public_key_x, (const void*)vv, l);
    (*p_lib_its_security_context)->public_key_y = (uint8_t*)malloc(l);
    memcpy((void*)(*p_lib_its_security_context)->public_key_y, (const void*)(vv + l), l);
    (*p_lib_its_security_context)->public_key_c = (uint8_t*)malloc(l);
    memcpy((void*)(*p_lib_its_security_context)->public_key_c, (const void*)p_public_key, (*p_lib_its_security_context)->key_length);
    (*p_lib_its_security_context)->compressed_mode = p_compressed_mode;
    free(vv);
    EC_POINT_free(ec_point);

    return 0;
}

int32_t uninitialize(lib_its_security_context_t** p_lib_its_security_context) {
    // Sanity checks
    if ((p_lib_its_security_context == NULL) || (*p_lib_its_security_context == NULL)) {
        return -1;
    }

    if ((*p_lib_its_security_context)->private_key != NULL) free((*p_lib_its_security_context)->private_key);
    if ((*p_lib_its_security_context)->public_key_x != NULL) free((*p_lib_its_security_context)->public_key_x);
    if ((*p_lib_its_security_context)->public_key_y != NULL) free((*p_lib_its_security_context)->public_key_y);
    if ((*p_lib_its_security_context)->public_key_c != NULL) free((*p_lib_its_security_context)->public_key_c);
    if ((*p_lib_its_security_context)->secret_key != NULL) free((*p_lib_its_security_context)->secret_key);
    if ((*p_lib_its_security_context)->sym_key != NULL) free((*p_lib_its_security_context)->sym_key);
    if ((*p_lib_its_security_context)->enc_sym_key != NULL) free((*p_lib_its_security_context)->enc_sym_key);
    if ((*p_lib_its_security_context)->tag != NULL) free((*p_lib_its_security_context)->tag);
    if ((*p_lib_its_security_context)->nonce != NULL) free((*p_lib_its_security_context)->nonce);

    if ((*p_lib_its_security_context)->ec_key != NULL) {
        EC_KEY_free((*p_lib_its_security_context)->ec_key);
    }
    // Not required to free ec_group, it was a reference in ec_key
    if ((*p_lib_its_security_context)->bn_ctx != NULL) {
        BN_CTX_free((*p_lib_its_security_context)->bn_ctx);
    }

    free(*p_lib_its_security_context);
    *p_lib_its_security_context = NULL;

    return 0;
}

int32_t hash_with_sha256(
    const uint8_t* p_to_be_hashed_data,
    const size_t p_to_be_hashed_data_length,
    uint8_t** p_hashed_data
) {
    static uint8_t sha256_empty_string[] = { 0xe3, 0xb0, 0xc4, 0x42, 0x98, 0xfc, 0x1c, 0x14, 0x9a, 0xfb, 0xf4, 0xc8, 0x99, 0x6f, 0xb9, 0x24, 0x27, 0xae, 0x41, 0xe4, 0x64, 0x9b, 0x93, 0x4c, 0xa4, 0x95, 0x99, 0x1b, 0x78, 0x52, 0xb8, 0x55 }; //! SHA-256 of an empty string

    // Sanity checks
    if (p_hashed_data == NULL) {
        return -1;
    }

    *p_hashed_data = (uint8_t*)malloc(32);
    if ((p_to_be_hashed_data == NULL) || (p_to_be_hashed_data_length == 0)) {
        // SHA-256 of an empty value
        memcpy((void*)*p_hashed_data, (const void*)sha256_empty_string, 32);
    }
    else {
        SHA256_CTX ctx;
        SHA256_Init(&ctx);
        SHA256_Update(&ctx, p_to_be_hashed_data, p_to_be_hashed_data_length);
        SHA256_Final(*p_hashed_data, &ctx);
    }

    return 0;
}

int32_t hash_with_sha384(
    const uint8_t* p_to_be_hashed_data,
    const size_t p_to_be_hashed_data_length,
    uint8_t** p_hashed_data
) {
    static unsigned char sha384_empty_string[] = { 0x38, 0xb0, 0x60, 0xa7, 0x51, 0xac, 0x96, 0x38, 0x4c, 0xd9, 0x32, 0x7e, 0xb1, 0xb1, 0xe3, 0x6a, 0x21, 0xfd, 0xb7, 0x11, 0x14, 0xbe, 0x07, 0x43, 0x4c, 0x0c, 0xc7, 0xbf, 0x63, 0xf6, 0xe1, 0xda, 0x27, 0x4e, 0xde, 0xbf, 0xe7, 0x6f, 0x65, 0xfb, 0xd5, 0x1a, 0xd2, 0xf1, 0x48, 0x98, 0xb9, 0x5b }; //! SHA-384 of an empty string

    // Sanity checks
    if (p_hashed_data == NULL) {
        return -1;
    }

    *p_hashed_data = (uint8_t*)malloc(48);
    if ((p_to_be_hashed_data == NULL) || (p_to_be_hashed_data_length == 0)) {
        // SHA-256 of an empty value
        memcpy((void*)*p_hashed_data, (const void*)sha384_empty_string, 48);
    }
    else {
        SHA512_CTX ctx;
        SHA384_Init(&ctx);
        SHA384_Update(&ctx, p_to_be_hashed_data, p_to_be_hashed_data_length);
        SHA384_Final(*p_hashed_data, &ctx);
    }

    return 0;
}

int32_t hmac_sha256(
    const uint8_t* p_secret_key,
    const size_t p_secret_key_length,
    const uint8_t* p_message,
    const size_t p_message_length,
    uint8_t** p_hmac
) {
    /* Sanity checks */
    if ((p_secret_key == NULL) || (p_secret_key_length == 0) || (p_message == NULL) || (p_message_length == 0) || (p_hmac == NULL)) {
        return -1;
    }

    show_hex((const int8_t*)">>> hmac_sha256: p_secret_key", p_secret_key, p_secret_key_length);
    show_hex((const int8_t*)">>> hmac_sha256: p_message", p_message, p_message_length);

    uint32_t length = 64;
    uint8_t* hmac = (uint8_t*)malloc(length);
    HMAC_CTX* ctx = HMAC_CTX_new();
    HMAC_Init_ex(ctx, (const void*)p_secret_key, (long unsigned int)p_secret_key_length, EVP_sha256(), NULL);
    /* Compute the hash value */
    HMAC_Update(ctx, p_message, p_message_length);
    HMAC_Final(ctx, hmac, &length);
    HMAC_CTX_free(ctx);
    /* Resize the hmac */
    *p_hmac = (uint8_t*)malloc(16);
    memcpy((void*)*p_hmac, (const void*)hmac, 16);
    free(hmac);
    show_hex((const int8_t*)"<<< hmac_sha256: p_message", *p_hmac, 16);

    return 0;
}

int32_t prepare_data_to_be_verify(
    const lib_its_security_context_t* p_lib_its_security_context,
    const uint8_t* p_data,
    const size_t p_data_length,
    const uint8_t* p_certificate_issuer,
    uint8_t** p_hashed_data
) {
    show_hex((const int8_t*)">>> prepare_data_to_be_verify: p_data", p_data, p_data_length);

    // Calculate the SHA of the hashed data for signing: Hash ( Hash (Data input) || Hash (Signer identifier input) )
    uint8_t* hashed_data1; // Hash (Data input)
    int32_t result;
    if (p_lib_its_security_context->elliptic_curve == brainpool_p_384_r1) {
        result = hash_with_sha384(p_data, p_data_length, &hashed_data1);
    }
    else {
        result = hash_with_sha256(p_data, p_data_length, &hashed_data1); // Hash (Data input)
    }
    if (result == -1) {
        return -1;
    }
    show_hex((const int8_t*)"prepare_data_to_be_verify: hashed_data1", hashed_data1, p_lib_its_security_context->key_length);
    // Check if issuer is '00...00'O vector
    bool foundNonZero = false;
    for (int i = 0; i < 32; i++) {
        if (*(p_certificate_issuer + i) != 0x00) {
            foundNonZero = true;
            break;
        }
    }
    fprintf(stderr, "prepare_data_to_be_verify: foundNonZero=%d\n", foundNonZero);
    uint8_t* hashed_data2; // Hash (Signer identifier input)
    if (foundNonZero) {
        // p_certificate_issuer is already the hashed id
        hashed_data2 = (uint8_t*)malloc(p_lib_its_security_context->key_length);
        memcpy((void*)hashed_data2, (const void*)p_certificate_issuer, p_lib_its_security_context->key_length);
        result = 0;
    }
    else { // Use hash of empty string
        if (p_lib_its_security_context->elliptic_curve == brainpool_p_384_r1) {
            result = hash_with_sha384(NULL, 0, &hashed_data2);
        }
        else {
            result = hash_with_sha256(NULL, 0, &hashed_data2); // Hash of empty string
        }
    }
    if (result == -1) {
        free(hashed_data1);
        free(hashed_data2);
        return -1;
    }
    show_hex((const int8_t*)"prepare_data_to_be_verify: hashed_data2", hashed_data2, p_lib_its_security_context->key_length);
    uint8_t* hash_data_buffer = (uint8_t*)malloc(2 * p_lib_its_security_context->key_length); // Hash (Data input) || Hash (Signer identifier input)
    memcpy((void*)hash_data_buffer, (const void*)hashed_data1, p_lib_its_security_context->key_length);
    memcpy((void*)(hash_data_buffer + p_lib_its_security_context->key_length), (const void*)hashed_data2, p_lib_its_security_context->key_length);
    show_hex((const int8_t*)"prepare_data_to_be_verify: hash_data_buffer", hash_data_buffer, 2 * p_lib_its_security_context->key_length);
    if (p_lib_its_security_context->elliptic_curve == brainpool_p_384_r1) {
        result = hash_with_sha384(hash_data_buffer, 2 * p_lib_its_security_context->key_length, p_hashed_data); // Hash ( Hash (Data input) || Hash (Signer identifier input) )
    }
    else {
        result = hash_with_sha256(hash_data_buffer, 2 * p_lib_its_security_context->key_length, p_hashed_data); // Hash ( Hash (Data input) || Hash (Signer identifier input) )
    }
    show_hex((const int8_t*)"prepare_data_to_be_verify: p_hashed_data", *p_hashed_data, p_lib_its_security_context->key_length);
    free(hashed_data1);
    free(hashed_data2);
    free(hash_data_buffer);

    return 0;
}

int32_t generic_signature(
    lib_its_security_context_t* p_lib_its_security_context,
    const uint8_t* p_to_be_signed_secured_message,
    const size_t p_to_be_signed_secured_message_length,
    const uint8_t* p_certificate_issuer,
    const uint8_t* p_private_key,
    uint8_t** p_signature
) {
    show_hex((const int8_t*)">>> generic_signature: p_to_be_signed_secured_message", p_to_be_signed_secured_message, p_to_be_signed_secured_message_length);
    show_hex((const int8_t*)">>> generic_signature: p_certificate_issuer", p_certificate_issuer, p_lib_its_security_context->key_length);
    show_hex((const int8_t*)">>> generic_signature: p_private_key", p_private_key, p_lib_its_security_context->key_length);

    uint8_t* hashed_data;
    if (prepare_data_to_be_verify(p_lib_its_security_context, p_to_be_signed_secured_message, p_to_be_signed_secured_message_length, p_certificate_issuer, &hashed_data) == -1) {
        return -1;
    }

    // Calculate the signature
    uint8_t* r_sig;
    uint8_t* s_sig;
    size_t sig_length;
    if (sign(p_lib_its_security_context, hashed_data, p_lib_its_security_context->key_length, &r_sig, &s_sig, &sig_length) == -1) {
        free(hashed_data);
        return -1;
    }
    free(hashed_data);
    // Copy the signature
    show_hex((const int8_t*)"generic_signature: r_sig", r_sig, p_lib_its_security_context->key_length);
    show_hex((const int8_t*)"generic_signature: s_sig", s_sig, p_lib_its_security_context->key_length);
    *p_signature = (uint8_t*)malloc(2 * p_lib_its_security_context->key_length); // r_sig || s_sig
    memcpy((void*)*p_signature, (const void*)r_sig, p_lib_its_security_context->key_length);
    memcpy((void*)(*p_signature + p_lib_its_security_context->key_length), (const void*)s_sig, p_lib_its_security_context->key_length);

    free(r_sig);
    free(s_sig);

    return 0;
}

int32_t generic_verify(
    lib_its_security_context_t* p_lib_its_security_context,
    const uint8_t* p_to_be_verified_data,
    const size_t p_to_be_verified_data_length,
    const uint8_t* p_certificate_issuer,
    const uint8_t* p_signature,
    const uint8_t* p_ecdsa_nistp256_publicKey_compressed,
    const ecc_compressed_mode_t p_compressed_mode
) {
    uint8_t* hashed_data;
    if (prepare_data_to_be_verify(p_lib_its_security_context, p_to_be_verified_data, p_to_be_verified_data_length, p_certificate_issuer, &hashed_data) == -1) {
        return -1;
    }
    show_hex((const int8_t*)"generic_verify: p_data", hashed_data, p_lib_its_security_context->key_length);

    uint8_t* sig_r = (uint8_t*)malloc(p_lib_its_security_context->key_length);
    memcpy((void*)sig_r, (const void*)p_signature, p_lib_its_security_context->key_length);
    show_hex((const int8_t*)"generic_verify: sig_r", sig_r, p_lib_its_security_context->key_length);
    uint8_t* sig_s = (uint8_t*)malloc(p_lib_its_security_context->key_length);
    memcpy((void*)sig_s, (const void*)(p_signature + p_lib_its_security_context->key_length), p_lib_its_security_context->key_length);
    show_hex((const int8_t*)"generic_verify: sig_s", sig_s, p_lib_its_security_context->key_length);
    if (sign_verify(p_lib_its_security_context, hashed_data, p_lib_its_security_context->key_length, sig_r, sig_s, p_lib_its_security_context->key_length) == -1) {
        free(sig_r);
        free(sig_s);
        free(hashed_data);
        return -1;
    }
    free(sig_r);
    free(sig_s);
    free(hashed_data);

    return 0;
}

int32_t sign_with_ecdsa_nistp256_with_sha256(
    lib_its_security_context_t* p_lib_its_security_context,  // FIXME To be removed or remove p_private_key