GeoNetworkingLayer.cc

#include <thread>
#include <chrono>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/wait.h>

#include "GeoNetworkingTypes.hh"

#include "GeoNetworkingLayerFactory.hh"

#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}, _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
  Params::convert(_params, param);
  // Sanity checks
  INTEGER latitude;
  Params::const_iterator it = _params.find(Params::latitude);
  if (it != _params.cend()) {
    latitude = converter::get_instance().string_to_int(it->second);
  }
  INTEGER longitude;
  it = _params.find(Params::longitude);
  if (it != _params.cend()) {
    longitude = converter::get_instance().string_to_int(it->second);
  }
  OCTETSTRING ll_address;
  it = _params.find(Params::ll_address);
  if (it != _params.cend()) {
    ll_address = str2oct(CHARSTRING(it->second.c_str()));
  }
  INTEGER distanceA = 1000; // 1km
  it = _params.find(Params::distanceA);
  if (it != _params.cend()) {
    distanceA = converter::get_instance().string_to_int(it->second);
  }
  INTEGER distanceB = 1000; // 1Km
  it = _params.find(Params::distanceB);
  if (it != _params.cend()) {
    distanceB = converter::get_instance().string_to_int(it->second);
  }
  INTEGER angle = 0;
  it = _params.find(Params::angle);
  if (it != _params.cend()) {
    angle = converter::get_instance().string_to_int(it->second);
  }
  it = _params.find(Params::device_mode);
  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()) {
    _params.insert(std::pair<std::string, std::string>(std::string("mac_bc"), "FFFFFFFFFFFF"));
  }
  // 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 even if secured_mode is set to 0. Later, we can receive an AcEnableSecurity request, the sertificate caching will be ready to go
  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
    // Prepare beaconing operation
    fill_beacon(latitude, longitude, ll_address);
    start_beaconing();
  }

  // Fill packet templates
  fill_gbc_packet(latitude, longitude, ll_address, latitude, longitude, distanceA, distanceB, angle);// TODO Check if GeoBroadcastArea lat/lon are identical to lat/lon of the Test System
  fill_shb_packet(latitude, longitude, ll_address);
  fill_ls_reply(latitude, longitude, ll_address);
} // End of constructor

GeoNetworkingLayer::~GeoNetworkingLayer() {
  loggers::get_instance().log(">>> GeoNetworkingLayer::~GeoNetworkingLayer");

  if (_timerid != 0) {
    timer_delete(_timerid);
  }
  if (_beacon != nullptr) {
    delete _beacon;
  }
  if (_gbc_packet != nullptr) {
    delete _gbc_packet;
  }
  if (_shb_packet != nullptr) {
    delete _shb_packet;
  }
  if (_ls_reply != nullptr) {
    delete _ls_reply;
  }
} // End of destructor

void GeoNetworkingLayer::sendMsg(const LibItsGeoNetworking__TestSystem::GeoNetworkingReq& p, Params& params) {
  loggers::get_instance().log(">>> GeoNetworkingLayer::sendMsg");

  // Encode GeoNetworking PDU
  OCTETSTRING data;
  _codec.encode(p.msgOut(), data);
  sendData(data, params);
}

