security_services.cc

#include <chrono>
#include <cmath>

#include "security_services.hh"

using namespace std; // Required for isnan()
#include "etsi_ts103097_tobesigned_data_codec.hh"
#include "etsi_ts103097_data_codec.hh"
#include "etsi_ts103097_certificate_codec.hh"

#include "sha256.hh"
#include "sha384.hh"
#include "security_ecc.hh"

#include "params.hh"

#include "loggers.hh"

#include "converter.hh"

security_services * security_services::instance = nullptr;

security_services::security_services() : _setup_done{false}, _ec_keys_enc(nullptr), _security_cache(new security_cache), _security_db(nullptr), _last_generation_time(0), _unknown_certificate(), _latitude(0), _longitude(0), _elevation(0) {
  loggers::get_instance().log(">>> security_services::security_services");
} // End of ctor

int security_services::setup(params& p_params) { // FIXME Rename this method
  loggers::get_instance().log(">>> security_services::setup");
  _params = p_params;
  _params.log();

  if (_setup_done) {
    loggers::get_instance().warning("security_services::setup: Already done");
    return 0;
  }
  
  // Build the certificate caching
  try {
    _security_db.reset(new security_db(_params[params::sec_db_path]));
    if (_security_db.get() == nullptr) { // Memory allocation issue
      loggers::get_instance().warning("security_services::setup: _security_db pointer is NULL");
      return -1;
    }
    _setup_done = true;
  } catch(...) {
    loggers::get_instance().error("security_services::setup: Filesystem access error, terminate test suite on TTCN-3 error. Please check user name and paths in the test suite configuration file.");
    return -1;
  }
  
  // Initialise encryption mechanism
  if (_params[params::encrypted_mode].compare("1") == 0) {
    params::const_iterator it = _params.find(params::cypher);
    if (it == _params.cend()) {
      _ec_keys_enc.reset(new security_ecc(ec_elliptic_curves::nist_p_256));
      _params.insert(std::pair<std::string, std::string>(params::cypher, std::string("NISTP-256")));
      p_params.insert(std::pair<std::string, std::string>(params::cypher, std::string("NISTP-256")));
    } else if (it->second.compare("NISTP-256")) {
      _ec_keys_enc.reset(new security_ecc(ec_elliptic_curves::nist_p_256));
    } else if (it->second.compare("BP-256")) {
      _ec_keys_enc.reset(new security_ecc(ec_elliptic_curves::brainpool_p_256_r1));
    } else {
      loggers::get_instance().warning("security_services::setup: Failed to encode ToBeSignedData");
      return -1;
    }
  }
  return 0;
}

int security_services::store_certificate(const CHARSTRING& p_cert_id, const OCTETSTRING& p_cert, const OCTETSTRING& p_private_key, const OCTETSTRING& p_public_key_x, const OCTETSTRING& p_public_key_y, const OCTETSTRING& p_public_comp_key, const INTEGER& p_public_comp_key_mode, const OCTETSTRING& p_hashid8, const OCTETSTRING& p_issuer, const OCTETSTRING& p_private_enc_key, const OCTETSTRING& p_public_enc_key_x, const OCTETSTRING& p_public_enc_key_y, const OCTETSTRING& p_public_enc_compressed_key, const INTEGER& p_public_enc_key_compressed_mode) {
  loggers::get_instance().log_msg(">>> security_services::store_certificate: ", p_cert_id);

  // Sanity checks
  if (_security_db.get() == nullptr) { // Setup not called
    loggers::get_instance().warning("security_services::store_certificate: Not initialised");
    return -1;
  }
  return _security_db.get()->store_certificate(p_cert_id, p_cert, p_private_key, p_public_key_x, p_public_key_y, p_public_comp_key, p_public_comp_key_mode, p_hashid8, p_issuer, p_private_enc_key, p_public_enc_key_x, p_public_enc_key_y, p_public_enc_compressed_key, p_public_enc_key_compressed_mode);
}

int security_services::verify_and_extract_gn_payload(const OCTETSTRING& p_secured_gn_payload, const bool p_verify, IEEE1609dot2::Ieee1609Dot2Data& p_ieee_1609dot2_data, OCTETSTRING& p_unsecured_gn_payload, params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::verify_and_extract_gn_payload: ", p_secured_gn_payload);

  // Sanity checks
  if (p_secured_gn_payload.lengthof() == 0) {
    return -1;
  }
  
  // Decode the secured message (OER encoding)
  etsi_ts103097_data_codec codec;
  codec.decode(p_secured_gn_payload, p_ieee_1609dot2_data, &p_params);
  // Sanity checks
  if (!p_ieee_1609dot2_data.is_bound()) {
    loggers::get_instance().warning("security_services::verify_and_extract_gn_payload: Unbound value, discard it");
    return -1;
  }
  if (p_verify && ((unsigned int)(int)p_ieee_1609dot2_data.protocolVersion() != security_services::ProtocolVersion)) {
    loggers::get_instance().warning("security_services::verify_and_extract_gn_payload: Wrong version protocol, discard it");
    return -1;
  }

  return process_ieee_1609_dot2_content(p_ieee_1609dot2_data.content(), p_verify, p_unsecured_gn_payload, p_params);
} // End of method verify_and_extract_gn_payload

int security_services::process_ieee_1609_dot2_content(const IEEE1609dot2::Ieee1609Dot2Content& p_ieee_1609_dot2_content, const bool p_verify, OCTETSTRING& p_unsecured_payload, params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::process_ieee_1609_dot2_content: ", p_ieee_1609_dot2_content);
  
  if (p_ieee_1609_dot2_content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_unsecuredData)) { // Unsecured packet, End of recursivity
    p_unsecured_payload = p_ieee_1609_dot2_content.unsecuredData();
  } else if (p_ieee_1609_dot2_content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_signedData)) {
    const IEEE1609dot2::SignedData& signedData = p_ieee_1609_dot2_content.signedData();
    if (process_ieee_1609_dot2_signed_data(signedData, p_verify, p_unsecured_payload, p_params) != 0) {
      if (p_verify) {
        return -1;
      }
    }
  } else if (p_ieee_1609_dot2_content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_encryptedData)) {
    const IEEE1609dot2::EncryptedData& encrypted_data = p_ieee_1609_dot2_content.encryptedData();
    OCTETSTRING signed_payload;
    if (process_ieee_1609_dot2_encrypted_data(encrypted_data, p_verify, signed_payload, p_params) != 0) {
      return -1;
    }
    loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_content: Decrypted payload: ", signed_payload);
    IEEE1609dot2::Ieee1609Dot2Data ieee_1609dot2_data; // TODO Check if it could be reused
    if (verify_and_extract_gn_payload(signed_payload, p_verify, ieee_1609dot2_data, p_unsecured_payload, p_params) != 0) {
      if (p_verify) {
        return -1;
      }
    }
  } else if (p_ieee_1609_dot2_content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_signedCertificateRequest)) {
    // Reset certificate timer
    loggers::get_instance().log("security_services::process_ieee_1609_dot2_content: Set Certificate re-transmission flag and reset timer");
    _last_generation_time = 0;
    return 0;
  } else { // Shall never be reached
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_content: Undefined IEEE 1609.2 Content, discard it");
    if (p_verify) {
      return -1;
    }
  }
  
  loggers::get_instance().log_msg("<<< security_services::process_ieee_1609_dot2_content: ", p_unsecured_payload);
  return 0;
} // End of method process_ieee_1609_dot2_content

