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
                                             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
                                             ) {
  // Sanity checks
  if ((p_lib_its_security_context == NULL) || (p_to_be_signed_secured_message == NULL) || (p_private_key == NULL)) {
    return -1;
  }