void GeoNetworkingLayer::sendData(OCTETSTRING& data, Params& params) {
  loggers::get_instance().log_msg(">>> GeoNetworkingLayer::sendData: ", data);
  params.log();

  if (_device_mode) { // Need to build a GN packet
    if (build_geonetworking_pdu(data, params) != 0) {
      return;
    }
  }
  if (_secured_mode) {
    if (build_secured_pdu(data, _params) != 0) {
      return;
    }
  }
  
  // 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
  sendToAllLayers(data, params);
  _sendData.unlock();
  loggers::get_instance().log("<<< GeoNetworkingLayer::sendData");
}

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
    unsigned int basic_header_len = 4;// FIXME How to retrive the BasicHeader length basic_header.get_descriptor()->raw->fieldlength / 8;
    loggers::get_instance().log("GeoNetworkingLayer::receiveData: basic_header_len = %d", basic_header_len);
    // 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;
    OCTETSTRING secured_data = OCTETSTRING(data.lengthof() - basic_header_len, static_cast<const unsigned char*>(data) + basic_header_len);
    if (security_services::get_instance().verify_and_extract_gn_payload(secured_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(basic_header_len, 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);
  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();
    if (p.extendedHeader().ispresent()) { // Update location table
      const LibItsGeoNetworking__TypesAndValues::ExtendedHeader& ex = p.extendedHeader();
      if (htst.ischosen(LibItsGeoNetworking__TypesAndValues::HeaderTST::ALT_beaconHdr)) { // Receive a beacon
        sopv = &ex.beaconHeader().srcPosVector();
      } else if (htst.ischosen(LibItsGeoNetworking__TypesAndValues::HeaderTST::ALT_tsbHdr)) { // Receive a topologicallyScopeBroadcast
        if (ex.ischosen(LibItsGeoNetworking__TypesAndValues::ExtendedHeader::ALT_tsbHeader)) {
          sopv = &ex.tsbHeader().srcPosVector();
        } else {
          sopv = &ex.shbHeader().srcPosVector();
        }
      } else if (htst.ischosen(LibItsGeoNetworking__TypesAndValues::HeaderTST::ALT_geoBroadcastHdr)) {
        sopv = &ex.geoBroadcastHeader().srcPosVector();
      } else if (htst.ischosen(LibItsGeoNetworking__TypesAndValues::HeaderTST::ALT_lsHdr)) { // Receive a location service
        if (ex.ischosen(LibItsGeoNetworking__TypesAndValues::ExtendedHeader::ALT_lsRequestHeader)) { // Receive a LocationService/LsRequest
          sopv = &ex.lsRequestHeader().srcPosVector();
          // TODO Send LsReply if we are not in context of GN ATS in case of non GN test suite
          if (_device_mode) {
            // Update _ls_reply
            ExtendedHeader* eh = static_cast<ExtendedHeader *>(_ls_reply->gnPacket().packet().extendedHeader().get_opt_value());
            if (eh != nullptr) {
              // Update sequence number
              eh->lsReplyHeader().seqNumber() = _sequence_number++;
              // Update destination
              eh->lsReplyHeader().dstPosVector().gnAddr() = sopv->gnAddr();
              eh->lsReplyHeader().dstPosVector().latitude() = sopv->latitude();
              eh->lsReplyHeader().dstPosVector().longitude() = sopv->longitude();
              // Update 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
              eh->lsReplyHeader().srcPosVector().timestamp__().set_long_long_val(ms);
              eh->lsReplyHeader().dstPosVector().timestamp__() = eh->lsReplyHeader().srcPosVector().timestamp__();
              
              loggers::get_instance().log_msg("GeoNetworkingLayer::receiveData: ", *_ls_reply);
              
              // send it
              // Encode GeoNetworking PDU
              OCTETSTRING os;
              _codec.encode(*_ls_reply, os);
              // Apply security
              if (_secured_mode) {
                if (build_secured_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
              sendToAllLayers(os, params);
              _sendData.unlock();
            } else {
              loggers::get_instance().error("GeoNetworkingLayer::sendData: Wrong cast");
              return;
            }
          }
        } else {
          sopv = &ex.lsReplyHeader().srcPosVector();
        }
      } else if (htst.ischosen(LibItsGeoNetworking__TypesAndValues::HeaderTST::ALT_geoAnycastHdr)) { // Receive a GeoAnycast
        sopv = &ex.geoAnycastHeader().srcPosVector();
      } else if (htst.ischosen(LibItsGeoNetworking__TypesAndValues::HeaderTST::ALT_geoUnicastHdr)) {
        sopv = &ex.geoUnicastHeader().srcPosVector();
      } else if (htst.ischosen(LibItsGeoNetworking__TypesAndValues::HeaderTST::ALT_saHdr)) { // Receive Service Advertisement
        if (ex.ischosen(LibItsGeoNetworking__TypesAndValues::ExtendedHeader::ALT_tsbHeader)) { // TODO Check that Service Advertisment (ETSI TS 102 890-1) uses TSB
          sopv = &ex.tsbHeader().srcPosVector();
        } else {
          sopv = &ex.tsbHeader().srcPosVector();
        }
      } // else, nothing to do
      loggers::get_instance().log("GeoNetworkingLayer::receiveData: sopv is boud: %d", sopv->is_bound());
      if(sopv->is_bound()) {
        const LibItsGeoNetworking__TypesAndValues::LongPosVector& lpv = *sopv;
        _location_table.add_entry(lpv);
      }
    }
    
    // By default incoming beacons are filtered by the test adapter
    if (htst.ischosen(LibItsGeoNetworking__TypesAndValues::HeaderTST::ALT_beaconHdr)) {
      loggers::get_instance().log_msg("GeoNetworkingLayer::receiveData: Pass beaconing filtering", sopv->gnAddr().mid());
      if (_pass_beacon_table.empty()) { // Discard beacon
        loggers::get_instance().log("GeoNetworkingLayer::receiveData: Pass beaconing table empty, discard it");
        return;
      } else { // Check beacon filter for StartPassBeaconing/Stop
        if (!_pass_beacon_table.has_entry(sopv->gnAddr().mid())) { // Discard beacon
          loggers::get_instance().log_msg("GeoNetworkingLayer::receiveData: Not in pass beaconing table, discard it", *sopv);
          return;
        } // else, continue
      }
    } // else, continue
  } 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
  Params::const_iterator it = params.find(Params::mac_dst);
  if (it != params.cend()) {
    loggers::get_instance().log("GeoNetworkingLayer::receiveData: dst=%s", it->second.c_str());
    ind.macDestinationAddress() = str2oct(CHARSTRING(it->second.c_str()));
  } else {
    ind.macDestinationAddress() = str2oct(CHARSTRING(_params["mac_bc"].c_str()));
  }
  // 2. ssp
  it = params.find(Params::ssp);
  if (it != params.cend()) {
    loggers::get_instance().log("GeoNetworkingLayer::receiveData: ssp=%s", it->second.c_str());
    ind.ssp() = str2bit(CHARSTRING(it->second.c_str()));
  } else {
    ind.ssp().set_to_omit();
  }
  // 3. its_aid
  it = params.find(Params::its_aid);
  if (it != params.cend()) {
    loggers::get_instance().log("GeoNetworkingLayer::receiveData: its_aid=%s", it->second.c_str());
    ind.its__aid() = std::stoi(it->second.c_str());
  } else {
    ind.its__aid().set_to_omit();
  }
  
  // Pass the GeoNetworking raw payload to the upper layers if any
  it = params.find(Params::gn_payload);
  if (it != params.cend()) {
    loggers::get_instance().log("GeoNetworkingLayer::receiveData: gn_payload=%s", it->second.c_str());
    OCTETSTRING os(str2oct(CHARSTRING(it->second.c_str())));
    receiveToAllLayers(os, params);
  } else {
    loggers::get_instance().warning("GeoNetworkingLayer::receiveData: No payload to pass to upper layers");
  }
  
  // Pass it to the ports
  toAllUpperPorts(ind, params);
}

OCTETSTRING GeoNetworkingLayer::trigger_ac_event(OCTETSTRING& data, Params& params)
{
  loggers::get_instance().log_to_hexa(">>> GeoNetworkingLayer::trigger_ac_event: ", data);


  return int2oct(0, 2);  
} // End of trigger_ac_event method

void GeoNetworkingLayer::start_beaconing() {
  loggers::get_instance().log(">>> GeoNetworkingLayer::start_beaconing");
  //loggers::get_instance().log_msg("GeoNetworkingLayer::start_beaconing: _beacon=", *_beacon);

  // Establish handler for timer signal
  loggers::get_instance().log("GeoNetworkingLayer::start_beaconing: Establishing handler for signal %d\n", _signal_id);
  _sa.sa_flags = SA_SIGINFO;
  _sa.sa_sigaction = timer_irq_sigalrm_handler;
  sigemptyset(&_sa.sa_mask);
  if (sigaction(_signal_id, &_sa, nullptr) == -1) {
    loggers::get_instance().error("GeoNetworkingLayer::start_beaconing: Sigaction failure: %d", errno);
  }
  // Block timer signal temporarily
  loggers::get_instance().log("GeoNetworkingLayer::start_beaconing: Blocking signal %d\n", _signal_id);
  sigemptyset(&_mask);
  sigaddset(&_mask, _signal_id);
  if (sigprocmask(SIG_SETMASK, &_mask, nullptr) == -1) {
    loggers::get_instance().error("GeoNetworkingLayer::start_beaconing: Sigprocmask failure: %d", errno);
  }	
  // Create the timer 
  _sev.sigev_notify = SIGEV_SIGNAL;
  _sev.sigev_signo = _signal_id; // Use signal alarm
  _sev.sigev_value.sival_ptr = this; // The GeoNetworkingLayer object address
  if (timer_create(CLOCK_REALTIME, &_sev, &_timerid) == -1) {
    loggers::get_instance().error("GeoNetworkingLayer::start_beaconing: Timer failure: %d", errno);
  }
  loggers::get_instance().log("GeoNetworkingLayer::start_beaconing: timer ID is 0x%x\n", (long)_timerid);
  // Start the timer
  unsigned int expiry = 1000; // Default expiry time 1000ms
  Params::const_iterator i = _params.find("expiry");
  if (i != _params.cend()) {
    expiry = static_cast<unsigned int>(std::strtoul(i->second.c_str(), nullptr, 10));
  }
  _freq_nanosecs = expiry * 1000000;
  _its.it_value.tv_sec = _freq_nanosecs / 1000000000;
  _its.it_value.tv_nsec = _freq_nanosecs % 1000000000;
  _its.it_interval.tv_sec = _its.it_value.tv_sec;
  _its.it_interval.tv_nsec = _its.it_value.tv_nsec;
  if (timer_settime(_timerid, 0, &_its, nullptr) == -1) {
    loggers::get_instance().error("GeoNetworkingLayer::start_beaconing: Sigprocmask failure: %d", errno);
  }
  // Unlock the timer signal, so that timer notification can be delivered
  loggers::get_instance().log("GeoNetworkingLayer::start_beaconing: Unblocking signal %d\n", _signal_id);
  if (sigprocmask(SIG_UNBLOCK, &_mask, nullptr) == -1) {
    loggers::get_instance().error("GeoNetworkingLayer::start_beaconing: Sigprocmask failure: %d", errno);
  }
} // End of start_beaconing method

void GeoNetworkingLayer::start_beaconing(const LibItsGeoNetworking__TypesAndValues::GeoNetworkingPdu& p_beacon) {
  loggers::get_instance().log_msg(">>> GeoNetworkingLayer::start_beaconing", p_beacon);

  // Initialize the beacon
  if (_beacon != nullptr) {
    delete _beacon;
  }
  _beacon = new LibItsGeoNetworking__TypesAndValues::GeoNetworkingPdu(p_beacon);

  start_beaconing(); // TODO Refined adding a boolean return code
} // End of start_beaconing method

void GeoNetworkingLayer::stop_beaconing() {
  loggers::get_instance().log(">>> GeoNetworkingLayer::stop_beaconing");
  
  // Block timer signal temporarily
  loggers::get_instance().log("GeoNetworkingLayer::stop_beaconing: Blocking signal %d\n", _signal_id);
  sigemptyset(&_mask);
  sigaddset(&_mask, _signal_id);
  if (sigprocmask(SIG_SETMASK, &_mask, nullptr) == -1) {
    loggers::get_instance().error("GeoNetworkingLayer::stop_beaconing: Sigprocmask failure: %d", errno);
  }	
  timer_delete(_timerid);
  _timerid = 0;
} // End of stop_beaconing method

void GeoNetworkingLayer::send_beacon() {
  loggers::get_instance().log(">>> GeoNetworkingLayer::send_beacon");
  
  ExtendedHeader* eh = static_cast<ExtendedHeader *>(_beacon->gnPacket().packet().extendedHeader().get_opt_value());
  if (eh == nullptr) {
    loggers::get_instance().error("GeoNetworkingLayer::send_beacon: Wrong cast");
  }
  // Update 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_time() & its_time_mod()
  eh->beaconHeader().srcPosVector().timestamp__().set_long_long_val(static_cast<unsigned int>(ms));
  //loggers::get_instance().log_msg("GeoNetworkingLayer::send_beacon: ", *_beacon);
  // Encode message using TITAN because of payload in omited
  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_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
  sendToAllLayers(data, params);
  _sendData.unlock();
  
  //loggers::get_instance().log("<<< GeoNetworkingLayer::send_beacon");
} // End of send_beacon method

void GeoNetworkingLayer::start_pass_beaconing(const LibItsGeoNetworking__TypesAndValues::BeaconHeader& p_beacon) {
  loggers::get_instance().log_msg(">>> GeoNetworkingLayer::start_pass_beaconing", p_beacon);

  const LibItsGeoNetworking__TypesAndValues::LongPosVector& lpv = p_beacon.srcPosVector();
  if (!_pass_beacon_table.has_entry(lpv.gnAddr().mid())) {
    _pass_beacon_table.add_entry(lpv);
  } // TODO Refined adding a boolean return code
} // End of start_pass_beaconing method

void GeoNetworkingLayer::stop_pass_beaconing() {
  loggers::get_instance().log(">>> GeoNetworkingLayer::stop_pass_beaconing");

  _pass_beacon_table.reset();
} // End of stop_pass_beaconing method

const LongPosVector* GeoNetworkingLayer::get_lpv(const GN__Address& p_gn_address)
{
  loggers::get_instance().log_msg(">>> GeoNetworkingLayer::get_lpv", p_gn_address);
  
  const LongPosVector* lpv = nullptr;
  if (_location_table.has_entry(p_gn_address.mid())) {
    lpv = _location_table.get_entry(p_gn_address.mid());
  }
  return lpv;
} // End of get_lpv

void GeoNetworkingLayer::fill_beacon(INTEGER& p_latitude, INTEGER& p_longitude, OCTETSTRING& p_ll_address)
{
  _beacon = new GeoNetworkingPdu();
  HeaderTST h;
  h.beaconHdr() = BeaconHeaderType(
                                   HeaderType(HeaderType::e__beacon),
                                   0
                                   );
  ExtendedHeader eh;
  eh.beaconHeader() = BeaconHeader(
                                   LongPosVector(
                                                 GN__Address(
                                                             TypeOfAddress(TypeOfAddress::e__manual), // TODO Use Params
                                                             StationType(StationType::e__roadSideUnit), // TODO Use Params
                                                             33,
                                                             p_ll_address
                                                             ),
                                                 0,
                                                 p_latitude,
                                                 p_longitude,
                                                 int2bit(0, 1),
                                                 0,
                                                 0
                                                 )
                                   );
  _beacon->basicHeader() = BasicHeader(
                                       0,
                                       BasicNextHeader(
                                                       BasicNextHeader::e__commonHeader
                                                       ),
                                       0,
                                       Lifetime(
                                                4,
                                                LtBase(LtBase::e__50ms)
                                                ),
                                       5
                                       );
  _beacon->gnPacket().packet() = GnNonSecuredPacket(
                                                    CommonHeader(
                                                                 NextHeader(
                                                                            NextHeader::e__any
                                                                            ),
                                                                 0,
                                                                 h,
                                                                 TrafficClass(
                                                                              SCF(SCF::e__scfDisabled),
                                                                              ChannelOffload(ChannelOffload::e__choffDisabled),
                                                                              0
                                                                              ),
                                                                 int2bit(0, 8),
                                                                 0,
                                                                 1,
                                                                 0
                                                                 ),
                                                    OPTIONAL<ExtendedHeader>(eh),
                                                    OPTIONAL<GnRawPayload>()
                                                    );
  _beacon->gnPacket().packet().payload().set_to_omit();
  _beacon->gnPacket().securedMsg().set_to_omit();
  //loggers::get_instance().log_msg("GeoNetworkingLayer::fill_beacon: beacon value: ", *_beacon);
} // End of fill_beacon method
  
void GeoNetworkingLayer::fill_gbc_packet(const INTEGER& p_latitude, const INTEGER& p_longitude, const OCTETSTRING& p_ll_address, const INTEGER& p_geoAreaPosLatitude, const INTEGER& p_geoAreaPosLongitude, const INTEGER& p_distanceA, const INTEGER& p_distanceB, const INTEGER& p_angle)
{
  _gbc_packet = new GeoNetworkingPdu();
  HeaderTST h;
  h.geoBroadcastHdr() = GeoBroadcastHeaderType(
                                               HeaderType(HeaderType::e__geoBroadcast),
                                               HeaderSubTypeGeoBroadcast(HeaderSubTypeGeoBroadcast::e__geoBroadcastElip)
                                               );
  ExtendedHeader eh;
  eh.geoBroadcastHeader() = GeoAnycastHeader( // GeoBradcastHeader is identical as GeoAnycastHeader
                                             0,
                                             0,
                                             LongPosVector(
                                                           GN__Address(
                                                                       TypeOfAddress(TypeOfAddress::e__manual), // TODO Use Params
                                                                       StationType(StationType::e__passengerCar), // TODO Use Params
                                                                       33,
                                                                       p_ll_address
                                                                       ),
                                                           0,
                                                           p_latitude,
                                                           p_longitude,
                                                           int2bit(1, 1), // PAI
                                                           0,
                                                           0
                                                           ),
                                             p_geoAreaPosLatitude,
                                             p_geoAreaPosLongitude,
                                             p_distanceA,
                                             p_distanceB,
                                             p_angle,
                                             0
                                              );
  _gbc_packet->basicHeader() = BasicHeader(
                                           0,
                                           BasicNextHeader(
                                                           BasicNextHeader::e__commonHeader
                                                           ),
                                           0,
                                           Lifetime(
                                                    4,
                                                    LtBase(LtBase::e__50ms)
                                                    ),
                                           5
                                           );
  _gbc_packet->gnPacket().packet() = GnNonSecuredPacket(
                                                        CommonHeader(
                                                                     NextHeader(
                                                                                NextHeader::e__btpA
                                                                                ),
                                                                     0,
                                                                     h,
                                                                     TrafficClass(
                                                                                  SCF(SCF::e__scfDisabled),
                                                                                  ChannelOffload(ChannelOffload::e__choffDisabled),
                                                                                  0
                                                                                  ),
                                                                     int2bit(128, 8), // Mobile stationnary flag set
                                                                     0,
                                                                     5,
                                                                     0
                                                                     ),
                                                        OPTIONAL<ExtendedHeader>(eh),
                                                        OPTIONAL<GnRawPayload>()
                                                        );
  _gbc_packet->gnPacket().packet().payload().set_to_omit();
  _gbc_packet->gnPacket().securedMsg().set_to_omit();
  //loggers::get_instance().log_msg("GeoNetworkingLayer::fill_gbc_packet: packet value: ", *_gbc_packet);
} // End of fill_gbc_packet method

void GeoNetworkingLayer::fill_shb_packet(const INTEGER& p_latitude, const INTEGER& p_longitude, const OCTETSTRING& p_ll_address)
{
  _shb_packet = new GeoNetworkingPdu();
  HeaderTST h;
  h.tsbHdr() = TsbHeaderType(
                             HeaderType(HeaderType::e__topologicallyScopedBroadcast),
                             HeaderSubTypeTSB(HeaderSubTypeTSB::e__singleHop)
                             );
  ExtendedHeader eh;
  eh.shbHeader() = SHBHeader(
                             LongPosVector(
                                           GN__Address(
                                                       TypeOfAddress(TypeOfAddress::e__manual), // TODO Use Params
                                                       StationType(StationType::e__passengerCar), // TODO Use Params
                                                       33,
                                                       p_ll_address
                                                       ),
                                           0,
                                           p_latitude,
                                           p_longitude,
                                           int2bit(1, 1), // PAI
                                           0,
                                           0
                                           ),
                             0
                             );
  _shb_packet->basicHeader() = BasicHeader(
                                           0,
                                           BasicNextHeader(
                                                           BasicNextHeader::e__commonHeader
                                                           ),
                                           0,
                                           Lifetime(
                                                    4,
                                                    LtBase(LtBase::e__50ms)
                                                    ),
                                           1
                                           );
  _shb_packet->gnPacket().packet() = GnNonSecuredPacket(
                                                        CommonHeader(
                                                                     NextHeader(
                                                                                NextHeader::e__btpA
                                                                                ),
                                                                     0,
                                                                     h,
                                                                     TrafficClass(
                                                                                  SCF(SCF::e__scfDisabled),
                                                                                  ChannelOffload(ChannelOffload::e__choffDisabled),
                                                                                  0
                                                                                  ),
                                                                     int2bit(128, 8), // Mobile stationnary flag set
                                                                     0,
                                                                     1,
                                                                     0
                                                                     ),
                                                        OPTIONAL<ExtendedHeader>(eh),
                                                        OPTIONAL<GnRawPayload>()
                                                        );
  _shb_packet->gnPacket().packet().payload().set_to_omit();
  _shb_packet->gnPacket().securedMsg().set_to_omit();
  //loggers::get_instance().log_msg("GeoNetworkingLayer::fill_shb_packet: packet value: ", *_shb_packet);
} // End of fill_shb_packet method

void GeoNetworkingLayer::fill_ls_reply(const INTEGER& p_latitude, const INTEGER& p_longitude, const OCTETSTRING& p_ll_address)
{
  _ls_reply = new GeoNetworkingPdu();
  HeaderTST h;
  h.lsHdr() = LsHeaderType(
                           HeaderType(HeaderType::e__locationService),
                           HeaderSubTypeLs(HeaderSubTypeLs::e__lsReply)
                           );
  ExtendedHeader eh;
  eh.lsReplyHeader() = LSReplyHeader(
                                     0,
                                     0,
                                     LongPosVector(
                                                   GN__Address(
                                                               TypeOfAddress(TypeOfAddress::e__manual), // TODO Use Params
                                                               StationType(StationType::e__passengerCar), // TODO Use Params
                                                               33,
                                                               p_ll_address
                                                               ),
                                                   0,
                                                   p_latitude,
                                                   p_longitude,
                                                   int2bit(1, 1), // PAI
                                                   0,
                                                   0
                                                   ),
                                     ShortPosVector(
                                                    GN__Address(
                                                                TypeOfAddress(TypeOfAddress::e__manual), // TODO Use Params
                                                                StationType(StationType::e__passengerCar), // TODO Use Params
                                                                33,
                                                                p_ll_address
                                                                ),
                                                    0,
                                                    p_latitude,
                                                    p_longitude
                                                    )
                                     );
  _ls_reply->basicHeader() = BasicHeader(
                                         0,
                                         BasicNextHeader(
                                                         BasicNextHeader::e__commonHeader
                                                         ),
                                         0,
                                         Lifetime(
                                                  4,
                                                  LtBase(LtBase::e__50ms)
                                                  ),
                                         5
                                         );
  _ls_reply->gnPacket().packet() = GnNonSecuredPacket(
                                                      CommonHeader(
                                                                   NextHeader(
                                                                              NextHeader::e__any
                                                                              ),
                                                                   0,
                                                                   h,
                                                                   TrafficClass(
                                                                                SCF(SCF::e__scfDisabled),
                                                                                ChannelOffload(ChannelOffload::e__choffDisabled),
                                                                                0
                                                                                ),
                                                                   int2bit(128, 8), // Mobile stationnary flag set
                                                                   0,
                                                                   5,
                                                                   0
                                                                   ),
                                                      OPTIONAL<ExtendedHeader>(eh),
                                                      OPTIONAL<GnRawPayload>()
                                                      );
  _ls_reply->gnPacket().packet().payload().set_to_omit();
  _ls_reply->gnPacket().securedMsg().set_to_omit();
  //loggers::get_instance().log_msg("GeoNetworkingLayer::fill_ls_reply: packet value: ", *_ls_reply);
} // End of fill_ls_reply method

void GeoNetworkingLayer::timer_irq_sigalrm_handler(int p_signal, siginfo_t *p_signal_info, void *p_uc) {
  //loggers::get_instance().log(">>> GeoNetworkingLayer::timer_irq_sigalrm_handler: Caught signal %d", p_signal);

  static_cast<GeoNetworkingLayer *>(p_signal_info->si_value.sival_ptr)->send_beacon();
} // End of method timer_irq_sigalrm_handler

int GeoNetworkingLayer::build_geonetworking_pdu(OCTETSTRING& data, Params& params) {
  loggers::get_instance().log(">>> GeoNetworkingLayer::build_geonetworking_pdu");
  params.log();
  
  std::string next_header;
  Params::const_iterator it = params.find(Params::next_header);
  if (it != params.cend()) {
    next_header = it->second.c_str();
  }
  std::string header_type;
  it = params.find(Params::header_type);
  if (it != params.cend()) {
    header_type = it->second.c_str();
  }
  std::string header_sub_type;
  it = params.find(Params::header_sub_type);
  if (it != params.cend()) {
    header_sub_type = it->second.c_str();
  }
  loggers::get_instance().log("GeoNetworkingLayer::build_geonetworking_pdu: %s, %s, %s", next_header.c_str(), header_type.c_str(), header_sub_type.c_str());
  
  if (header_type.compare("tsb") == 0) {
    if (header_sub_type.compare("sh") == 0) { // Use SHB
      ExtendedHeader* eh = static_cast<ExtendedHeader *>(_shb_packet->gnPacket().packet().extendedHeader().get_opt_value());
      if (eh == nullptr) {
        loggers::get_instance().error("GeoNetworkingLayer::build_geonetworking_pdu: Wrong cast");
        return -1;
      }
      // Update NextHeader
      it = params.find(Params::next_header);
      if (next_header.compare("btpB") == 0) {
        _shb_packet->gnPacket().packet().commonHeader().nextHeader() = NextHeader::e__btpB;
      } else { // Default btp is btpA
        _shb_packet->gnPacket().packet().commonHeader().nextHeader() = NextHeader::e__btpA;
      }
      // Update payload
      _shb_packet->gnPacket().packet().commonHeader().plLength() = data.lengthof();
      _shb_packet->gnPacket().packet().payload() = OPTIONAL<OCTETSTRING>(data);
      // Update 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()
      eh->shbHeader().srcPosVector().timestamp__().set_long_long_val(static_cast<unsigned int>(ms));
      
      loggers::get_instance().log_msg("GeoNetworkingLayer::build_geonetworking_pdu: ", *_shb_packet);
      // Encode GeoNetworking PDU
      OCTETSTRING os;
      _codec.encode(*_shb_packet, os);
      data = os;
    } else { // TODO Use TSB
      loggers::get_instance().error("GeoNetworkingLayer::build_geonetworking_pdu: Not implemented");
      return -2;
    }
  } else { // TODO To be continued
    // Default: Use GBC
    ExtendedHeader* eh = static_cast<ExtendedHeader *>(_shb_packet->gnPacket().packet().extendedHeader().get_opt_value());
    if (eh == nullptr) {
      loggers::get_instance().error("GeoNetworkingLayer::build_geonetworking_pdu: Wrong cast");
      return -1;
    }
    // Update NextHeader
    it = params.find(Params::next_header);
    if (next_header.compare("btpB") == 0) {
      _gbc_packet->gnPacket().packet().commonHeader().nextHeader() = NextHeader::e__btpB;
    } else { // Default btp is btpA
      _gbc_packet->gnPacket().packet().commonHeader().nextHeader() = NextHeader::e__btpA;
    }
    // Update sequence number
    eh->geoBroadcastHeader().seqNumber() = _sequence_number++;
    // Update payload
    _gbc_packet->gnPacket().packet().commonHeader().plLength() = data.lengthof();
    _gbc_packet->gnPacket().packet().payload() = OPTIONAL<OCTETSTRING>(data);
    // Update 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()
    eh->geoBroadcastHeader().srcPosVector().timestamp__().set_long_long_val(static_cast<unsigned int>(ms));
    
    loggers::get_instance().log_msg("GeoNetworkingLayer::build_geonetworking_pdu: ", *_gbc_packet);

    // Encode GeoNetworking PDU
    OCTETSTRING os;
    _codec.encode(*_gbc_packet, os);
    data = os;
  }
  
  return 0;
}

int GeoNetworkingLayer::build_secured_pdu(OCTETSTRING& data, Params& params) {
  loggers::get_instance().log_msg(">>> GeoNetworkingLayer::build_secured_pdu: ", data);
  
  LibItsGeoNetworking__TypesAndValues::BasicHeader basic_header;
  decode_basic_header(data, basic_header);
  // Update security mode
  unsigned int basic_header_len = 4;// FIXME How to retrive the BasicHeader length basic_header.get_descriptor()->raw->fieldlength / 8;
  loggers::get_instance().log("GeoNetworkingLayer::build_secured_pdu: basic_header_len = %d", basic_header_len);
  basic_header.nextHeader() = BasicNextHeader::e__securedPacket;
  OCTETSTRING unsecured_gn_payload = OCTETSTRING(data.lengthof() - basic_header_len, static_cast<const unsigned char*>(data) + basic_header_len);
  OCTETSTRING secured_gn_payload;
  if (security_services::get_instance().secure_gn_payload(unsecured_gn_payload, false, secured_gn_payload, params) != 0) {
    loggers::get_instance().warning("GeoNetworkingLayer::build_secured_pdu: failed to build secured pdu");
    return -1;
  }
  // TODO Add timer to check if certificate shall be sent

  // Encode the basid header
  //loggers::get_instance().log_msg("GeoNetworkingLayer::build_secured_pdu: New basic_header = ", basic_header);
  RAW_enc_tr_pos rp;
  rp.level=0;
  rp.pos=NULL;
  RAW_enc_tree enc_tree(FALSE, NULL, &rp, 1, basic_header.get_descriptor()->raw);
  basic_header.RAW_encode(*basic_header.get_descriptor(), enc_tree);
  TTCN_Buffer encoding_buffer;
  enc_tree.put_to_buf(encoding_buffer);
  // Copy result
  data =
    OCTETSTRING(encoding_buffer.get_len(), encoding_buffer.get_data()) +
    secured_gn_payload;
  loggers::get_instance().log_msg("GeoNetworkingLayer::build_secured_pdu: Secured pdu = ", data);

  return 0;
}

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::decode_basic_header: ", p_basic_header);
  
  return 0;
}

GeoNetworkingLayerFactory GeoNetworkingLayerFactory::_f;