int security_services::process_ieee_1609_dot2_signed_data(const IEEE1609dot2::SignedData& p_signed_data, const bool p_verify, OCTETSTRING& p_unsecured_payload, params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::process_ieee_1609_dot2_signed_data: ", p_signed_data);

  // Check the headerInfo content
  const IEEE1609dot2::HeaderInfo& header_info = p_signed_data.tbsData().headerInfo();
  p_params[params::its_aid] = std::to_string(header_info.psid().get_long_long_val());
  if (!header_info.generationTime().is_present()) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: HeaderInfo::GenerationTime field is missing");
    if (p_verify) {
      return -1;
    }
  } else {
    const OPTIONAL<INTEGER>& v = dynamic_cast<const OPTIONAL<INTEGER>& >(header_info.generationTime());
    unsigned long long gt = ((INTEGER&)(*v.get_opt_value())).get_long_long_val() * 1000 - 1072911600000L;
    // Get current time timestamp
    unsigned long long ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count() - 1072911600000L;  // TODO Add method such as its_tme() & its_time_mod() beacuse it is used also in LibItsCommon_externals
    loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: generation time check %ld / %ld", header_info.generationTime(), ms);
    if (abs((double)gt - (double)ms) >= 5.0) { // TODO Use a params for generation_time_epsilon
      loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Invalid generation time, discard it");
      if (p_verify) {
        return -1;
      }
    }
  }

  // Check encryption keys if present
  if (header_info.encryptionKey().is_present()) {
    // TODO
  }

  // Check request certificate
  if (header_info.inlineP2pcdRequest().is_present()) {
      loggers::get_instance().error("security_services::process_ieee_1609_dot2_signed_data: inlineP2pcdRequest not supported yet");
      // TODO
  }

  // Check requested certificate
  if (header_info.requestedCertificate().is_present()) {
      loggers::get_instance().error("security_services::process_ieee_1609_dot2_signed_data: requestedCertificate not supported yet");
      // TODO
  }

  // Check and extract unsecured payload
  if (p_signed_data.tbsData().payload().data().is_present()) {
    // Check protocol version
    const OPTIONAL<IEEE1609dot2::Ieee1609Dot2Data>& v = dynamic_cast<const OPTIONAL<IEEE1609dot2::Ieee1609Dot2Data>& >(p_signed_data.tbsData().payload().data());
    loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: SignedDataPayload.data = ", v);
    const IEEE1609dot2::Ieee1609Dot2Data& ieee_1609dot2_data = static_cast<const IEEE1609dot2::Ieee1609Dot2Data&>(*v.get_opt_value());
    if (p_verify && ((unsigned int)(int)ieee_1609dot2_data.protocolVersion() != security_services::ProtocolVersion)) {
      loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Wrong version protocol, discard it");
      if (p_verify) {
        return -1;
      }
    }
    if (process_ieee_1609_dot2_content(ieee_1609dot2_data.content(), p_verify, p_unsecured_payload, p_params) != 0) {
      loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Failed to process SignedData, discard it");
      if (p_verify) {
        return -1;
      }
    }
  } else if (p_signed_data.tbsData().payload().extDataHash().is_present()) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Unsupported extDataHash, discard it");
    if (p_verify) {
      return -1;
    }
  } else { // Shall not be reached
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Unsupported SignedDataPayload, discard it");
    return -1;
  }

  // Encode the ToBeSignedData
  etsi_ts103097_tobesigned_data_codec tbs_data_codec;
  OCTETSTRING os;
  tbs_data_codec.encode(p_signed_data.tbsData(), os);
  if (os.lengthof() == 0) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Failed to encode ToBeSignedData");
    return -1;
  }
  loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: encoded tbs_data = ", os);
  // Calculate the hash according to the hashId
  OCTETSTRING hashed_data;
  if (p_signed_data.hashId() == IEEE1609dot2BaseTypes::HashAlgorithm::sha256) {
    hash_sha256(os, hashed_data);
  } else {
    hash_sha384(os, hashed_data);
  }
  loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: hashed_data = ", hashed_data);
  // Retrieve certificate identifier
  loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: signer = ", p_signed_data.signer());
  std::string certificate_id;
  int result = -1;
  if (p_signed_data.signer().ischosen(IEEE1609dot2::SignerIdentifier::ALT_digest)) {
    // Retrieve the certificate identifier from digest
    loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: Retrieve the certificate identifier from digest");
    result = _security_db.get()->get_certificate_id(p_signed_data.signer().digest(), certificate_id);
    if (result == -1) {
      // Check in the cache
      if (_security_cache.get()->get_certificate_id(p_signed_data.signer().digest(), certificate_id) == -1) {
        // Unknown certificate, request it
        loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: Unknown certificate, request it");
        const OCTETSTRING& os = p_signed_data.signer().digest();
        _unknown_certificate.resize(3);
        const unsigned char* p = static_cast<const unsigned char*>(os) + os.lengthof() - 3;
        for (int i = 0; i < 3; i++) {
          _unknown_certificate[i] = *(p + i);
        } // End of 'for' statement
        loggers::get_instance().log_to_hexa("security_services::process_ieee_1609_dot2_signed_data: HashedId3: ", _unknown_certificate.data(), _unknown_certificate.size());
      }
      // Reset certificate timer
      loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: Set Certificate re-transmission flag and reset timer");
      _last_generation_time = 0;
    }
  } else if (p_signed_data.signer().ischosen(IEEE1609dot2::SignerIdentifier::ALT_certificate) && (p_signed_data.signer().certificate().size_of() != 0)) {
    // Extract the certificates
    std::vector<std::string> certificate_ids;
    for (int i = 0; i < p_signed_data.signer().certificate().size_of(); i++) {
      IEEE1609dot2::CertificateBase cert = p_signed_data.signer().certificate()[i];
      // Retrieve ssps
      OPTIONAL<IEEE1609dot2BaseTypes::SequenceOfPsidSsp>& v = cert.toBeSigned().appPermissions();
      if (v.is_present()) {
        IEEE1609dot2BaseTypes::SequenceOfPsidSsp psid_ssps = static_cast<const IEEE1609dot2BaseTypes::SequenceOfPsidSsp&>(*v.get_opt_value());
        loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: psid_ssps size: %d", psid_ssps.lengthof());
        for (int i = 0; i < psid_ssps.lengthof(); i++) {
          const IEEE1609dot2BaseTypes::PsidSsp& psid_ssp = psid_ssps[i];
          loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: Processing psid_ssp ", psid_ssp);
          const OPTIONAL<IEEE1609dot2BaseTypes::ServiceSpecificPermissions>& s = psid_ssp.ssp();
          if (s.is_present()) {
            const IEEE1609dot2BaseTypes::ServiceSpecificPermissions& ssp = static_cast<const IEEE1609dot2BaseTypes::ServiceSpecificPermissions>(s);
            loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: Processing ssp ", ssp);
            params::const_iterator it = p_params.find(std::to_string(psid_ssp.psid()));
            if (it == p_params.cend()) {
              OCTETSTRING os;
              if (ssp.ischosen(IEEE1609dot2BaseTypes::ServiceSpecificPermissions::ALT_opaque)) {
                os = ssp.opaque();
              } else {
                os = ssp.bitmapSsp();
              }
              p_params[std::to_string(psid_ssp.psid())] = std::string(static_cast<const char *>(oct2str(os)));
            }
          }
        } // End of 'for' statement
      }

      std::string certificate_id;
      if (extract_and_store_certificate(cert, certificate_id) != 0) {
        loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Failed to store certificate");
        if (p_verify) {
          return -1;
        }
      }
      loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: certificate_id: '%s'", certificate_id.c_str());
      certificate_ids.push_back(certificate_id);
      loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: certificate_ids size: %d", certificate_ids.size());
    } // End of 'for' statement
    certificate_id = certificate_ids[0];
    loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: After extract_and_store_certificate, certificate_id: '%s'", certificate_id.c_str());
  } else {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Unsupported SignerIdentifier");
    return -1;
  }
  loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: certificate id = '%s'", certificate_id.c_str());
  // Verify the signature of the ToBeSignedData
  loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: signature = ", p_signed_data.signature__());
  result = -1;
  if (p_signed_data.signature__().ischosen(IEEE1609dot2BaseTypes::Signature::ALT_ecdsaNistP256Signature)) {
    result = verify_sign_ecdsa_nistp256(hashed_data, p_signed_data.signature__(), certificate_id, p_params);
  } else {
    // TODO
    loggers::get_instance().error("security_services::process_ieee_1609_dot2_signed_data: TODO");
  }
  if (result != 0) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Failed to verify signature");
    return -1;
  }
  
  loggers::get_instance().log_msg("<<< security_services::process_ieee_1609_dot2_signed_data: ", p_unsecured_payload);
  return 0;
} // End of method process_ieee_1609_dot2_signed_data

