Merge branch 'STF525' of https://forge.etsi.org/gitlab/ITS/ITS into STF525

ccsrc/Externals/LibItsSecurity_externals.cc

@@ -1224,7 +1224,8 @@ namespace LibItsSecurity__Functions
 
   BOOLEAN fx__readCertificateFromDigest(
                                         const OCTETSTRING& p__digest,
-                                        CHARSTRING& p__certificateId) {
+                                        CHARSTRING& p__certificateId
+                                        ) {
     loggers::get_instance().log_msg(">>> fx__readCertificateFromDigest: ", p__digest);
 
     if (security_services::get_instance().read_certificate_from_digest(p__digest, p__certificateId) == -1) {
@@ -1235,6 +1236,20 @@ namespace LibItsSecurity__Functions
     return TRUE;
   }
   
+  BOOLEAN fx__readCertificateFromHashedId3(
+                                           const OCTETSTRING& p__digest,
+                                           CHARSTRING& p__certificateId
+                                           ) {
+    loggers::get_instance().log_msg(">>> fx__readCertificateFromHashedId3: ", p__digest);
+
+    if (security_services::get_instance().read_certificate_from_hashed_id3(p__digest, p__certificateId) == -1) {
+      return FALSE;
+    }
+    loggers::get_instance().log_msg("fx__readCertificateFromHashedId3: ", p__certificateId);
+    
+    return TRUE;
+  }
+  
   /**
    * \brief    Read the specified certificate digest
    * \param   p_certificate_id the certificate identifier
@@ -1261,9 +1276,9 @@ namespace LibItsSecurity__Functions
    * \return  true on success, false otherwise
    */
   BOOLEAN fx__readCertificateHash(
-                                    const CHARSTRING& p__certificateId,
-                                    OCTETSTRING& p__hash
-                                    ) {
+                                  const CHARSTRING& p__certificateId,
+                                  OCTETSTRING& p__hash
+                                  ) {
     loggers::get_instance().log(">>> fx__readCertificateHash: '%s'", static_cast<const char*>(p__certificateId));
 
     if (security_services::get_instance().read_certificate_hash(p__certificateId, p__hash) == -1) {

ccsrc/Protocols/Commsignia/commsignia_layer.cc

@@ -45,16 +45,16 @@ void commsignia_layer::receive_data(OCTETSTRING& data, params& params) {
   const unsigned char* p = static_cast<const unsigned char *>(data);
 
   const commsignia_layer::c2p_recv* r = (const commsignia_layer::c2p_recv*)p;
-  //loggers::get_instance().log("commsignia_layer::receive_data: version=%02x", r->version);
-  //loggers::get_instance().log("commsignia_layer::receive_data: timestamp1=%08x", ntohl(r->timestamp_sec));
-  //loggers::get_instance().log("commsignia_layer::receive_data: timestamp2=%08x", ntohl(r->timestamp_msec));
-  //loggers::get_instance().log("commsignia_layer::receive_data: timestamp2=%08x", ntohl(r->timestamp_msec));
-  //loggers::get_instance().log("commsignia_layer::receive_data: antenna=%02x", r->antenna);
-  //loggers::get_instance().log("commsignia_layer::receive_data: rssi_ant2=%02x", r->rssi_ant2);
-  //loggers::get_instance().log("commsignia_layer::receive_data: noise_ant1=%02x", r->noise_ant1);
-  //loggers::get_instance().log("commsignia_layer::receive_data: noise_ant2=%02x", r->noise_ant2);
+  loggers::get_instance().log("commsignia_layer::receive_data: version=%02x", r->version);
+  loggers::get_instance().log("commsignia_layer::receive_data: timestamp1=%08x", ntohl(r->timestamp_sec));
+  loggers::get_instance().log("commsignia_layer::receive_data: timestamp2=%08x", ntohl(r->timestamp_msec));
+  loggers::get_instance().log("commsignia_layer::receive_data: timestamp2=%08x", ntohl(r->timestamp_msec));
+  loggers::get_instance().log("commsignia_layer::receive_data: antenna=%02x", r->antenna);
+  loggers::get_instance().log("commsignia_layer::receive_data: rssi_ant2=%02x", r->rssi_ant2);
+  loggers::get_instance().log("commsignia_layer::receive_data: noise_ant1=%02x", r->noise_ant1);
+  loggers::get_instance().log("commsignia_layer::receive_data: noise_ant2=%02x", r->noise_ant2);
   // Filtering on antenna index
-  //loggers::get_instance().log("commsignia_layer::receive_data: compare %02x with %02x", r->antenna, static_cast<unsigned char>(std::strtoul(_params[params::interface_id].c_str(), NULL, 10)));
+  loggers::get_instance().log("commsignia_layer::receive_data: compare %02x with %02x", r->antenna, static_cast<unsigned char>(std::strtoul(_params[params::interface_id].c_str(), NULL, 10)));
   if (r->antenna != static_cast<unsigned char>(std::strtoul(_params[params::interface_id].c_str(), NULL, 10))) {
     // Discard packet
     loggers::get_instance().warning("commsignia_layer::receive_data: Discard packet due to wrong antenna id");

ccsrc/Protocols/Http/http_codec.cc

@@ -138,13 +138,13 @@ int http_codec::encode_request(const LibItsHttp__TypesAndValues::Request& p_requ
   p_encoding_buffer.put_cs(int2str(p_request.version__minor()));
   p_encoding_buffer.put_cs("\r\n");
   
-  // Encode headers excepeted the Content-length
+  // Encode headers excepeted the Content-Length
   const LibItsHttp__TypesAndValues::HeaderLines& headers = p_request.header();
   std::string content_type;
   for (int i = 0; i < headers.size_of(); i++) {
     const LibItsHttp__TypesAndValues::HeaderLine& header = headers[i];
     loggers::get_instance().log_msg("http_codec::encode_request: Processing header ", header.header__name());
-    if (std::string(static_cast<const char*>(header.header__name())).compare("Content-length") == 0) { // Skip it, processed later
+    if (std::string(static_cast<const char*>(header.header__name())).compare("Content-Length") == 0) { // Skip it, processed later
       loggers::get_instance().log("http_codec::encode_request: Skip it");
       continue;
     } else {
@@ -155,8 +155,8 @@ int http_codec::encode_request(const LibItsHttp__TypesAndValues::Request& p_requ
         const LibItsHttp__TypesAndValues::charstring__list& v = dynamic_cast<const OPTIONAL<LibItsHttp__TypesAndValues::charstring__list> &>(o);
         if (v.size_of() > 0) {
           loggers::get_instance().log_msg("http_codec::encode_request: Processing value ", v[0]);
-          if (std::string(static_cast<const char*>(header.header__name())).compare("Content-type") == 0) { // Store it for HTTP body payload encoding
-            loggers::get_instance().log("http_codec::encode_request: Storing Content-type");
+          if (std::string(static_cast<const char*>(header.header__name())).compare("Content-Type") == 0) { // Store it for HTTP body payload encoding
+            loggers::get_instance().log("http_codec::encode_request: Storing Content-Yype");
             int j = 0;
             while (j < v.size_of()) {
               content_type += v[j++];
@@ -196,10 +196,10 @@ int http_codec::encode_request(const LibItsHttp__TypesAndValues::Request& p_requ
     _ec.is_content_length_present = 0x00;
   }
 
-  // Encode Content-length header
-  p_encoding_buffer.put_cs("Content-length: ");
+  // Encode Content-Length header
+  p_encoding_buffer.put_cs("Content-Length: ");
   if (_ec.length != 0) {
-    loggers::get_instance().log("http_codec::encode_request: Content-length: %s", static_cast<const char*>(int2str(_ec.length + 2/*Stand for the last CRLF*/)));
+    loggers::get_instance().log("http_codec::encode_request: Content-Length: %s", static_cast<const char*>(int2str(_ec.length + 2/*Stand for the last CRLF*/)));
     p_encoding_buffer.put_cs(static_cast<const char*>(int2str(_ec.length + 2/*Stand for the last CRLF*/)));
     _ec.is_content_length_present = 0x01;
   } else {
@@ -236,7 +236,7 @@ int http_codec::encode_response (const LibItsHttp__TypesAndValues::Response& p_r
   }
   p_encoding_buffer.put_cs("\r\n");
   
-  // Encode headers excepeted the Content-length
+  // Encode headers excepeted the Content-Length
   const LibItsHttp__TypesAndValues::HeaderLines& headers = p_response.header();
   std::string content_type;
   for (int i = 0; i < headers.size_of(); i++) {
@@ -252,7 +252,7 @@ int http_codec::encode_response (const LibItsHttp__TypesAndValues::Response& p_r
         const LibItsHttp__TypesAndValues::charstring__list& v = dynamic_cast<const OPTIONAL<LibItsHttp__TypesAndValues::charstring__list> &>(o);
         if (v.size_of() > 0) {
           loggers::get_instance().log_msg("http_codec::encode_response: Processing value ", v[0]);
-          if (std::string(static_cast<const char*>(header.header__name())).compare("Content-type") == 0) { // Store it for HTTP body payload encoding
+          if (std::string(static_cast<const char*>(header.header__name())).compare("Content-Type") == 0) { // Store it for HTTP body payload encoding
             int j = 1;
             while (j < v.size_of()) {
               content_type += v[j++];
@@ -292,7 +292,7 @@ int http_codec::encode_response (const LibItsHttp__TypesAndValues::Response& p_r
     _ec.is_content_length_present = 0x00;
   }
 
-  // Encode Content-length header
+  // Encode Content-Length header
   if (_ec.length != 0) {
     p_encoding_buffer.put_cs(int2str(_ec.length + 2/*Stand for the last CRLF*/));
     _ec.is_content_length_present = 0x01;
@@ -300,7 +300,7 @@ int http_codec::encode_response (const LibItsHttp__TypesAndValues::Response& p_r
     p_encoding_buffer.put_cs("0");
     _ec.is_content_length_present = 0x00;
   }
-  loggers::get_instance().log("http_codec::encode_response: Content-length: %d - %x", _ec.length, _ec.is_content_length_present);
+  loggers::get_instance().log("http_codec::encode_response: Content-Length: %d - %x", _ec.length, _ec.is_content_length_present);
   p_encoding_buffer.put_cs("\r\n");
   
   p_encoding_buffer.put_cs("\r\n");
@@ -370,6 +370,7 @@ int http_codec::decode_header(CHARSTRING& header_line, LibItsHttp__TypesAndValue
     } else if (m[1].str().compare("Transfer-Encoding") == 0) {
       if (m[2].str().find("chunked") != std::string::npos) {
         _dc.chunked = true;
+        loggers::get_instance().log("http_codec::decode_header: decoded Transfer-Encoding %x", _dc.chunked);
       }
     }
 
@@ -431,7 +432,7 @@ int http_codec::encode_body(const LibItsHttp__MessageBodyTypes::HttpMessageBody&
     } else {
       std::map<std::string, std::unique_ptr<codec<Record_Type, Record_Type> > >::const_iterator it;
       bool processed = false;
-      loggers::get_instance().log("http_codec::encode_body: Content-type:'%s'", p_content_type.c_str());
+      loggers::get_instance().log("http_codec::encode_body: Content-Type:'%s'", p_content_type.c_str());
       if (p_content_type.find("held") != std::string::npos) {
         it = _codecs.find("held"); // TODO Use params
         if (it != _codecs.cend()) {
@@ -515,26 +516,45 @@ int http_codec::decode_body(TTCN_Buffer& decoding_buffer, LibItsHttp__MessageBod
   }
   loggers::get_instance().log("http_codec::decode_body: counter=%d", counter);
   body = OCTETSTRING(body.lengthof() - counter, static_cast<const unsigned char*>(body));
-  if (_dc.chunked) {
-    // Extract the size of the chunk <chunk size>\r[\n]
+  if (_dc.chunked){
     counter = 0;
-    while (counter < body.lengthof()) {
-      if ((body[counter].get_octet() == '\r') || (body[counter].get_octet() == '\n')) {
-        break;
-      }
-      counter += 1;
-    } // End of 'while' statement
-    loggers::get_instance().log("http_codec::decode_body: Chunked: counter=%d", counter);
-    if (counter < body.lengthof()) { // Skip additional \n
-      OCTETSTRING os(counter - 1, static_cast<const unsigned char*>(body));
-      loggers::get_instance().log_msg("http_codec::decode_body: os: ", os);
-      int len = oct2int(os);
-      loggers::get_instance().log("http_codec::decode_body: Chunk len: %d", len);
-      if (body[counter].get_octet() == '\n') {
+    int prev = 0;
+    OCTETSTRING os(0, nullptr);
+    do {
+      while (counter < body.lengthof()) { // Extract the size of the chunk <chunk size>\r[\n]
+        if ((body[counter].get_octet() == '\r') || (body[counter].get_octet() == '\n')) {
+          break;
+        }
         counter += 1;
+      } // End of 'while' statement
+      loggers::get_instance().log("http_codec::decode_body: Chunked(0): prev = %d, counter=%d / %d", prev, counter, body.lengthof());
+      if (counter < body.lengthof()) {
+        int idx = counter - prev;
+        OCTETSTRING trunk(idx, static_cast<const unsigned char*>(body));
+        loggers::get_instance().log_msg("http_codec::decode_body: trunk: ", trunk);
+        std::string str((const char*)static_cast<const unsigned char*>(trunk), idx);
+        loggers::get_instance().log("http_codec::decode_body: str: '%s'", str.c_str());
+        int len = std::stoi(str, nullptr, 16);//converter::get_instance().string_to_int(str);
+        loggers::get_instance().log("http_codec::decode_body: Chunk len: %d", len);
+        while (counter < body.lengthof() && ((body[counter].get_octet() == '\r') || (body[counter].get_octet() == '\n'))) { // Skip additional \n
+          counter += 1;
+        } // End of 'while' statement
+        if (counter < body.lengthof()) {
+          loggers::get_instance().log("http_codec::decode_body: Chunked (1): prev = %d, counter=%d / %d", prev, counter, body.lengthof());
+          os += OCTETSTRING(len, counter + static_cast<const unsigned char*>(body));
+          loggers::get_instance().log_msg("http_codec::decode_body: os=", os);
+          counter += len;
+          loggers::get_instance().log("http_codec::decode_body: Chunked: %02x %02x %02x", body[counter].get_octet(), body[counter + 1].get_octet(), body[counter + 2].get_octet());
+          loggers::get_instance().log("http_codec::decode_body: Chunked (2): prev = %d, counter=%d / %d", prev, counter, body.lengthof());
+          while (counter < body.lengthof() && ((body[counter].get_octet() == '\r') || (body[counter].get_octet() == '\n'))) { // Skip additional \n
+            counter += 1;
+          } // End of 'while' statement
+          prev = counter;
+          loggers::get_instance().log("http_codec::decode_body: Chunked (3): prev = %d, counter=%d / %d", prev, counter, body.lengthof());
+        }
       }
-      body = OCTETSTRING(len, counter + static_cast<const unsigned char*>(body));
-    }
+    } while (counter < body.lengthof()); // Process next chunk if any
+    body = os;
   }
   loggers::get_instance().log_msg("http_codec::decode_body: Finalised body=", body);
   // Check if HTTP message body contains binary characters

ccsrc/Protocols/Security/certificates_loader.cc

@@ -149,21 +149,23 @@ int certificates_loader::build_certificates_cache(std::set<std::experimental::fi
     p_files.erase(it);
     
     // Load private key file
+    OCTETSTRING private_key;
     p = p.replace_extension(_privateKeyExt);
     loggers::get_instance().log("certificates_loader::build_certificates_cache: Caching private keys '%s'", p.string().c_str());
     is.open(p, ios::in | ios::binary);
     if (!is.is_open()) {
       loggers::get_instance().warning("certificates_loader::build_certificates_cache: Failed to open Private key");
-      return -1;
-    }
-    int size = std::experimental::filesystem::file_size(p);
-    if ((size != 32) && (size != 48)) {
-      loggers::get_instance().warning("certificates_loader::build_certificates_cache: Private key size is incorrect for '%s'", key.c_str());
-      return -1;
+      private_key = OCTETSTRING(0, nullptr);
+    } else {
+      int size = std::experimental::filesystem::file_size(p);
+      if ((size != 32) && (size != 48)) {
+        loggers::get_instance().warning("certificates_loader::build_certificates_cache: Private key size is incorrect for '%s'", key.c_str());
+        return -1;
+      }
+      private_key = int2oct(0, size);
+      is.read((char *)static_cast<const unsigned char*>(private_key), private_key.lengthof());
+      is.close();
     }
-    OCTETSTRING private_key = int2oct(0, size);
-    is.read((char *)static_cast<const unsigned char*>(private_key), private_key.lengthof());
-    is.close();
     
     // Load private encryption key file if present
     OCTETSTRING private_enc_key;
@@ -174,7 +176,7 @@ int certificates_loader::build_certificates_cache(std::set<std::experimental::fi
       loggers::get_instance().warning("certificates_loader::build_certificates_cache: Failed to open Private encryption key file");
       private_enc_key = OCTETSTRING(0, nullptr);
     } else {
-      size = std::experimental::filesystem::file_size(p);
+      int size = std::experimental::filesystem::file_size(p);
       if (size != 32) { // IEEE Std 1609.2 2017: NistP256 or BrainpoolP256r1
         loggers::get_instance().warning("certificates_loader::build_certificates_cache: Private encryption key size is incorrect for '%s'", key.c_str());
         return -1;

ccsrc/Protocols/Security/security_cache.cc

@@ -23,8 +23,8 @@ using namespace std; // Required for isnan()
 #include "loggers.hh"
 
 bool security_cache_comp::operator()(const OCTETSTRING& p_lhs, const OCTETSTRING& p_rhs) const {
-  loggers::get_instance().log_msg(">>> security_cache_comp::operator(): p_lhs= ", p_lhs);
-  loggers::get_instance().log_msg(">>> security_cache_comp::operator(): p_rhs= ", p_rhs);
+  //loggers::get_instance().log_msg(">>> security_cache_comp::operator(): p_lhs= ", p_lhs);
+  //loggers::get_instance().log_msg(">>> security_cache_comp::operator(): p_rhs= ", p_rhs);
   unsigned char* first1 = (unsigned char*)static_cast<const unsigned char*>(p_lhs);
   unsigned char* first2 = (unsigned char*)static_cast<const unsigned char*>(p_rhs);
   unsigned char* last1 = p_lhs.lengthof() + (unsigned char*)static_cast<const unsigned char*>(p_lhs);
@@ -59,7 +59,7 @@ int security_cache::clear() {
   return 0;
 } // End of clear method
 
-int security_cache::get_certificate_id(const OCTETSTRING& p_hashed_id8, std::string& p_certifcate_id) const {
+int security_cache::get_certificate_id(const OCTETSTRING& p_hashed_id8, std::string& p_certificate_id) const {
   loggers::get_instance().log_msg(">>> security_cache::get_certificate_id: ", p_hashed_id8);
 
   std::map<OCTETSTRING, std::string>::const_iterator it = _hashed_id8s.find(p_hashed_id8);
@@ -67,10 +67,10 @@ int security_cache::get_certificate_id(const OCTETSTRING& p_hashed_id8, std::str
   //std::map<std::vector<unsigned char>, std::string>::const_iterator it = _hashed_id8s.find(v);
   if (it == _hashed_id8s.cend()) {
     dump();
-    p_certifcate_id = "";
+    p_certificate_id = "";
     return -1;
   }
-  p_certifcate_id = it->second;
+  p_certificate_id = it->second;
 
   return 0;
 }
@@ -91,7 +91,7 @@ int security_cache::get_certificate(const std::string& p_certificate_id, OCTETST
 
 int security_cache::get_certificate(const std::string& p_certificate_id, IEEE1609dot2::CertificateBase& p_certificate) const {
   loggers::get_instance().log(">>> security_cache::get_certificate: '%s'", p_certificate_id.c_str());
-
+  
   std::map<std::string, std::unique_ptr<security_db_record> >::const_iterator it = _certificates.find(p_certificate_id);
   if (it == _certificates.cend()) {
     loggers::get_instance().warning("security_cache::get_certificate: record not found");
@@ -102,6 +102,65 @@ int security_cache::get_certificate(const std::string& p_certificate_id, IEEE160
   return 0;
 }
 
+int security_cache::get_certificate(const OCTETSTRING& p_hashed_id3, IEEE1609dot2::CertificateBase& p_certificate) const {
+  loggers::get_instance().log_msg(">>> security_cache::get_certificate: ", p_hashed_id3);
+
+  std::map<OCTETSTRING, std::string, security_cache_comp>::const_iterator s;
+  for (s = _hashed_id8s.cbegin(); s != _hashed_id8s.cend(); ++s) {
+    if (
+        (p_hashed_id3[2] == (s->first)[(s->first).lengthof() - 1]) &&
+        (p_hashed_id3[1] == (s->first)[(s->first).lengthof() - 2]) &&
+        (p_hashed_id3[0] == (s->first)[(s->first).lengthof() - 3])
+        ) {
+      break;
+    }
+  } // End of 'for' statement
+  if (s == _hashed_id8s.cend()) {
+    loggers::get_instance().warning("security_cache::get_certificate: hashedId3 does not match");
+    return -1;
+  }
+  /*TODO std::map<OCTETSTRING, std::string, security_cache_comp>::const_iterator s = std::find_if(
+                                                                                           _hashed_id8s.cbegin(),
+                                                                                           _hashed_id8s.cend(),
+                                                                                           hashed_id_matching
+                                                                                           );*/
+  std::map<std::string, std::unique_ptr<security_db_record> >::const_iterator it = _certificates.find(s->second);
+  if (it == _certificates.cend()) {
+    loggers::get_instance().warning("security_cache::get_certificate: record not found");
+    return -1;
+  }
+  p_certificate = it->second.get()->decoded_certificate();
+  
+  return 0;
+}
+
+int security_cache::get_certificate_hashed_id3(const OCTETSTRING& p_hashed_id3, std::string& p_certificate_id) const {
+  loggers::get_instance().log_msg(">>> security_cache::get_certificate_hashed_id3: ", p_hashed_id3);
+
+  std::map<OCTETSTRING, std::string, security_cache_comp>::const_iterator s;
+  for (s = _hashed_id8s.cbegin(); s != _hashed_id8s.cend(); ++s) {
+    if (
+        (p_hashed_id3[2] == (s->first)[(s->first).lengthof() - 1]) &&
+        (p_hashed_id3[1] == (s->first)[(s->first).lengthof() - 2]) &&
+        (p_hashed_id3[0] == (s->first)[(s->first).lengthof() - 3])
+        ) {
+      break;
+    }
+  } // End of 'for' statement
+  if (s == _hashed_id8s.cend()) {
+    loggers::get_instance().warning("security_cache::get_certificate_hashed_id3: hashedId3 does not match");
+    return -1;
+  }
+  /*TODO std::map<OCTETSTRING, std::string, security_cache_comp>::const_iterator s = std::find_if(
+                                                                                           _hashed_id8s.cbegin(),
+                                                                                           _hashed_id8s.cend(),
+                                                                                           hashed_id_matching
+                                                                                           );*/
+  p_certificate_id = s->second;
+  
+  return 0;
+}
+
 int security_cache::get_issuer(const std::string& p_certificate_id, OCTETSTRING& p_hashed_id_issuer) const {
   loggers::get_instance().log(">>> security_cache::get_issuer: '%s'", p_certificate_id.c_str());
 

ccsrc/Protocols/Security/security_cache.hh

@@ -52,6 +52,8 @@ public: /*! \publicsection */
   int get_certificate_id(const OCTETSTRING& p_hashed_id8, std::string& p_certificate_id) const;
   int get_certificate(const std::string& p_certificate_id, OCTETSTRING& p_certificate) const;
   int get_certificate(const std::string& p_certificate_id, IEEE1609dot2::CertificateBase& p_certificate) const;
+  int get_certificate(const OCTETSTRING& p_hashed_id3, IEEE1609dot2::CertificateBase& p_certificate) const;
+  int get_certificate_hashed_id3(const OCTETSTRING& p_hashed_id3, std::string& p_certificate_id) const;
   int get_issuer(const std::string& p_certificate_id, OCTETSTRING& p_hashed_id_issuer) const;
   int get_hashed_id(const std::string& p_certificate_id, OCTETSTRING& p_hashed_id) const;
   int get_hash(const std::string& p_certificate_id, OCTETSTRING& p_hash) const;

ccsrc/Protocols/Security/security_services.cc

@@ -23,7 +23,7 @@ using namespace std; // Required for isnan()
 
 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(0, nullptr), _latitude(0), _longitude(0), _elevation(0) {
+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(0, nullptr), _requested_certificate(), _latitude(0), _longitude(0), _elevation(0) {
   loggers::get_instance().log(">>> security_services::security_services");
 } // End of ctor
 
@@ -176,8 +176,13 @@ int security_services::process_ieee_1609_dot2_signed_data(const IEEE1609dot2::Si
 
   // 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
+    loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: inlineP2pcdRequest: ", header_info.inlineP2pcdRequest());
+    const IEEE1609dot2BaseTypes::SequenceOfHashedId3& s = static_cast<const IEEE1609dot2BaseTypes::SequenceOfHashedId3&>(*header_info.inlineP2pcdRequest().get_opt_value());
+    _requested_certificate.clear();
+    for (int i = 0; i < s.lengthof(); i++) {
+      loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: Add requested certificate= ", s[i]);
+      _requested_certificate.push_back(s[i]);
+    } // End of 'for' statement
   }
 
   // Check requested certificate
@@ -505,8 +510,15 @@ int security_services::sign_payload(const OCTETSTRING& p_unsecured_gn_payload, O
   } else {
     header_info.inlineP2pcdRequest().set_to_omit();
   }
-  header_info.requestedCertificate().set_to_omit();
-
+  if (_requested_certificate.size() != 0) {
+    IEEE1609dot2::CertificateBase c;
+    const IEEE1609dot2BaseTypes::HashedId3& hashed_id3 = _requested_certificate[0];
+    _security_db->get_certificate(hashed_id3, c);
+    header_info.requestedCertificate() = OPTIONAL<IEEE1609dot2::CertificateBase>(c);
+  } else {
+    header_info.requestedCertificate().set_to_omit();
+  }
+  
   IEEE1609dot2::ToBeSignedData tbs_data;
   tbs_data.payload() = payload;
   tbs_data.headerInfo() = header_info;
@@ -1348,6 +1360,15 @@ int security_services::read_certificate_from_digest(const OCTETSTRING& p_digest,
   return -1;
 }
 
+int security_services::read_certificate_from_hashed_id3(const OCTETSTRING& p_digest, CHARSTRING& p_certificate_id) const {
+  std::string certificate_id;
+  if (_security_db.get()->get_certificate_hashed_id3(p_digest, certificate_id) != -1) {
+    p_certificate_id = CHARSTRING(certificate_id.c_str());
+    return 0;
+  }
+  return -1;
+}
+
 int security_services::read_private_key(const CHARSTRING& p_certificate_id, OCTETSTRING& p_private_key) const {
   return _security_db.get()->get_private_key(std::string(static_cast<const char*>(p_certificate_id)), p_private_key);
 }

ccsrc/Protocols/Security/security_services.hh

@@ -47,6 +47,7 @@ class security_services {
   std::unique_ptr<security_db> _security_db;
   unsigned long long _last_generation_time;
   OCTETSTRING _unknown_certificate;
+  std::vector<OCTETSTRING> _requested_certificate;
   int _latitude;
   int _longitude;
   int _elevation;
@@ -109,6 +110,7 @@ public: /*! \publicsection */
   int read_certificate_digest(const CHARSTRING& p_certificate_id, OCTETSTRING& p_digest) const;
   int read_certificate_hash(const CHARSTRING& p_certificate_id, OCTETSTRING& p_hash) const;
   int read_certificate_from_digest(const OCTETSTRING& p_digest, CHARSTRING& p_certificate_id) const;
+  int read_certificate_from_hashed_id3(const OCTETSTRING& p_digest, CHARSTRING& p_certificate_id) const;
   int read_private_key(const CHARSTRING& p_certificate_id, OCTETSTRING& p_private_key) const;
   int read_private_enc_key(const CHARSTRING& p_certificate_id, OCTETSTRING& p_private_enc_key) const;
   

ccsrc/Protocols/UDP/udp_pcap_layer.cc

+#include "udp_pcap_layer_factory.hh"
+
+#include "loggers.hh"
+
+udp_pcap_layer::udp_pcap_layer(const std::string & p_type, const std::string & param) : layer(p_type), _params() {
+  loggers::get_instance().log(">>> udp_pcap_layer::udp_pcap_layer: %s, %s", to_string().c_str(), param.c_str());
+  // Setup parameters
+  params::convert(_params, param);
+  params::const_iterator it = _params.find("src_ip");
+  if (it == _params.cend()) {
+    _params.insert(std::pair<std::string, std::string>(std::string("src_ip"), "127.0.0.1"));
+  }
+  it = _params.find("src_port");
+  if (it == _params.cend()) {
+    _params.insert(std::pair<std::string, std::string>(std::string("src_port"), "12345"));
+  }
+  it = _params.find("dst_ip");
+  if (it == _params.cend()) {
+    _params.insert(std::pair<std::string, std::string>(std::string("dst_ip"), "127.0.0.1"));
+  }
+  it = _params.find("dst_port");
+  if (it == _params.cend()) {
+    _params.insert(std::pair<std::string, std::string>(std::string("dst_port"), "12346"));
+  }
+
+  //_params.log();
+}
+
+void udp_pcap_layer::send_data(OCTETSTRING& data, params& params) {
+  loggers::get_instance().log_msg(">>> udp_pcap_layer::send_data: ", data);
+  
+  // Create IP/UDP packet
+  unsigned int len = sizeof(struct iphdr) + sizeof(struct udphdr) + data.lengthof();
+  unsigned char *buffer = new unsigned char[len];
+  // Set ip header
+  _iphdr = (struct iphdr *)buffer;
+  _daddr.sin_family = AF_INET;
+  _saddr.sin_family = AF_INET;
+  params::const_iterator it = _params.find("dst_port");
+  _daddr.sin_port = htons(std::strtoul(it->second.c_str(), NULL, 10));
+  it = _params.find("src_port");
+  _saddr.sin_port = htons(std::strtoul(it->second.c_str(), NULL, 10));
+  it = _params.find("dst_ip");
+  inet_pton(AF_INET, it->second.c_str(), (struct in_addr *)&_daddr.sin_addr.s_addr);
+  it = _params.find("src_ip");
+  inet_pton(AF_INET, it->second.c_str(), (struct in_addr *)&_saddr.sin_addr.s_addr);
+  _iphdr->ihl = 5;
+  _iphdr->version = 4;
+  _iphdr->tos = IPTOS_LOWDELAY;
+  _iphdr->id = 0;
+  _iphdr->frag_off = htons(0x4000); /* Don't fragment */
+  _iphdr->ttl = 64;
+  _iphdr->tot_len = htons(sizeof(struct iphdr) + sizeof(struct udphdr) + data.lengthof());
+  _iphdr->protocol = IPPROTO_UDP;
+  _iphdr->saddr = _saddr.sin_addr.s_addr;
+  _iphdr->daddr = _daddr.sin_addr.s_addr;
+  _iphdr->check = 0;
+  _iphdr->check = inet_check_sum((const void *)_iphdr, sizeof(struct iphdr));
+  // Set udp header
+  _udphdr = (struct udphdr *)(buffer + sizeof(struct iphdr));
+  _udphdr->source = _saddr.sin_port;
+  _udphdr->dest = _daddr.sin_port;
+  _udphdr->len = htons(sizeof(struct udphdr) + data.lengthof());
+  _udphdr->check = 0;
+  // Set payload
+  unsigned char *payload = buffer + sizeof(struct iphdr) + sizeof(struct udphdr);
+  memcpy(payload, static_cast<const unsigned char *>(data), data.lengthof());
+  // Calculate UDP checksum
+  _udphdr->check = inet_check_sum(
+                                  (const void *)_udphdr,
+                                  sizeof(struct udphdr),
+                                  inet_check_sum(
+                                                 static_cast<const unsigned char*>(data),
+                                                 data.lengthof(),
+                                                 inet_check_sum(
+                                                                (const unsigned char*)(&(_iphdr->saddr)),
+                                                                2 * sizeof(_iphdr->saddr),
+                                                                IPPROTO_UDP + static_cast<unsigned int>(ntohs(_udphdr->len))
+                                                                )
+                                                 )
+                                  );
+  // Send data lower layers
+  OCTETSTRING udp(len, buffer);
+  send_to_all_layers(udp, params);
+  // Free buffer
+  delete [] buffer;
+}
+
+void udp_pcap_layer::receive_data(OCTETSTRING& data, params& params) {
+  loggers::get_instance().log_msg(">>> udp_pcap_layer::receive_data: ", data);
+
+  // Decode UDP packet
+  const unsigned char* buffer = static_cast<const unsigned char *>(data);
+  _iphdr = (struct iphdr*)buffer;
+  _udphdr = (struct udphdr*)(buffer + sizeof(struct iphdr));
+  loggers::get_instance().log("udp_pcap_layer::receive_data: src_port = %d, payload length = %d", ntohs(_udphdr->source), ntohs(_udphdr->len));
+  // TODO To be refined
+  data = OCTETSTRING(ntohs(_udphdr->len) - sizeof(struct udphdr), (unsigned char*)(buffer + sizeof(struct iphdr) + sizeof(struct udphdr)));
+  //loggers::get_instance().log_msg("udp_pcap_layer::receive_data: message payload", data);
+  
+  receive_to_all_layers(data, params);
+}
+
+unsigned short udp_pcap_layer::inet_check_sum(const void *buf, size_t len, const unsigned short p_initial_sum) {
+  unsigned long sum = p_initial_sum;
+  unsigned int i;
+