Start Security support in the TA

Start ATS security review
Validation of TD_AUTO_IOT_DENM_RWW_BV_01 in progress

ccsrc/EncDec/LibItsSecurity_Encdec.cc

@@ -3,6 +3,7 @@
 #include "EtsiTs103097Codec_Certificate.hh"
 #include "EtsiTs103097Codec_ToBeSignedCertificate.hh"
 #include "EtsiTs103097Codec_Data.hh"
+#include "EtsiTs103097Codec_ToBeSignedData.hh"
 
 #include "loggers.hh"
 
@@ -75,6 +76,33 @@ namespace LibItsSecurity__EncdecDeclarations {
     return 0;
   }
   
+  BITSTRING fx__enc__Ieee1609Dot2ToBeSignedData(const IEEE1609dot2::ToBeSignedData& p__toBeSignedData) {
+    loggers::get_instance().log_msg(">>> fx__enc__Ieee1609Dot2ToBeSignedData: ", p__toBeSignedData);
+
+    EtsiTs103097Codec_ToBeSignedData codec;
+    OCTETSTRING os;
+    if (codec.encode(p__toBeSignedData, os) == -1) {
+      loggers::get_instance().warning("fx__enc__Ieee1609Dot2ToBeSignedData: -1 result code was returned");
+      return int2bit(0, 1);
+    }
+    
+    return oct2bit(os);
+  }
+  
+  INTEGER fx__dec__Ieee1609Dot2ToBeSignedData(BITSTRING& p__data, IEEE1609dot2::ToBeSignedData& p__toBeSignedData) {
+    loggers::get_instance().log_msg(">>> fx__dec__Ieee1609Dot2ToBeSignedData: ", p__data);
+
+    EtsiTs103097Codec_ToBeSignedData codec;
+    OCTETSTRING is = bit2oct(p__data);
+    if (codec.decode(is, p__toBeSignedData) == -1) {
+      loggers::get_instance().warning("fx__dec__Ieee1609Dot2ToBeSignedData: -1 result code was returned");
+      return -1;
+    }
+    
+    loggers::get_instance().log_msg("<<< fx__dec__Ieee1609Dot2ToBeSignedData: ", p__toBeSignedData);
+    return 0;
+  }
+  
   BITSTRING fx__enc__SspCAM(const LibItsSecurity__TypesAndValues::SspCAM& p__ssp) {
     loggers::get_instance().log_msg(">>> fx__enc__SspCAM: ", p__ssp);
 

ccsrc/Externals/LibItsSecurity_externals.cc

@@ -16,6 +16,9 @@
 
 namespace LibItsSecurity__Functions 
 {
+
+  // FIXME Unify code with security_services
+  
   /**
    * @desc    Produces a 256-bit (32-byte) hash value
    * @param   p_toBeHashedData Data to be used to calculate the hash value

ccsrc/Framework/LayerFactory.hh

@@ -23,8 +23,81 @@
  * \abstract
  */
 class LayerFactory {
-public:
+public: //! \publicsection
+  /*!
+   * \fn Codec();
+   * \brief  Default constructor
+   * \todo Remove logs
+   */
   LayerFactory() {};
-  virtual Layer * createLayer(const std::string & type, const std::string & param) = 0;
+  /*!
+   * \fn Layer * createLayer(const std::string & type, const std::string & param);
+   * \brief  Create the layers stack based on the provided layers stack description (cf. remark)
+   * \param[in] p_type The provided layers stack description
+   * \param[in] p_params Optional parameters
+   * \return 0 on success, -1 otherwise
+   * \remark The description below introduces layers stack in case of ITS project:
+   *     CAM Layer
+   *       next_header     : btpA|btpB (overwrite BTP.type)
+   *       header_type     : tsb|gbc
+   *       header_sub_type : sh (single hop)
+   *     DENM Layer
+   *       next_header     : btpA|btpB (overwrite BTP.type)
+   *       header_type     : tsb|gbc
+   *     BTP Layer
+   *       type            : btpA|btpB
+   *       destination port: dst_port
+   *       source port     : src_port
+   *       device_mode     : Set to 1 if the layer shall encapsulate upper layer PDU
+   *       device_mode     : Set to 1 if the layer shall encapsulate upper layer PDU
+   *     GN Layer
+   *       ll_address             : GeoNetworking address of the Test System
+   *       latitude               : latitude of the Test System
+   *       longitude              : longitude of the Test System
+   *       beaconing              : Set to 1 if GnLayer shall start beaconing
+   *       Beaconning timer expiry: expiry (ms)
+   *       device_mode            : Set to 1 if the layer shall encapsulate upper layer PDU
+   *     Ethernet layer
+   *       mac_src  :Source MAC address
+   *       mac_bc   :Broadcast address
+   *       eth_type : Ethernet type
+   *     Commsignia layer
+   *       mac_src     : Device MAC address, used to discard packets
+   *                     To indicate no filering, use the value 000000000000
+   *       mac_bc      : Broadcast address
+   *       eth_type    : Ethernet type, used to discard packets
+   *       target_host : Device address
+   *       target_port : Device port
+   *       source_port : Test System port
+   *       interface_id: Interface id, used to discard packets
+   *       tx_power    : TX power (dB)
+   *     UDP layer (IP/UDP based on Pcap)
+   *       dst_ip  : destination IPv4 address (aa.bb.cc.dd)
+   *       dst_port: destination port
+   *       src_ip  : source IPv4 address (aa.bb.cc.dd)
+   *       src_port: source port
+   *     Pcap layer
+   *       mac_src    : Source MAC address, used to exclude from capture the acket sent by the Test System
+   *       filter     : Pcap filter (compliant with tcpdump syntax) 
+   *                    E.g. filter=and ether src 04e548000001
+   *       Online mode:
+   *         nic: Local NIC
+   *              If set, online mode is used
+   *       Offline mode (nic is present but not set):
+   *         file        : File to read
+   *         frame_offset: Frame offset, used to skip packets with frame number < frame_offset
+   *         time_offset : Time offset, used to skip packets with time offset < time_offset
+   *         save_mode   : 1 to save sent packet, 0 otherwise
+   *     Here are some examples:
+   *       Geonetworking multiple component case:
+   *         NodeB.geoNetworkingPort.params := "GN(ll_address=04e548000001,latitude=43551050,longitude=10298730,beaconing=0,expiry=1000,its_aid=141)/COMMSIGNIA(mac_src=04e548000001,mac_bc=FFFFFFFFFFFF,eth_type=8947,target_host=10.200.1.101,target_port=7942,source_port=7943,its_aid=141,interface_id=2,tx_power=-32)/UDP(dst_ip=192.168.56.1,dst_port=12346,src_ip=192.168.156.4,src_port=12345)/ETH(mac_src=04e548000001,mac_dst=0A0027000011,eth_type=0800)/PCAP(mac_src=04e548000001,file=/home/vagrant/TriesAndDelete/etsi_its/testdata/TC_AUTO_IOT_DENM_RWW_BV_01_short.pcap,filter=and (udp port 30000 or udp port 7943))"
+NodeC.geoNetworkingPort.params := "GN(ll_address=70b3d5791b48,latitude=43551050,longitude=10298730,beaconing=0,expiry=1000,its_aid=141)/COMMSIGNIA(mac_src=70b3d5791b48,mac_bc=FFFFFFFFFFFF,eth_type=8947,target_host=10.200.1.101,target_port=7942,source_port=7943,its_aid=141,interface_id=2,tx_power=-32)/UDP(dst_ip=192.168.56.1,dst_port=12346,src_ip=192.168.156.4,src_port=12345)/ETH(mac_src=70b3d5791b48,mac_dst=0A0027000011,eth_type=0800)/PCAP(mac_src=70b3d5791b48,file=/home/vagrant/TriesAndDelete/etsi_its/testdata/TC_AUTO_IOT_DENM_RWW_BV_01_short.pcap,filter=and (udp port 30000 or udp port 7943))"
+   *         NodeB.geoNetworkingPort.params := "GN(ll_address=04e548000001,latitude=43551050,longitude=10298730,beaconing=0,expiry=1000,its_aid=141)/ETH(mac_src=04e548000001,mac_dst=0A0027000011,eth_type=0800)/PCAP(mac_src=04e548000001,file=/home/vagrant/TriesAndDelete/etsi_its/testdata/TC_AUTO_IOT_DENM_RWW_BV_01.pcap,filter=and ether src 04e548000001)"
+#NodeC.geoNetworkingPort.params := "GN(ll_address=70b3d5791b48,latitude=43551050,longitude=10298730,beaconing=0,expiry=1000,its_aid=141)/ETH(mac_src=70b3d5791b48,mac_dst=0A0027000011,eth_type=0800)/PCAP(mac_src=70b3d5791b48,file=/home/vagrant/TriesAndDelete/etsi_its/testdata/TC_AUTO_IOT_DENM_RWW_BV_01.pcap,filter=and  ether src 70b3d5791b48)"
+   *       UpperTester port based on UDP 
+   *         ystem.utPort.params := "UT_GN/UDP(dst_ip=192.168.1.1,dst_port=12346,src_ip=192.168.156.4,src_port=12345)/ETH(mac_src=026f8338c1e5,mac_dst=0A0027000011,eth_type=0800)/PCAP(mac_src=0800275c4959,nic=enp0s8,filter=and udp port 12346)"
+   * \pure
+   */
+  virtual Layer * createLayer(const std::string & p_type, const std::string & p_params) = 0;
 }; // End of class LayerFactory
 

ccsrc/Framework/Params.hh

@@ -46,6 +46,13 @@ public:
   static const std::string& expiry;                //! Test system GeoNetworking Lifetime parameter name (in ms)
   
   static const std::string& device_mode;           //! To indicate to the lower layer to act as a standalone device
+  static const std::string& secured_mode;          //! To indicate to the lower layer to apply securty on message exchanges as defined in IEEE 1609.2 & ETSI TS 102 965
+  static const std::string& enable_security_checks;//! To indicates if security check failures shall be treated as error or warning
+  static const std::string& sec_db_path;
+  static const std::string& certificate;
+  static const std::string& hash;
+  static const std::string& signature;
+  static const std::string& cypher;
   static const std::string& distanceA;             //! Test system GeoNetworking DistanceA parameter name
   static const std::string& distanceB;             //! Test system GeoNetworking DistanceB parameter name
   static const std::string& angle;                 //! Test system GeoNetworking Angle parameter name
@@ -74,7 +81,12 @@ public:
   virtual ~Params() { };
 
   /*!
-   * \fn void log();
+   * \fn void log() const;
+   * \brief Provides a dump of the content of this instance
+   */
+  void log() const;
+  /*!
+   * \fn void log() const;
    * \brief Provides a dump of the content of this instance
    */
   void log();

ccsrc/Framework/src/Params.cc

@@ -39,6 +39,13 @@ const std::string& Params::ll_address = std::string("ll_address");
 const std::string& Params::expiry = std::string("expiry");
 
 const std::string& Params::device_mode = std::string("device_mode");
+const std::string& Params::secured_mode = std::string("secured_mode");
+const std::string& Params::enable_security_checks = std::string("enable_security_checks");
+const std::string& Params::certificate = std::string("certificate");
+const std::string& Params::sec_db_path = std::string("sec_db_path");
+const std::string& Params::hash = std::string("hash");
+const std::string& Params::signature = std::string("signature");
+const std::string& Params::cypher = std::string("cypher");
 const std::string& Params::distanceA = std::string("distanceA");
 const std::string& Params::distanceB = std::string("distanceB");
 const std::string& Params::angle = std::string("angle");
@@ -54,6 +61,7 @@ void Params::convert(Params& p_param, const std::string p_parameters) {
   if (p_parameters.length() == 0) {
     return;
   }
+  loggers::get_instance().log(">>> Params::convert: %s", p_parameters.c_str());
   // Extract parameters
   try {
     std::regex rgx ("(\\w+)=(.*?)(,|$)");
@@ -61,13 +69,25 @@ void Params::convert(Params& p_param, const std::string p_parameters) {
     std::sregex_iterator end = std::sregex_iterator();
     for (std::sregex_iterator it = begin; it != end; ++it) {
       std::smatch m = *it;
-      //loggers::get_instance().log("Params::convert: %d - %s - %s - %s - %s", m.size(), m[0].str().c_str(), m[1].str().c_str(), m[2].str().c_str(), m[3].str().c_str());
+      loggers::get_instance().log("Params::convert: %d - %s - %s - %s - %s", m.size(), m[0].str().c_str(), m[1].str().c_str(), m[2].str().c_str(), m[3].str().c_str());
       p_param.insert(std::pair<std::string, std::string>(m[1].str(), m[2].str()));
     } // End of 'for' statement
   }
   catch(const std::logic_error& e){
     p_param.clear();
   }
+  loggers::get_instance().log("<<< Params::convert");
+}
+
+void Params::log() const {
+  loggers::get_instance().log("Params::log");
+  if (size() == 0) {
+    loggers::get_instance().log("\tEmpty");
+  } else {
+    for (const_iterator it = cbegin(); it != cend(); ++it) {
+      loggers::get_instance().log("\t(%s, %s)", it->first.c_str(), it->second.c_str());
+    } // End of 'for' statement
+  }
 }
 
 void Params::log() {

ccsrc/Ports/LibIts_ports/CAM_ports/CamPort.cc

@@ -95,7 +95,8 @@ namespace LibItsCam__TestSystem {
     
     float duration;
     loggers::get_instance().set_start_time(_time_key);
-    static_cast<CAMLayer *>(_layer)->sendMsg(send_par, _layer_params);
+    Params params;
+    static_cast<CAMLayer *>(_layer)->sendMsg(send_par, params);
     loggers::get_instance().set_stop_time(_time_key, duration);
   }
 

ccsrc/Ports/LibIts_ports/DENM_ports/DenmPort.cc

@@ -95,7 +95,8 @@ namespace LibItsDenm__TestSystem {
     
     float duration;
     loggers::get_instance().set_start_time(_time_key);
-    static_cast<DENMLayer *>(_layer)->sendMsg(send_par, _layer_params);
+    Params params;
+    static_cast<DENMLayer *>(_layer)->sendMsg(send_par, params);
     loggers::get_instance().set_stop_time(_time_key, duration);
   }
 

ccsrc/Ports/LibIts_ports/GN_ports/GeoNetworkingPort.cc

@@ -103,7 +103,8 @@ namespace LibItsGeoNetworking__TestSystem {
     
     float duration;
     loggers::get_instance().set_start_time(_time_key);
-    static_cast<GeoNetworkingLayer *>(_layer)->sendMsg(send_par, _layer_params);
+    Params params;
+    static_cast<GeoNetworkingLayer *>(_layer)->sendMsg(send_par, params);
     loggers::get_instance().set_stop_time(_time_key, duration);
   }
 

ccsrc/Ports/LibIts_ports/IVIM_ports/IvimPort.cc

@@ -92,7 +92,8 @@ namespace LibItsIvim__TestSystem {
     
     float duration;
     loggers::get_instance().set_start_time(_time_key);
-    static_cast<IVIMLayer *>(_layer)->sendMsg(send_par, _layer_params);
+    Params params;
+    static_cast<IVIMLayer *>(_layer)->sendMsg(send_par, params);
     loggers::get_instance().set_stop_time(_time_key, duration);
   }
 

ccsrc/Ports/LibIts_ports/MapemSpatem_ports/MapemSpatemPort.cc

@@ -92,7 +92,8 @@ namespace LibItsMapemSpatem__TestSystem {
     
     float duration;
     loggers::get_instance().set_start_time(_time_key);
-    static_cast<MapemSpatemLayer *>(_layer)->sendMsg(send_par, _layer_params);
+    Params params;
+    static_cast<MapemSpatemLayer *>(_layer)->sendMsg(send_par, params);
     loggers::get_instance().set_stop_time(_time_key, duration);
   }
 
@@ -102,7 +103,8 @@ namespace LibItsMapemSpatem__TestSystem {
     
     float duration;
     loggers::get_instance().set_start_time(_time_key);
-    static_cast<MapemSpatemLayer *>(_layer)->sendMsg(send_par, _layer_params);
+    Params params;
+    static_cast<MapemSpatemLayer *>(_layer)->sendMsg(send_par, params);
     loggers::get_instance().set_stop_time(_time_key, duration);
   }
 

ccsrc/Ports/LibIts_ports/SremSsem_ports/SremSsemPort.cc

@@ -92,7 +92,8 @@ namespace LibItsSremSsem__TestSystem {
     
     float duration;
     loggers::get_instance().set_start_time(_time_key);
-    static_cast<SremSsemLayer *>(_layer)->sendMsg(send_par, _layer_params);
+    Params params;
+    static_cast<SremSsemLayer *>(_layer)->sendMsg(send_par, params);
     loggers::get_instance().set_stop_time(_time_key, duration);
   }
 
@@ -102,7 +103,8 @@ namespace LibItsSremSsem__TestSystem {
     
     float duration;
     loggers::get_instance().set_start_time(_time_key);
-    static_cast<SremSsemLayer *>(_layer)->sendMsg(send_par, _layer_params);
+    Params params;
+    static_cast<SremSsemLayer *>(_layer)->sendMsg(send_par, params);
     loggers::get_instance().set_stop_time(_time_key, duration);
   }
 

ccsrc/Protocols/GeoNetworking/GeoNetworkingLayer.cc

@@ -11,11 +11,13 @@
 #include "registration.hh"
 #include "loggers.hh"
 
+#include "security_services.hh"
+
 #include "converter.hh"
 
 using namespace LibItsGeoNetworking__TypesAndValues;
 
-GeoNetworkingLayer::GeoNetworkingLayer(const std::string & p_type, const std::string & param) : TLayer<LibItsGeoNetworking__TestSystem::GeoNetworkingPort>(p_type), _params(), _codec(), _beacon(nullptr), _gbc_packet(nullptr), _shb_packet(nullptr), _ls_reply(nullptr), _location_table(), _pass_beacon_table(), _device_mode{false}, _sendData(), _timerid{0}, _sev{0}, _its{0}, _freq_nanosecs(0), _mask{0}, _sa{0}, _sequence_number{0} {
+GeoNetworkingLayer::GeoNetworkingLayer(const std::string & p_type, const std::string & param) : TLayer<LibItsGeoNetworking__TestSystem::GeoNetworkingPort>(p_type), _params(), _codec(), _beacon(nullptr), _gbc_packet(nullptr), _shb_packet(nullptr), _ls_reply(nullptr), _location_table(), _pass_beacon_table(), _device_mode{false}, _secured_mode{false}, _enable_security_checks{false}, _sendData(), _timerid{0}, _sev{0}, _its{0}, _freq_nanosecs(0), _mask{0}, _sa{0}, _sequence_number{0} {
   loggers::get_instance().log(">>> GeoNetworkingLayer::GeoNetworkingLayer: %s, %s", to_string().c_str(), param.c_str());
   
   // Setup parameters
@@ -55,6 +57,14 @@ GeoNetworkingLayer::GeoNetworkingLayer(const std::string & p_type, const std::st
   if (it != _params.cend()) {
     _device_mode = (1 == converter::get_instance().string_to_int(it->second));
   }
+  it = _params.find(Params::secured_mode);
+  if (it != _params.cend()) {
+    _secured_mode = (1 == converter::get_instance().string_to_int(it->second));
+  }
+  it = _params.find(Params::enable_security_checks);
+  if (it != _params.cend()) {
+    _enable_security_checks = (1 == converter::get_instance().string_to_int(it->second));
+  }
   // Add broadcast address if needed
   it = _params.find("mac_bc");
   if (it == _params.cend()) {
@@ -63,6 +73,26 @@ GeoNetworkingLayer::GeoNetworkingLayer(const std::string & p_type, const std::st
   // Register this object for AdapterControlPort
   loggers::get_instance().log("GeoNetworkingLayer::GeoNetworkingLayer: register %s/%p", p_type.c_str(), this);
   registration<GeoNetworkingLayer>::get_instance().add_item(p_type, this);
+
+  // Set up default security parameters value
+  it = _params.find(Params::certificate);
+  if (it == _params.cend()) {
+    _params.insert(std::pair<std::string, std::string>(std::string("certificate"), "cert_ta"));
+  }
+  it = _params.find(Params::sec_db_path);
+  if (it == _params.cend()) {
+    _params.insert(std::pair<std::string, std::string>(std::string("sec_db_path"), ""));
+  }
+  it = _params.find(Params::hash);
+  if (it == _params.cend()) {
+    _params.insert(std::pair<std::string, std::string>(std::string("sec_sign"), "SHA-256"));
+  }
+  it = _params.find(Params::signature);
+  if (it == _params.cend()) {
+    _params.insert(std::pair<std::string, std::string>(std::string("signature"), "NISTP-256"));
+  }
+  // Set up security services
+  security_services::get_instance().setup(_params);
   
   Params::const_iterator i = _params.find(Params::beaconing);
   if ((i != _params.cend()) && (i->second.compare("1") == 0)) { // Immediate beaconing was requested
@@ -83,7 +113,6 @@ GeoNetworkingLayer::~GeoNetworkingLayer() {
   if (_timerid != 0) {
     timer_delete(_timerid);
   }
-
   if (_beacon != nullptr) {
     delete _beacon;
   }
@@ -111,13 +140,16 @@ void GeoNetworkingLayer::sendData(OCTETSTRING& data, Params& params) {
   loggers::get_instance().log_msg(">>> GeoNetworkingLayer::sendData: ", data);
   params.log();
 
-  // TODO Take into account the Security
   if (_device_mode) { // Need to build a GN packet
-    // TODO Use helper functions
     if (build_geonetworking_pdu(data, params) != 0) {
       return;
     }
   }
+  if (_secured_mode) {
+    if (build_secured_geonetworking_pdu(data, _params) != 0) {
+      return;
+    }
+  }
   
   // TODO To be removed
   while (_sendData.try_lock() == FALSE) {
@@ -131,12 +163,29 @@ void GeoNetworkingLayer::sendData(OCTETSTRING& data, Params& params) {
 
 void GeoNetworkingLayer::receiveData(OCTETSTRING& data, Params& params) { 
   loggers::get_instance().log_msg(">>> GeoNetworkingLayer::receiveData: ", data);
+
+  // Check security mode
+  LibItsGeoNetworking__TypesAndValues::BasicHeader basic_header;
+  decode_basic_header(data, basic_header);
+  if ((int)basic_header.nextHeader() == 2) { // Verify and extract the GeoNetworking Secured Packet as specified in ETSI EN 302 636-4-1 V1.3.1 (2017-08) Clause 9.6.1 Composition of the Basic Header
+    // Verify and extract the GeoNetworking Secured Packet as specified in ETSI EN 302 636-4-1 V1.3.1 (2017-08) Clause 9.4 GeoNetworking Secured Packet
+    OCTETSTRING unsecured_gn_payload;
+    if (security_services::get_instance().verify_and_extract_gn_payload(data, _enable_security_checks, unsecured_gn_payload, params) != 0) {
+      loggers::get_instance().warning("GeoNetworkingLayer::receiveData: security error");
+      if (_enable_security_checks) {
+        return;
+      }
+    }
+    // Update data
+    loggers::get_instance().log_msg("GeoNetworkingLayer::receiveData: Unsecured payload: ", unsecured_gn_payload);
+    data = OCTETSTRING(4, static_cast<const unsigned char*>(data)) + unsecured_gn_payload;
+  }
+  
   // Decode the payload
+  loggers::get_instance().log_msg("GeoNetworkingLayer::receiveData: Geonetworking payload to decode: ", data);
   LibItsGeoNetworking__TestSystem::GeoNetworkingInd ind;
   _codec.decode(data, ind.msgIn(), &params);
-  
-  // Update GeoNetworking layer
-  if (ind.msgIn().basicHeader().version() == 0) { // Non secured mode
+  if (ind.msgIn().is_bound()) {
     const LibItsGeoNetworking__TypesAndValues::LongPosVector* sopv = nullptr;
     const LibItsGeoNetworking__TypesAndValues::GnNonSecuredPacket& p = ind.msgIn().gnPacket().packet();
     const LibItsGeoNetworking__TypesAndValues::HeaderTST& htst = p.commonHeader().headerTST();
@@ -177,6 +226,12 @@ void GeoNetworkingLayer::receiveData(OCTETSTRING& data, Params& params) {
               // Encode GeoNetworking PDU
               OCTETSTRING os;
               _codec.encode(*_ls_reply, os);
+              // Apply security
+              if (_secured_mode) {
+                if (build_secured_geonetworking_pdu(data, _params) != 0) {
+                  return;
+                }
+              }
               // Send it
               // TODO To be removed
               while (_sendData.try_lock() == FALSE) {
@@ -224,7 +279,11 @@ void GeoNetworkingLayer::receiveData(OCTETSTRING& data, Params& params) {
         } // else, continue
       }
     } // else, continue
-  } // TODO else security mode, becarefull to duplicate code
+  } else {
+    // Inavlid GeoNetworking payload, discard it
+    loggers::get_instance().warning("GeoNetworkingLayer::receiveData: Failed to decode payload, discard it");
+    return;
+  }
 
   // Add lower layers parameters
   // 1. Destination MAC address
@@ -363,13 +422,19 @@ void GeoNetworkingLayer::send_beacon() {
   TTCN_Buffer encoding_buffer;
   _beacon->encode(*(_beacon->get_descriptor()), encoding_buffer, TTCN_EncDec::CT_RAW);
   OCTETSTRING data(encoding_buffer.get_len(), encoding_buffer.get_data());
+  // Apply security
+  Params params(_params);
+  if (_secured_mode) {
+    if (build_secured_geonetworking_pdu(data, _params) != 0) {
+      return;
+    }
+  }
   // Send it
   // TODO To be removed
   while (_sendData.try_lock() == FALSE) {
     // not ready yet
     std::this_thread::sleep_for(std::chrono::milliseconds(1));
   } // End of 'while' statement
-  Params params(_params);
   sendToAllLayers(data, params);
   _sendData.unlock();
   
@@ -763,4 +828,29 @@ int GeoNetworkingLayer::build_geonetworking_pdu(OCTETSTRING& data, Params& param
   return 0;
 }
 
+int GeoNetworkingLayer::build_secured_geonetworking_pdu(OCTETSTRING& data, Params& params) {
+  loggers::get_instance().log_msg(">>> GeoNetworkingLayer::build_secured_geonetworking_pdu: ", data);
+  
+  LibItsGeoNetworking__TypesAndValues::BasicHeader basic_header;
+  decode_basic_header(data, basic_header);
+  // Update security mode
+  basic_header.nextHeader() = BasicNextHeader::e__securedPacket;
+  OCTETSTRING unsecured_gn_payload = OCTETSTRING(data.lengthof() - 4, static_cast<const unsigned char*>(data) + 4);
+  OCTETSTRING secured_gn_payload;
+  return security_services::get_instance().secure_gn_payload(unsecured_gn_payload, false, secured_gn_payload, params); // TODO Add timer to check if certificate shall be sent
+}
+
+int GeoNetworkingLayer::decode_basic_header(const OCTETSTRING& p_data, LibItsGeoNetworking__TypesAndValues::BasicHeader& p_basic_header) {
+  loggers::get_instance().log_msg(">>> GeoNetworkingLayer::decode_basic_header: ", p_data);
+  
+  // Update security mode
+  OCTETSTRING bh = OCTETSTRING(4, static_cast<const unsigned char*>(p_data)); // Extract the basic header as specified in ETSI EN 302 636-4-1 V1.3.1 (2017-08) Clause 9.6
+  loggers::get_instance().log_msg("GeoNetworkingLayer::receiveData: bh: ", bh);
+  TTCN_Buffer decoding_buffer(bh);
+  p_basic_header.RAW_decode(*p_basic_header.get_descriptor(), decoding_buffer, decoding_buffer.get_len() * 8, raw_order_t::ORDER_MSB);
+  loggers::get_instance().log_msg("GeoNetworkingLayer::receiveData: decode_basic_header: ", p_basic_header);
+  
+  return 0;
+}
+
 GeoNetworkingLayerFactory GeoNetworkingLayerFactory::_f;

ccsrc/Protocols/GeoNetworking/GeoNetworkingLayer.hh

@@ -47,6 +47,8 @@ class GeoNetworkingLayer : public TLayer<LibItsGeoNetworking__TestSystem::GeoNet
   LocationTable _location_table;
   LocationTable _pass_beacon_table;
   bool _device_mode;
+  bool _secured_mode;
+  bool _enable_security_checks;
   std::mutex _sendData; // FIXME To be removed, useless
   timer_t _timerid;
   struct sigevent _sev;
@@ -103,16 +105,21 @@ public:
   virtual void sendData(OCTETSTRING& data, Params& params);
   virtual void receiveData(OCTETSTRING& data, Params& params);
   virtual OCTETSTRING trigger_ac_event(OCTETSTRING& data, Params& params);
-  
+
+  const LibItsGeoNetworking__TypesAndValues::LongPosVector* get_lpv(const LibItsGeoNetworking__TypesAndValues::GN__Address& p_gn_address);
   void start_beaconing();
   void start_beaconing(const LibItsGeoNetworking__TypesAndValues::GeoNetworkingPdu& p_beacon);
   void stop_beaconing();
-  void send_beacon();
   void start_pass_beaconing(const LibItsGeoNetworking__TypesAndValues::BeaconHeader& p_beacon);
   void stop_pass_beaconing();
-  const LibItsGeoNetworking__TypesAndValues::LongPosVector* get_lpv(const LibItsGeoNetworking__TypesAndValues::GN__Address& p_gn_address);
+  
+private:
+  void send_beacon();
 
   int build_geonetworking_pdu(OCTETSTRING& data, Params& params);
+  int build_secured_geonetworking_pdu(OCTETSTRING& data, Params& params);
 
+  int decode_basic_header(const OCTETSTRING& p_data, LibItsGeoNetworking__TypesAndValues::BasicHeader& p_basic_header);
+  
 }; // End of class GeoNetworkingLayer
 

ccsrc/Protocols/Security/EtsiTs103097Codec_ToBeSignedData.cc

+#include "EtsiTs103097Module.hh"
+
+#include "EtsiTs103097Codec_ToBeSignedData.hh"
+#include "asn1/asn_application.h" // from asn1c
+#include "asn1/EtsiTs103097Data.h"
+
+#include "loggers.hh"
+
+int EtsiTs103097Codec_ToBeSignedData::encode (const IEEE1609dot2::ToBeSignedData& p_cert, OCTETSTRING& data)
+{
+  loggers::get_instance().log(">>> EtsiTs103097Codec_ToBeSignedData::encode: %s", p_cert.get_descriptor()->name);
+  
+  BITSTRING b;
+  TTCN_EncDec::clear_error();
+  TTCN_Buffer buffer;
+  p_cert.encode(*p_cert.get_descriptor(), buffer, TTCN_EncDec::CT_OER);
+  data = OCTETSTRING(buffer.get_len(), buffer.get_data());
+  loggers::get_instance().log_msg("EtsiTs103097Codec_ToBeSignedData::encode: ", data);
+
+  return 0;
+}
+
+int EtsiTs103097Codec_ToBeSignedData::decode (const OCTETSTRING& data, IEEE1609dot2::ToBeSignedData& p_cert, Params* params)
+{
+  loggers::get_instance().log_msg(">>> EtsiTs103097Codec_ToBeSignedData::decode: ", data);
+
+  TTCN_EncDec::clear_error();
+  TTCN_Buffer decoding_buffer(data);
+  _params = params;
+  p_cert.decode(*p_cert.get_descriptor(), decoding_buffer, TTCN_EncDec::CT_OER);
+  
+  loggers::get_instance().log_msg("<<< EtsiTs103097Codec_ToBeSignedData::decode: ", (const Base_Type&)p_cert);
+  return 0;
+}
+
+// extern "C" {
+//   extern asn_TYPE_descriptor_t asn_DEF_EtsiTs103097;
+// }
+
+// int EtsiTs103097PDUCodec::encode (const IEEE1609dot2::ToBeSignedData& p_cert, BITSTRING& p_data)
+// {
+//   return _encode(IEEE1609dot2::ToBeSignedData_descr_, asn_DEF_EtsiTs103097Data, p_cert, p_data);
+// }
+
+// int EtsiTs103097PDUCodec::decode (const BITSTRING& p_data, IEEE1609dot2::ToBeSignedData& p_cert)
+// {
+//   return _decode(IEEE1609dot2::ToBeSignedData_descr_, asn_DEF_EtsiTs103097Data, p_data, p_cert);
+// }

ccsrc/Protocols/Security/EtsiTs103097Codec_ToBeSignedData.hh

+#ifndef EtsiTs103097CODEC_TBSDATA_H
+#define EtsiTs103097CODEC_TBSDATA_H
+
+#include "Codec.hh"
+//#include "CodecOer.hh"