int security_services::process_ieee_1609_dot2_encrypted_data(const IEEE1609dot2::EncryptedData& p_encrypted_data, const bool p_verify, OCTETSTRING& p_unsecured_payload, params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::process_ieee_1609_dot2_encrypted_data: ", p_encrypted_data);
  
  // 1. Retrieve the RecipientId
  const IEEE1609dot2::RecipientInfo& r = p_encrypted_data.recipients()[0]; // TODO Add multiple support of recipients
  const IEEE1609dot2BaseTypes::EciesP256EncryptedKey* ecies = nullptr; // TODO Use smart pointer
  const OCTETSTRING* recipient_id = nullptr; // TODO Use smart pointer
  if (r.ischosen(IEEE1609dot2::RecipientInfo::ALT_certRecipInfo)) {
    recipient_id = &r.certRecipInfo().recipientId();
    if (r.certRecipInfo().encKey().ischosen(IEEE1609dot2::EncryptedDataEncryptionKey::ALT_eciesNistP256)) {
      ecies = &r.certRecipInfo().encKey().eciesNistP256();
    } else if (r.certRecipInfo().encKey().ischosen(IEEE1609dot2::EncryptedDataEncryptionKey::ALT_eciesBrainpoolP256r1)) {
      ecies = &r.certRecipInfo().encKey().eciesBrainpoolP256r1();
    } else {
      loggers::get_instance().warning("security_services::process_ieee_1609_dot2_encrypted_data: Unsupported encryption algorithm");
      return -1;
    }
  } else {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_encrypted_data: Unsupported RecipientInfo variant");
    return -1;
  }
  if (!ecies->v().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_uncompressedP256)) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_encrypted_data: Unsupported EccP256CurvePoint variant");
    return -1;
  }
  if (!p_encrypted_data.ciphertext().ischosen(IEEE1609dot2::SymmetricCiphertext::ALT_aes128ccm)) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_encrypted_data: Unsupported AES 128 algorithm");
    return -1;
  }
  
  // 2. Retrieve the certificate if present
  std::string certificate_id;
  if (_security_db.get()->get_certificate_id(*recipient_id, certificate_id) == -1) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_encrypted_data: Unknown certificate");
    // TODO Setup request certificate mechanism
    return -1;
  }
  OCTETSTRING p_enc_key;
  if (_security_db.get()->get_private_enc_key(certificate_id, p_enc_key) == -1) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_encrypted_data: Failed to retrieve private encryption key");
    return -1;
  }
  std::vector<unsigned char> private_enc_key(static_cast<const unsigned char*>(p_enc_key), p_enc_key.lengthof() + static_cast<const unsigned char*>(p_enc_key));

  // 3. Generate the shared secret value based on recipient's public ephemeral keys will be required
  security_ecc ec(ec_elliptic_curves::nist_p_256, private_enc_key);
  std::vector<unsigned char> ephemeral_public_key_x(static_cast<const unsigned char*>(ecies->v().uncompressedP256().x()), ecies->v().uncompressedP256().x().lengthof() + static_cast<const unsigned char*>(ecies->v().uncompressedP256().x()));
  std::vector<unsigned char> ephemeral_public_key_y(static_cast<const unsigned char*>(ecies->v().uncompressedP256().y()), ecies->v().uncompressedP256().y().lengthof() + static_cast<const unsigned char*>(ecies->v().uncompressedP256().y()));
  std::vector<unsigned char> enc_sym_key(static_cast<const unsigned char*>(ecies->c()), ecies->c().lengthof() + static_cast<const unsigned char*>(ecies->c()));
  std::vector<unsigned char> nonce(static_cast<const unsigned char*>(p_encrypted_data.ciphertext().aes128ccm().nonce()), p_encrypted_data.ciphertext().aes128ccm().nonce().lengthof() + static_cast<const unsigned char*>(p_encrypted_data.ciphertext().aes128ccm().nonce()));
  std::vector<unsigned char> authentication_vector(static_cast<const unsigned char*>(ecies->t()), ecies->t().lengthof() + static_cast<const unsigned char*>(ecies->t()));
  if (ec.generate_and_derive_ephemeral_key(encryption_algotithm::aes_128_ccm, private_enc_key, ephemeral_public_key_x, ephemeral_public_key_y, enc_sym_key, nonce, authentication_vector) == -1) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_encrypted_data: Failed to generate shared secret");
    return -1;
  }

  // 4. Decrypt the message
  std::vector<unsigned char> enc_message(static_cast<const unsigned char*>(p_encrypted_data.ciphertext().aes128ccm().ccmCiphertext()), p_encrypted_data.ciphertext().aes128ccm().ccmCiphertext().lengthof() - ec.tag().size() + static_cast<const unsigned char*>(p_encrypted_data.ciphertext().aes128ccm().ccmCiphertext()));
  loggers::get_instance().log_to_hexa("security_services::process_ieee_1609_dot2_encrypted_data: enc_message: ", enc_message.data(), enc_message.size());
  std::vector<unsigned char> tag(p_encrypted_data.ciphertext().aes128ccm().ccmCiphertext().lengthof() - ec.tag().size() + static_cast<const unsigned char*>(p_encrypted_data.ciphertext().aes128ccm().ccmCiphertext()), p_encrypted_data.ciphertext().aes128ccm().ccmCiphertext().lengthof() + static_cast<const unsigned char*>(p_encrypted_data.ciphertext().aes128ccm().ccmCiphertext()));
  loggers::get_instance().log_to_hexa("security_services::process_ieee_1609_dot2_encrypted_data: tag: ", tag.data(), tag.size());
  std::vector<unsigned char> message;
  if (ec.decrypt(tag, enc_message, message) == -1) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_encrypted_data: Failed to generate shared secret");
    return -1;
  }
  p_unsecured_payload = OCTETSTRING(message.size(), message.data());
  loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_encrypted_data: ", p_unsecured_payload);
  
  return 0;
} // End of method process_ieee_1609_dot2_encrypted_data

int security_services::secure_gn_payload(const OCTETSTRING& p_unsecured_gn_payload, OCTETSTRING& p_secured_gn_payload, params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::secure_gn_payload: ", p_unsecured_gn_payload);
  p_params.log();

  OCTETSTRING signed_payload;
  if (sign_gn_payload(p_unsecured_gn_payload, signed_payload, p_params) != 0) {
    p_secured_gn_payload = p_unsecured_gn_payload;
    loggers::get_instance().warning("security_services::secure_gn_payload: Failed to signed payload");
    return -1;
  }

  if (_params[params::encrypted_mode].compare("1") == 0) {
    if (encrypt_gn_payload(signed_payload, p_secured_gn_payload, p_params) != 0) {
      p_secured_gn_payload = signed_payload;
      loggers::get_instance().warning("security_services::secure_gn_payload: Failed to encrypt payload");
      return -1;
    }
  } else { // No encryption required
    loggers::get_instance().log("security_services::secure_gn_payload: Encryption mode not set");
    p_secured_gn_payload = signed_payload;
  }

  return 0;
}

int security_services::sign_gn_payload(const OCTETSTRING& p_unsecured_gn_payload, OCTETSTRING& p_signed_gn_payload, params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::sign_gn_payload: ", p_unsecured_gn_payload);

  // Set unsecured data
  IEEE1609dot2::Ieee1609Dot2Content unsecured_data_content;
  unsecured_data_content.unsecuredData() = p_unsecured_gn_payload;
  IEEE1609dot2::Ieee1609Dot2Data unsecured_data(ProtocolVersion, unsecured_data_content);
  // Set hash algorithm
  IEEE1609dot2BaseTypes::HashAlgorithm hashId(IEEE1609dot2BaseTypes::HashAlgorithm::sha256);
  if (p_params[params::hash].compare("SHA-384") == 0) {
    hashId = IEEE1609dot2BaseTypes::HashAlgorithm::sha384;
  }
  // Set SignedDataPayload
  IEEE1609dot2::SignedDataPayload payload;
  payload.data() = unsecured_data;
  payload.extDataHash().set_to_omit();
  IEEE1609dot2::HeaderInfo header_info;
  // Set secured field according to the payload!
  header_info.psid() = converter::get_instance().string_to_int(p_params[params::its_aid]);
  header_info.expiryTime().set_to_omit();
  header_info.generationLocation().set_to_omit();
  header_info.p2pcdLearningRequest().set_to_omit();
  header_info.missingCrlIdentifier().set_to_omit();
  if (_params[params::encrypted_mode].compare("1") == 0) {
    // TODO Set the encrytion key. Not supported yet, need to clarify mechanism, see IEEE Std 1609.2-20XX Clause 6.3.9 HeaderInfo
    header_info.encryptionKey().set_to_omit();
  } else {
    header_info.encryptionKey().set_to_omit();
  }
  params::const_iterator it = p_params.find(params::payload_type);
  if (it != p_params.cend()) {
    loggers::get_instance().log("security_services::sign_gn_payload: Payload type: %s", it->second.c_str());
    if (it->second.compare("1") == 0) { // DENM
      OPTIONAL<IEEE1609dot2BaseTypes::ThreeDLocation> location(IEEE1609dot2BaseTypes::ThreeDLocation(_latitude, _longitude, _elevation));
      loggers::get_instance().log_msg("security_services::sign_gn_payload: generationLocation: ", location);
      header_info.generationLocation() = location;
      loggers::get_instance().log_msg("security_services::sign_gn_payload: generationLocation: ", header_info.generationLocation());
    } else if (it->second.compare("2") == 0) { // CAM
      // Noting to do
    } else {
      // Noting to do
    }
  } else { // Process it as a GeoNetworking payload
    loggers::get_instance().log("security_services::sign_gn_payload: Payload type not set");
    // Noting to do
  }
  unsigned long long ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count() - 1072911600000L;  // TODO Add method such as its_tme() & its_time_mod() beacuse it is used also in LibItsCommon_externals
  INTEGER i;
  i.set_long_long_val((unsigned int)ms);
  header_info.generationTime() = OPTIONAL<INTEGER>(i);
  // Check if a certificate shall be requested
  if (_unknown_certificate.size() == 3) { // HashedId3
    IEEE1609dot2BaseTypes::SequenceOfHashedId3 s;
    s[0] = OCTETSTRING(_unknown_certificate.size(), _unknown_certificate.data());
    header_info.inlineP2pcdRequest() = OPTIONAL<IEEE1609dot2BaseTypes::SequenceOfHashedId3>(s);
    _unknown_certificate.clear();
  } else {
    header_info.inlineP2pcdRequest().set_to_omit();
  }
  header_info.requestedCertificate().set_to_omit();

  IEEE1609dot2::ToBeSignedData tbs_data;
  tbs_data.payload() = payload;
  tbs_data.headerInfo() = header_info;
  loggers::get_instance().log_msg("security_services::sign_gn_payload: tbs_data = ", tbs_data);
  // Sign the ToBeSignedData data structure
  IEEE1609dot2BaseTypes::Signature signature;
  if (sign_tbs_data(tbs_data, hashId, signature, p_params) != 0) {
    loggers::get_instance().warning("security_services::sign_gn_payload: Failed to secure payload");
    return -1;
  }
  IEEE1609dot2::SignerIdentifier signer;
  loggers::get_instance().log("security_services::sign_gn_payload: ms = %d - _last_generation_time = %d - ms - _last_generation_time = %d", (unsigned int)ms, _last_generation_time, (unsigned int)(ms - _last_generation_time));
  std::string certificate_id = p_params[params::certificate];
  loggers::get_instance().log("security_services::sign_gn_payload: certificate_id = %s", certificate_id.c_str());
  if ((unsigned int)(ms - _last_generation_time) >= 1000 * 0.95) { // Need to add certificate
    IEEE1609dot2::CertificateBase cert;
    if (_security_db->get_certificate(certificate_id, cert) != 0) {
      loggers::get_instance().warning("security_services:sign_gn_payload: Failed to secure payload");
      return -1;
    }
    IEEE1609dot2::SequenceOfCertificate sequenceOfCertificate;
    sequenceOfCertificate[0] = cert;
    signer.certificate() = sequenceOfCertificate;
    // Reset send certificate timer
    _last_generation_time = ms;
  } else {
    OCTETSTRING digest;
    if (_security_db->get_hashed_id(certificate_id, digest) != 0) {
      loggers::get_instance().warning("security_services::sign_gn_payload: Failed to secure payload");
      return -1;
    }
    signer.digest() = digest;
  }
  IEEE1609dot2::SignedData signed_data(
                                       hashId,
                                       tbs_data,
                                       signer,
                                       signature
                                       );
  loggers::get_instance().log_msg("security_services::sign_gn_payload: signed_data = ", signed_data);
  IEEE1609dot2::Ieee1609Dot2Content ieee_dot2_content;
  ieee_dot2_content.signedData() = signed_data;
  IEEE1609dot2::Ieee1609Dot2Data ieee_1609dot2_data(
                                                    security_services::ProtocolVersion,
                                                    ieee_dot2_content
                                                    );
  loggers::get_instance().log_msg("security_services::sign_gn_payload: ieee_1609dot2_data = ", ieee_1609dot2_data);
  etsi_ts103097_data_codec codec;
  codec.encode(ieee_1609dot2_data, p_signed_gn_payload);
  if (!p_signed_gn_payload.is_bound()) {
    loggers::get_instance().warning("security_services::sign_gn_payload: Failed to encode Ieee1609Dot2Data");
    return -1;
  }
  
  return 0;
}

int security_services::encrypt_gn_payload(const OCTETSTRING& p_unsecured_gn_payload, OCTETSTRING& p_enc_gn_payload, params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::encrypt_gn_payload: ", p_unsecured_gn_payload);
  
  // Sanity checks
  if (_ec_keys_enc.get() == nullptr) {
    loggers::get_instance().warning("security_services::encrypt_gn_payload: Encryption not initialised");
    return -1;
  }
  params::const_iterator it = p_params.find("peer_certificate");
  if (it == p_params.cend()) {
    loggers::get_instance().warning("security_services::encrypt_gn_payload: Encryption impossible without a peer_certificte indication in parameters");
    return -1;
  }
  std::string certificate_id = it->second;
  
  // 1. Retrieve recipient's public keys
  OCTETSTRING r_public_key_x;
  OCTETSTRING r_public_key_y;
  if (_security_db.get()->get_public_enc_keys(certificate_id, r_public_key_x, r_public_key_y) == -1) {
    loggers::get_instance().warning("security_services::encrypt_gn_payload: Failed to retrieve recipient's public keys");
    // TODO Setup request certificate mechanism
    return -1;
  }
  loggers::get_instance().log_msg("security_services::encrypt_gn_payload: r_public_key_x = ", r_public_key_x);
  loggers::get_instance().log_msg("security_services::encrypt_gn_payload: r_public_key_y = ", r_public_key_y);
  std::vector<unsigned char> recipients_public_key_x(static_cast<const unsigned char *>(r_public_key_x), r_public_key_x.lengthof() + static_cast<const unsigned char *>(r_public_key_x));
  std::vector<unsigned char> recipients_public_key_y(static_cast<const unsigned char *>(r_public_key_y), r_public_key_y.lengthof() + static_cast<const unsigned char *>(r_public_key_y));
  
  // 2. Generate new Private/Public ephemeral keys
  if (_ec_keys_enc.get()->generate() == -1) {
    loggers::get_instance().warning("security_services::encrypt_gn_payload: Failed to generate ephemeral keys");
    return -1;
  }
  
  // 3. Generate and derive shared secret
  if (_ec_keys_enc.get()->generate_and_derive_ephemeral_key(encryption_algotithm::aes_128_ccm, recipients_public_key_x, recipients_public_key_y) == -1) {
    loggers::get_instance().warning("security_services::encrypt_gn_payload: Failed to generate and derive secret key");
    return -1;
  }

  // 4. Buil curve data structure
  OCTETSTRING public_ephemeral_key_x(_ec_keys_enc.get()->public_key_x().size(), _ec_keys_enc.get()->public_key_x().data());
  OCTETSTRING public_ephemeral_key_y(_ec_keys_enc.get()->public_key_y().size(), _ec_keys_enc.get()->public_key_y().data());
  OCTETSTRING encrypt_aes_128_key(_ec_keys_enc.get()->encrypted_symmetric_key().size(), _ec_keys_enc.get()->encrypted_symmetric_key().data());
  OCTETSTRING encrypt_aes_128_tag(_ec_keys_enc.get()->tag().size(), _ec_keys_enc.get()->tag().data());
  IEEE1609dot2BaseTypes::EccP256CurvePoint eccP256CurvePoint;
  eccP256CurvePoint.uncompressedP256().x() = public_ephemeral_key_x;
  eccP256CurvePoint.uncompressedP256().y() = public_ephemeral_key_y;
  IEEE1609dot2BaseTypes::EciesP256EncryptedKey ecies_key(
                                                         eccP256CurvePoint,
                                                         encrypt_aes_128_key,
                                                         encrypt_aes_128_tag
                                                         );
  loggers::get_instance().log_msg("security_services::encrypt_gn_payload: ecies_key = ", ecies_key);
  
  // 5. AES-128 encryption of the data
  std::vector<unsigned char> message(static_cast<const unsigned char*>(p_unsecured_gn_payload), p_unsecured_gn_payload.lengthof() + static_cast<const unsigned char*>(p_unsecured_gn_payload));
  std::vector<unsigned char> enc_message;
  if (_ec_keys_enc.get()->encrypt(encryption_algotithm::aes_128_ccm, _ec_keys_enc.get()->symmetric_encryption_key(), _ec_keys_enc.get()->nonce(), message, enc_message) == -1) {
    loggers::get_instance().warning("fx__encryptWithEciesNistp256WithSha256: Failed to encrypt message");
    return -1;
  }
  OCTETSTRING nonce = OCTETSTRING(_ec_keys_enc.get()->nonce().size(), _ec_keys_enc.get()->nonce().data());
  OCTETSTRING tag = OCTETSTRING(_ec_keys_enc.get()->tag().size(), _ec_keys_enc.get()->tag().data());
  OCTETSTRING enc_payload = OCTETSTRING(enc_message.size(), enc_message.data());
  IEEE1609dot2::AesCcmCiphertext aes_128_ccm(nonce, enc_payload + tag); // Add tag at the end of the ciphered text
  // 6. Build SymmetricCiphertext
  IEEE1609dot2::SymmetricCiphertext cipher_text;
  cipher_text.aes128ccm() = aes_128_ccm;
  loggers::get_instance().log_msg("security_services::encrypt_gn_payload: aes_128_ccm = ", cipher_text);
  
  // 7. Build the recipient_id
  OCTETSTRING recipient_id;
  _security_db.get()->get_hashed_id(certificate_id, recipient_id); // SHA-256 of the certificate which contain the recipient's public keys

  // 8. Build the encryption data
  IEEE1609dot2::EncryptedDataEncryptionKey enc_data_key;
  if (_params[params::cypher].compare("NISTP-256") == 0) {
    enc_data_key.eciesNistP256() = ecies_key;
  } else if (_params[params::cypher].compare("BP-256") == 0) {
    enc_data_key.eciesBrainpoolP256r1() = ecies_key;
  }
  loggers::get_instance().log_msg("security_services::encrypt_gn_payload: enc_data_key = ", enc_data_key);
  
  // 9. Finalise the encryption
  IEEE1609dot2::PKRecipientInfo cert_recipient_info(recipient_id, enc_data_key);
  IEEE1609dot2::RecipientInfo recipient_info;
  recipient_info.certRecipInfo() = cert_recipient_info;
  IEEE1609dot2::SequenceOfRecipientInfo recipients;
  recipients[0] = recipient_info;
  IEEE1609dot2::EncryptedData encrypted_data(recipients, cipher_text);
  loggers::get_instance().log_msg("security_services::encrypt_gn_payload: encrypted_data = ", encrypted_data);
  
  // 10. Encode it
  IEEE1609dot2::Ieee1609Dot2Content ieee_dot2_content;
  ieee_dot2_content.encryptedData() = encrypted_data;
  IEEE1609dot2::Ieee1609Dot2Data ieee_1609dot2_data(
                                                    security_services::ProtocolVersion,
                                                    ieee_dot2_content
                                                    );
  loggers::get_instance().log_msg("security_services::sign_gn_payload: ieee_1609dot2_data = ", ieee_1609dot2_data);
  etsi_ts103097_data_codec codec;
  codec.encode(ieee_1609dot2_data, p_enc_gn_payload);
  if (!p_enc_gn_payload.is_bound()) {
    loggers::get_instance().warning("security_services::sign_gn_payload: Failed to encode Ieee1609Dot2Data");
    return -1;
  }
  loggers::get_instance().log_msg("security_services::sign_gn_payload: Encoded ieee_1609dot2_data = ", p_enc_gn_payload);
  
  return 0;
}

int security_services::sign_tbs_data(const IEEE1609dot2::ToBeSignedData& p_tbs_data, const IEEE1609dot2BaseTypes::HashAlgorithm& p_hashAlgorithm, IEEE1609dot2BaseTypes::Signature& p_signature, params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::sign_tbs_data: ", p_tbs_data);
  
  // Encode the ToBeSignedData
  etsi_ts103097_tobesigned_data_codec tbs_data_codec;
  OCTETSTRING os;
  tbs_data_codec.encode(p_tbs_data, os);
  if (os.lengthof() == 0) {
    loggers::get_instance().warning("security_services::sign_tbs_data: Failed to encode ToBeSignedData");
    return -1;
  }
  loggers::get_instance().log_msg("security_services::sign_tbs_data: encoded tbs_data = ", os);
  // Hash ToBeSignedData
  OCTETSTRING hashed_data;
  if (p_hashAlgorithm == IEEE1609dot2BaseTypes::HashAlgorithm::sha256) {
    hash_sha256(os, hashed_data);
  } else {
    hash_sha384(os, hashed_data);
  }
  loggers::get_instance().log_msg("security_services::sign_tbs_data: encoded hashed_data = ", hashed_data);
  // Sign ToBeSignedData
  int result = -1;
  loggers::get_instance().log("security_services::sign_tbs_data: encoded params::signature = '%s'", p_params[params::signature].c_str());
  loggers::get_instance().log("security_services::sign_tbs_data: encoded params::certificate = '%s'", p_params[params::certificate].c_str());
  if (p_params[params::signature].compare("NISTP-256") == 0) {
    result = sign_ecdsa_nistp256(hashed_data, p_signature, p_params);
  } else if (p_params[params::signature].compare("BP-256") == 0) {
    //result = sign_ecdsa_brainpoolp256(hashed_data, p_signature, p_params);
    loggers::get_instance().error("security_services::sign_tbs_data: TODO");
    result = -1;
  } else if (p_params[params::signature].compare("BP-384") == 0) {
    //result = sign_ecdsa_brainpoolp256(hashed_data, p_signature, p_params);
    loggers::get_instance().error("security_services::sign_tbs_data: TODO");
    result = -1;
  } else  {
    loggers::get_instance().error("security_services::sign_tbs_data: Unsupported signature algorithm");
    result = -1;
  }
  if (result != 0) {
    loggers::get_instance().warning("security_services::sign_tbs_data: Failed to sign payload");
    return -1;
  }
  
  return 0;
}

int security_services::hash_sha256(const OCTETSTRING& p_data, OCTETSTRING& p_hash_data) {
  loggers::get_instance().log_msg(">>> security_services::hash_sha256: ", p_data);
  
  sha256 hash;
  std::vector<unsigned char> tbh(static_cast<const unsigned char *>(p_data), p_data.lengthof() + static_cast<const unsigned char *>(p_data));
  std::vector<unsigned char> hashData;
  hash.generate(tbh, hashData);
  p_hash_data = OCTETSTRING(hashData.size(), hashData.data());

  return 0;
}

int security_services::hash_sha384(const OCTETSTRING& p_data, OCTETSTRING& p_hash_data) {
  loggers::get_instance().log_msg(">>> security_services::hash_sha384: ", p_data);
  
  sha384 hash;
  std::vector<unsigned char> tbh(static_cast<const unsigned char *>(p_data), p_data.lengthof() + static_cast<const unsigned char *>(p_data));
  std::vector<unsigned char> hashData;
  hash.generate(tbh, hashData);
  p_hash_data = OCTETSTRING(hashData.size(), hashData.data());
  
  return 0;
}

int security_services::sign_ecdsa_nistp256(const OCTETSTRING& p_hash, IEEE1609dot2BaseTypes::Signature& p_signature, params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::sign_ecdsa_nistp256: ", p_hash);
  
  std::string certificate_id = p_params[params::certificate];
  loggers::get_instance().log("security_services::sign_ecdsa_nistp256: encoded certificate_id = '%s'", certificate_id.c_str());
  OCTETSTRING pkey;
  if (_security_db->get_private_key(certificate_id, pkey) != 0) {
    loggers::get_instance().warning("security_services::sign_ecdsa_nistp256: Failed to get private key");
    return -1;
  }
  std::vector<unsigned char> private_key(static_cast<const unsigned char *>(pkey), static_cast<const unsigned char *>(pkey) + pkey.lengthof());
  // Hash ( Hash (Data input) || Hash (Signer identifier input) )
  OCTETSTRING hash_cert;
  if (_security_db->get_hash(certificate_id, hash_cert) != 0) {
    loggers::get_instance().warning("security_services::sign_ecdsa_nistp256: Failed to get whole hash certificate");
    return -1;
  }
  loggers::get_instance().log_msg("security_services::sign_ecdsa_nistp256: hash_issuer: ", hash_cert);
  OCTETSTRING os = p_hash + hash_cert; // Hash (Data input) || Hash (Signer identifier input)
  loggers::get_instance().log_msg("security_services::sign_ecdsa_nistp256: hash: ", os);
  OCTETSTRING hashed_data;
  hash_sha256(os, hashed_data); // Hash ( Hash (Data input) || Hash (Signer identifier input) )
  std::vector<unsigned char> tbv(static_cast<const unsigned char *>(hashed_data), static_cast<const unsigned char *>(hashed_data) + hashed_data.lengthof());
  security_ecc k(ec_elliptic_curves::nist_p_256, private_key);
  std::vector<unsigned char> r_sig;
  std::vector<unsigned char> s_sig;
  if (k.sign(tbv, r_sig, s_sig) != 0) {
    loggers::get_instance().warning("security_services::sign_ecdsa_nistp256: Failed to sign payload");
    return -1;
  }
  IEEE1609dot2BaseTypes::EccP256CurvePoint ep;
  ep.x__only() = OCTETSTRING(r_sig.size(), r_sig.data());
  p_signature.ecdsaNistP256Signature() = IEEE1609dot2BaseTypes::EcdsaP256Signature(
                                                                                   ep,
                                                                                   OCTETSTRING(s_sig.size(), s_sig.data())
                                                                                   );
  loggers::get_instance().log_msg("security_services::sign_ecdsa_nistp256: signature = ", p_signature);
  
  return 0;
}

int security_services::verify_sign_ecdsa_nistp256(const OCTETSTRING& p_hash, const IEEE1609dot2BaseTypes::Signature& p_signature, const std::string& p_certificate_id, params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::verify_sign_ecdsa_nistp256: ", p_hash);
  
  OCTETSTRING public_key_x;
  OCTETSTRING public_key_y;
  if (_security_db->get_public_keys(p_certificate_id, public_key_x, public_key_y) != 0) {
    loggers::get_instance().warning("security_services::verify_sign_ecdsa_nistp256: Failed to get public keys");
    return -1;
  }
  
  // Generate the hash to be verified: Hash ( Hash (Data input) || Hash (Signer identifier input) )
  OCTETSTRING issuer;
  if (_security_db->get_hash(p_certificate_id, issuer) != 0) {
    loggers::get_instance().warning("security_services::verify_sign_ecdsa_nistp256: Failed to get hash of the issuer certificate");
    return -1;
  }
  std::vector<unsigned char> hash_issuer(static_cast<const unsigned char*>(issuer), issuer.lengthof() + static_cast<const unsigned char*>(issuer)); // Hash (Signer identifier input)
  loggers::get_instance().log_to_hexa("security_services::verify_sign_ecdsa_nistp256: hash_issuer: ", hash_issuer.data(), hash_issuer.size());
  std::vector<unsigned char> hash_data(static_cast<const unsigned char *>(p_hash), static_cast<const unsigned char *>(p_hash) + p_hash.lengthof());
  hash_data.insert(hash_data.end(), hash_issuer.cbegin(), hash_issuer.cend()); // Hash (Data input) || Hash (Signer identifier input)
  loggers::get_instance().log_to_hexa("security_services::verify_sign_ecdsa_nistp256: hash: ", hash_data.data(), hash_data.size());
  sha256 sha;
  std::vector<unsigned char> hash_to_be_verified;
  sha.generate(hash_data, hash_to_be_verified); // Hash ( Hash (Data input) || Hash (Signer identifier input) )
  loggers::get_instance().log_to_hexa("security_services::verify_sign_ecdsa_nistp256: hash_to_be_verified: ", hash_to_be_verified.data(), hash_to_be_verified.size());

  // Build the signature
  OCTETSTRING os;
  if (p_signature.ecdsaNistP256Signature().rSig().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_x__only)) {
    os = p_signature.ecdsaNistP256Signature().rSig().x__only() + p_signature.ecdsaNistP256Signature().sSig();
  } else if (p_signature.ecdsaNistP256Signature().rSig().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__0)) {
    os = p_signature.ecdsaNistP256Signature().rSig().compressed__y__0() + p_signature.ecdsaNistP256Signature().sSig();
  } else if (p_signature.ecdsaNistP256Signature().rSig().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__1)) {
    os = p_signature.ecdsaNistP256Signature().rSig().compressed__y__1() + p_signature.ecdsaNistP256Signature().sSig();
  } else if (p_signature.ecdsaNistP256Signature().rSig().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_uncompressedP256)) {
    os = p_signature.ecdsaNistP256Signature().rSig().uncompressedP256().x() + p_signature.ecdsaNistP256Signature().rSig().uncompressedP256().y() + p_signature.ecdsaNistP256Signature().sSig();
  } else {
    loggers::get_instance().warning("security_services::verify_sign_ecdsa_nistp256: Invalid curve point");
    return -1;
  }
  std::vector<unsigned char> signature(static_cast<const unsigned char *>(os), static_cast<const unsigned char *>(os) + os.lengthof());
  std::vector<unsigned char> key_x(static_cast<const unsigned char *>(public_key_x), static_cast<const unsigned char *>(public_key_x) + public_key_x.lengthof());
  std::vector<unsigned char> key_y(static_cast<const unsigned char *>(public_key_y), static_cast<const unsigned char *>(public_key_y) + public_key_y.lengthof());
  security_ecc k(ec_elliptic_curves::nist_p_256, key_x, key_y);
  if (k.sign_verif(hash_to_be_verified, signature) == 0) {
    return 0;
  }
  
  return -1;
}

int security_services::extract_verification_keys(const IEEE1609dot2::CertificateBase& p_cert, OCTETSTRING& p_public_key_x, OCTETSTRING& p_public_key_y, OCTETSTRING& p_public_comp_key, INTEGER& p_public_comp_key_mode) {
  loggers::get_instance().log("security_services::extract_verification_keys");
  
  if (p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ischosen(IEEE1609dot2BaseTypes::PublicVerificationKey::ALT_ecdsaNistP256)) {
    if (p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaNistP256().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__0)) {
      p_public_comp_key = p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaNistP256().compressed__y__0();
      std::vector<unsigned char> comp_key(static_cast<const unsigned char *>(p_public_comp_key), p_public_comp_key.lengthof() + static_cast<const unsigned char *>(p_public_comp_key));
      security_ecc ecc(ec_elliptic_curves::nist_p_256, comp_key, ecc_compressed_mode::compressed_y_0);
      p_public_key_x = OCTETSTRING(ecc.public_key_x().size(), ecc.public_key_x().data());
      p_public_key_y = OCTETSTRING(ecc.public_key_y().size(), ecc.public_key_y().data());
      p_public_comp_key_mode = INTEGER(0);
    } else if (p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaNistP256().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__1)) {
      p_public_comp_key = p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaNistP256().compressed__y__1();
      std::vector<unsigned char> comp_key(static_cast<const unsigned char *>(p_public_comp_key), p_public_comp_key.lengthof() + static_cast<const unsigned char *>(p_public_comp_key));
      security_ecc ecc(ec_elliptic_curves::nist_p_256, comp_key, ecc_compressed_mode::compressed_y_1);
      p_public_key_x = OCTETSTRING(ecc.public_key_x().size(), ecc.public_key_x().data());
      p_public_key_y = OCTETSTRING(ecc.public_key_y().size(), ecc.public_key_y().data());
      p_public_comp_key_mode = INTEGER(1);
    } else if (p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaNistP256().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_uncompressedP256)) {
      p_public_key_x = p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaNistP256().uncompressedP256().x();
      p_public_key_y = p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaNistP256().uncompressedP256().y();
    } else {
      loggers::get_instance().error("security_services::extract_verification_keys: Unsupported VerificationKey");
      return -1;
    }
  } else if (p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ischosen(IEEE1609dot2BaseTypes::PublicVerificationKey::ALT_ecdsaBrainpoolP256r1)) {
    if (p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP256r1().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__0)) {
      p_public_comp_key = p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP256r1().compressed__y__0();
      std::vector<unsigned char> comp_key(static_cast<const unsigned char *>(p_public_comp_key), p_public_comp_key.lengthof() + static_cast<const unsigned char *>(p_public_comp_key));
      security_ecc ecc(ec_elliptic_curves::brainpool_p_256_r1, comp_key, ecc_compressed_mode::compressed_y_0);
      p_public_key_x = OCTETSTRING(ecc.public_key_x().size(), ecc.public_key_x().data());
      p_public_key_y = OCTETSTRING(ecc.public_key_y().size(), ecc.public_key_y().data());
      p_public_comp_key_mode = INTEGER(0);
    } else if (p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP256r1().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__1)) {
      p_public_comp_key = p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP256r1().compressed__y__1();
      std::vector<unsigned char> comp_key(static_cast<const unsigned char *>(p_public_comp_key), p_public_comp_key.lengthof() + static_cast<const unsigned char *>(p_public_comp_key));
      security_ecc ecc(ec_elliptic_curves::brainpool_p_256_r1, comp_key, ecc_compressed_mode::compressed_y_1);
      p_public_key_x = OCTETSTRING(ecc.public_key_x().size(), ecc.public_key_x().data());
      p_public_key_y = OCTETSTRING(ecc.public_key_y().size(), ecc.public_key_y().data());
      p_public_comp_key_mode = INTEGER(1);
    } else if (p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP256r1().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_uncompressedP256)) {
      p_public_key_x = p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP256r1().uncompressedP256().x();
      p_public_key_y = p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP256r1().uncompressedP256().y();
    } else if (p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ischosen(IEEE1609dot2BaseTypes::PublicVerificationKey::ALT_ecdsaBrainpoolP384r1)) {
      p_public_comp_key = p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP384r1().compressed__y__0();
      std::vector<unsigned char> comp_key(static_cast<const unsigned char *>(p_public_comp_key), p_public_comp_key.lengthof() + static_cast<const unsigned char *>(p_public_comp_key));
      security_ecc ecc(ec_elliptic_curves::brainpool_p_384_r1, comp_key, ecc_compressed_mode::compressed_y_0);
      p_public_key_x = OCTETSTRING(ecc.public_key_x().size(), ecc.public_key_x().data());
      p_public_key_y = OCTETSTRING(ecc.public_key_y().size(), ecc.public_key_y().data());
      p_public_comp_key_mode = INTEGER(0);
    } else if (p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP384r1().ischosen(IEEE1609dot2BaseTypes::EccP384CurvePoint::ALT_compressed__y__1)) {
      p_public_comp_key = p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP384r1().compressed__y__1();
      std::vector<unsigned char> comp_key(static_cast<const unsigned char *>(p_public_comp_key), p_public_comp_key.lengthof() + static_cast<const unsigned char *>(p_public_comp_key));
      security_ecc ecc(ec_elliptic_curves::brainpool_p_384_r1, comp_key, ecc_compressed_mode::compressed_y_1);
      p_public_key_x = OCTETSTRING(ecc.public_key_x().size(), ecc.public_key_x().data());
      p_public_key_y = OCTETSTRING(ecc.public_key_y().size(), ecc.public_key_y().data());
      p_public_comp_key_mode = INTEGER(1);
    } else if (p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP384r1().ischosen(IEEE1609dot2BaseTypes::EccP384CurvePoint::ALT_uncompressedP384)) {
      p_public_key_x = p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP384r1().uncompressedP384().x();
      p_public_key_y = p_cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP384r1().uncompressedP384().y();
    } else {
      loggers::get_instance().error("security_services::extract_verification_keys: Unsupported VerificationKey");
      return -1;
    }
  } else {
    loggers::get_instance().error("security_services::extract_verification_keys: Unsupported VerificationKey");
    return -1;
  }

  return 0;
}

int security_services::extract_encryption_keys(const IEEE1609dot2::CertificateBase& p_cert, OCTETSTRING& p_public_enc_key_x, OCTETSTRING& p_public_enc_key_y, OCTETSTRING& p_public_enc_comp_key, INTEGER& p_public_enc_comp_key_mode) {
  loggers::get_instance().log("security_services::extract_encryption_keys");
  
  if (p_cert.toBeSigned().encryptionKey().ispresent()) {
    const IEEE1609dot2BaseTypes::PublicEncryptionKey& p = static_cast<const IEEE1609dot2BaseTypes::PublicEncryptionKey&>(p_cert.toBeSigned().encryptionKey());
    if (p.publicKey().ischosen(IEEE1609dot2BaseTypes::BasePublicEncryptionKey::ALT_eciesNistP256)) {
      if (p.publicKey().eciesNistP256().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__0)) {
        p_public_enc_comp_key = p.publicKey().eciesNistP256().compressed__y__0();
        std::vector<unsigned char> comp_key(static_cast<const unsigned char *>(p_public_enc_comp_key), p_public_enc_comp_key.lengthof() + static_cast<const unsigned char *>(p_public_enc_comp_key));
        security_ecc ecc(ec_elliptic_curves::nist_p_256, comp_key, ecc_compressed_mode::compressed_y_0);
        p_public_enc_key_x = OCTETSTRING(ecc.public_key_x().size(), ecc.public_key_x().data());
        p_public_enc_key_y = OCTETSTRING(ecc.public_key_y().size(), ecc.public_key_y().data());
        p_public_enc_comp_key_mode = INTEGER(0);
      } else if (p.publicKey().eciesNistP256().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__1)) {
        const OCTETSTRING& p_public_enc_comp_key = p.publicKey().eciesNistP256().compressed__y__1();
        std::vector<unsigned char> comp_key(static_cast<const unsigned char *>(p_public_enc_comp_key), p_public_enc_comp_key.lengthof() + static_cast<const unsigned char *>(p_public_enc_comp_key));
        security_ecc ecc(ec_elliptic_curves::nist_p_256, comp_key, ecc_compressed_mode::compressed_y_1);
        p_public_enc_key_x = OCTETSTRING(ecc.public_key_x().size(), ecc.public_key_x().data());
        p_public_enc_key_y = OCTETSTRING(ecc.public_key_y().size(), ecc.public_key_y().data());
        p_public_enc_comp_key_mode = INTEGER(1);
      } else if (p.publicKey().eciesNistP256().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_uncompressedP256)) {
        p_public_enc_key_x = p.publicKey().eciesNistP256().uncompressedP256().x();
        p_public_enc_key_y = p.publicKey().eciesNistP256().uncompressedP256().y();
      } else {
        loggers::get_instance().error("security_services::extract_encryption_keys: Unsupported EncryptionKey");
        return -1;
      }
    } else if (p.publicKey().ischosen(IEEE1609dot2BaseTypes::BasePublicEncryptionKey::ALT_eciesBrainpoolP256r1)) {
      if (p.publicKey().eciesBrainpoolP256r1().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__0)) {
        p_public_enc_comp_key = p.publicKey().eciesBrainpoolP256r1().compressed__y__0();
        std::vector<unsigned char> comp_key(static_cast<const unsigned char *>(p_public_enc_comp_key), p_public_enc_comp_key.lengthof() + static_cast<const unsigned char *>(p_public_enc_comp_key));
        security_ecc ecc(ec_elliptic_curves::brainpool_p_256_r1, comp_key, ecc_compressed_mode::compressed_y_0);
        p_public_enc_key_x = OCTETSTRING(ecc.public_key_x().size(), ecc.public_key_x().data());
        p_public_enc_key_y = OCTETSTRING(ecc.public_key_y().size(), ecc.public_key_y().data());
        p_public_enc_comp_key_mode = INTEGER(0);
      } else if (p.publicKey().eciesBrainpoolP256r1().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__1)) {
        p_public_enc_comp_key = p.publicKey().eciesBrainpoolP256r1().compressed__y__1();
        std::vector<unsigned char> comp_key(static_cast<const unsigned char *>(p_public_enc_comp_key), p_public_enc_comp_key.lengthof() + static_cast<const unsigned char *>(p_public_enc_comp_key));
        security_ecc ecc(ec_elliptic_curves::brainpool_p_256_r1, comp_key, ecc_compressed_mode::compressed_y_1);
        p_public_enc_key_x = OCTETSTRING(ecc.public_key_x().size(), ecc.public_key_x().data());
        p_public_enc_key_y = OCTETSTRING(ecc.public_key_y().size(), ecc.public_key_y().data());
        p_public_enc_comp_key_mode = INTEGER(1);
      } else if (p.publicKey().eciesBrainpoolP256r1().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_uncompressedP256)) {
        p_public_enc_key_x = p.publicKey().eciesBrainpoolP256r1().uncompressedP256().x();
        p_public_enc_key_y = p.publicKey().eciesBrainpoolP256r1().uncompressedP256().y();
      } else {
        loggers::get_instance().error("security_services::extract_encryption_keys: Unsupported EncryptionKey");
        return -1;
      }
    } else {
      loggers::get_instance().error("security_services::extract_encryption_keys: Unsupported EncryptionKey");
      return -1;
    }
  } else {
    loggers::get_instance().warning("security_services::extract_encryption_keys: EncryptionKey omitted");
    return 0; // Normal termination
  }

  return 0;
} // End of method extract_encryption_keys

 int security_services::extract_and_store_certificate(const IEEE1609dot2::CertificateBase& p_certificate, std::string& p_certificate_id) {
  loggers::get_instance().log_msg(">>> security_services::extract_and_store_certificate: ", p_certificate);

  // Encode certificate
  etsi_ts103097_certificate_codec codec;
  OCTETSTRING enc_cert;
  codec.encode(p_certificate, enc_cert);
  if (enc_cert.lengthof() == 0) {
    loggers::get_instance().warning("security_services::extract_and_store_certificate: Failed to encode certificate");
    return -1;
  }
  loggers::get_instance().log_msg("security_services::extract_and_store_certificate: Encoded certificate = ", enc_cert);
  int result = -1;
  if (p_certificate.issuer().ischosen(IEEE1609dot2::IssuerIdentifier::ALT_sha256AndDigest)) {
    // Calculate the hash according to the hashId
    OCTETSTRING hash_cert;
    hash_sha256(enc_cert, hash_cert);
    loggers::get_instance().log_msg("security_services::extract_and_store_certificate: hash_cert= ", hash_cert);
    const OCTETSTRING hashed_id8 = substr(hash_cert, hash_cert.lengthof() - 8, 8);
    // Retrieve the certificate identifier from digest
    loggers::get_instance().log_msg("security_services::extract_and_store_certificate: Retrieve the certificate identifier from digest: ", hashed_id8);
    result = _security_db.get()->get_certificate_id(hashed_id8, p_certificate_id);
    if (result == -1) { // Not found in current DB
      if (_security_cache.get()->get_certificate_id(hashed_id8, p_certificate_id) == -1) { // Not found in TS cache
        loggers::get_instance().log_msg("security_services::extract_and_store_certificate: Store new certificate in cache: ", p_certificate);
        const std::vector<unsigned char> v(static_cast<const unsigned char*>(hashed_id8), static_cast<const unsigned char*>(hashed_id8) + hashed_id8.lengthof());
        p_certificate_id = converter::get_instance().bytes_to_hexa(v);
        // Add it into the cache
        OCTETSTRING public_key_x, public_key_y, public_comp_key;
        INTEGER public_comp_key_mode;
        if (extract_verification_keys(p_certificate, public_key_x, public_key_y, public_comp_key, public_comp_key_mode) == -1) {
          loggers::get_instance().error("security_services::extract_and_store_certificate: Unsupported EncryptionKey");
          return -1;
        }
        // Add encryption keys
        OCTETSTRING public_enc_key_x, public_enc_key_y, public_enc_comp_key;
        INTEGER public_enc_comp_key_mode;
        if (extract_encryption_keys(p_certificate, public_enc_key_x, public_enc_key_y, public_enc_comp_key, public_enc_comp_key_mode) == -1) {
          loggers::get_instance().error("security_services::extract_and_store_certificate: Unsupported EncryptionKey");
          return -1;
        }
        // And store it into the cache
        _security_cache.get()->store_certificate(
                                                 CHARSTRING(p_certificate_id.c_str()),
                                                 enc_cert,
                                                 int2oct(0, 32), // No way to get the private key here
                                                 public_key_x,
                                                 public_key_y,
                                                 public_comp_key,
                                                 public_comp_key_mode,
                                                 hash_cert,
                                                 p_certificate.issuer().sha256AndDigest(),
                                                 int2oct(0, 32), // Encryption private not used
                                                 public_enc_key_x,
                                                 public_enc_key_y,