geonetworking_layer.cc 53.5 KB
Newer Older
1
2
3
4
5
6
#include <thread>
#include <chrono>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/wait.h>

garciay's avatar
garciay committed
7
#include "geonetworking_types.hh"
8

garciay's avatar
garciay committed
9
#include "geonetworking_layer_factory.hh"
10

11
12
13
#include "registration.hh"
#include "loggers.hh"

garciay's avatar
garciay committed
14
15
#include "security_services.hh"

16
17
#include "converter.hh"

18
19
using namespace LibItsGeoNetworking__TypesAndValues;

garciay's avatar
garciay committed
20
21
geonetworking_layer::geonetworking_layer(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), _tsb_packet(nullptr), _ls_reply(nullptr), _location_table(), _pass_beacon_table(), _device_mode{false}, _secured_mode{false}, _encrypted_mode{false}, _enable_security_checks{false}, _sendData(), _timerid{0}, _sev{0}, _its{0}, _freq_nanosecs(0), _mask{0}, _sa{0}, _sequence_number{0}, _latitude{0}, _longitude{0} {
  loggers::get_instance().log(">>> geonetworking_layer::geonetworking_layer: %s, %s", to_string().c_str(), param.c_str());
22
23
24
25
26
27
  
  // Setup parameters
  Params::convert(_params, param);
  // Sanity checks
  Params::const_iterator it = _params.find(Params::latitude);
  if (it != _params.cend()) {
28
    _latitude = converter::get_instance().string_to_int(it->second);
29
30
31
  }
  it = _params.find(Params::longitude);
  if (it != _params.cend()) {
32
    _longitude = converter::get_instance().string_to_int(it->second);
33
34
35
36
37
38
39
40
41
  }
  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()) {
42
    distanceA = converter::get_instance().string_to_int(it->second);
43
44
45
46
  }
  INTEGER distanceB = 1000; // 1Km
  it = _params.find(Params::distanceB);
  if (it != _params.cend()) {
47
    distanceB = converter::get_instance().string_to_int(it->second);
48
49
50
51
  }
  INTEGER angle = 0;
  it = _params.find(Params::angle);
  if (it != _params.cend()) {
52
    angle = converter::get_instance().string_to_int(it->second);
53
  }
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
  INTEGER station_type = 5; // passangerCar
  it = _params.find(Params::station_type);
  if (it != _params.cend()) {
    station_type = converter::get_instance().string_to_int(it->second);
  }
  INTEGER country = 0;
  it = _params.find(Params::country);
  if (it != _params.cend()) {
    country = converter::get_instance().string_to_int(it->second);
  }
  INTEGER type_of_address = 1; // Manual
  it = _params.find(Params::type_of_address);
  if (it != _params.cend()) {
    type_of_address = converter::get_instance().string_to_int(it->second);
  }
  
70
71
  it = _params.find(Params::device_mode);
  if (it != _params.cend()) {
72
    _device_mode = (1 == converter::get_instance().string_to_int(it->second));
73
  }
garciay's avatar
garciay committed
74
75
76
77
  it = _params.find(Params::secured_mode);
  if (it != _params.cend()) {
    _secured_mode = (1 == converter::get_instance().string_to_int(it->second));
  }
78
79
80
81
  it = _params.find(Params::encrypted_mode);
  if (it != _params.cend()) {
    _encrypted_mode = (1 == converter::get_instance().string_to_int(it->second));
  }
garciay's avatar
garciay committed
82
83
84
85
  it = _params.find(Params::enable_security_checks);
  if (it != _params.cend()) {
    _enable_security_checks = (1 == converter::get_instance().string_to_int(it->second));
  }
garciay's avatar
garciay committed
86
  
87
  // Add broadcast address if needed
garciay's avatar
garciay committed
88
89
  it = _params.find(Params::its_aid);
  if (it == _params.cend()) {
garciay's avatar
garciay committed
90
    _params.insert(std::pair<std::string, std::string>(std::string("its_aid"), "141"));
garciay's avatar
garciay committed
91
92
  }
  it = _params.find(Params::mac_bc);
93
94
95
  if (it == _params.cend()) {
    _params.insert(std::pair<std::string, std::string>(std::string("mac_bc"), "FFFFFFFFFFFF"));
  }
garciay's avatar
garciay committed
96
  
garciay's avatar
garciay committed
97
  // Set up default security parameters value
98
  if (_secured_mode || _encrypted_mode) {
garciay's avatar
garciay committed
99
    loggers::get_instance().log("geonetworking_layer::geonetworking_layer: Setup secured mode");
100
    setup_secured_mode();
garciay's avatar
garciay committed
101
  }
garciay's avatar
garciay committed
102
103

  // Automatic beaconing mode
104
105
106
  Params::const_iterator i = _params.find(Params::beaconing);
  if ((i != _params.cend()) && (i->second.compare("1") == 0)) { // Immediate beaconing was requested
    // Prepare beaconing operation
107
    fill_beacon(ll_address, station_type, country, type_of_address);
108
109
110
    start_beaconing();
  }

111
  // Fill packet templates
112
113
114
115
116
117
  fill_gbc_packet(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(ll_address);
  fill_tsb_packet(ll_address);
  fill_ls_reply(ll_address);
  
  // Register this object for AdapterControlPort
garciay's avatar
garciay committed
118
119
  loggers::get_instance().log("geonetworking_layer::geonetworking_layer: Register %s/%p", p_type.c_str(), this);
  registration<geonetworking_layer>::get_instance().add_item(p_type, this);
120
} // End of constructor
121

garciay's avatar
garciay committed
122
123
geonetworking_layer::~geonetworking_layer() {
  loggers::get_instance().log(">>> geonetworking_layer::~geonetworking_layer");
124
125
126
127
128
129
130

  if (_timerid != 0) {
    timer_delete(_timerid);
  }
  if (_beacon != nullptr) {
    delete _beacon;
  }
131
132
133
134
135
  if (_gbc_packet != nullptr) {
    delete _gbc_packet;
  }
  if (_shb_packet != nullptr) {
    delete _shb_packet;
136
  }
137
138
139
  if (_ls_reply != nullptr) {
    delete _ls_reply;
  }
140
141
} // End of destructor

garciay's avatar
garciay committed
142
143
void geonetworking_layer::sendMsg(const LibItsGeoNetworking__TestSystem::GeoNetworkingReq& p, Params& params) {
  loggers::get_instance().log(">>> geonetworking_layer::sendMsg");
144
145
146
147

  // Encode GeoNetworking PDU
  OCTETSTRING data;
  _codec.encode(p.msgOut(), data);
garciay's avatar
garciay committed
148
  sendData(data, _params);
149
150
}

garciay's avatar
garciay committed
151
152
void geonetworking_layer::sendData(OCTETSTRING& data, Params& params) {
  loggers::get_instance().log_msg(">>> geonetworking_layer::sendData: ", data);
153
  params.log();
154
155

  if (_device_mode) { // Need to build a GN packet
garciay's avatar
garciay committed
156
    params[Params::certificate] = _params[Params::certificate];
garciay's avatar
garciay committed
157
    params[Params::hash] = _params[Params::hash];
garciay's avatar
garciay committed
158
    params[Params::signature] = _params[Params::signature];
159
    if (build_geonetworking_pdu(data, params) != 0) {
160
161
162
      return;
    }
  }
163
  if (_secured_mode) { // Add signature support
garciay's avatar
garciay committed
164
    if (build_secured_pdu(data, params) != 0) {
garciay's avatar
garciay committed
165
166
167
      return;
    }
  }
168
169
170
171
172
173
174
175
  
  // 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();
garciay's avatar
garciay committed
176
  loggers::get_instance().log("<<< geonetworking_layer::sendData");
177
178
}

garciay's avatar
garciay committed
179
180
void geonetworking_layer::receiveData(OCTETSTRING& data, Params& params) { 
  loggers::get_instance().log_msg(">>> geonetworking_layer::receiveData: ", data);
garciay's avatar
garciay committed
181
182

  // Check security mode
garciay's avatar
garciay committed
183
  IEEE1609dot2::Ieee1609Dot2Data ieee_1609dot2_data;
garciay's avatar
garciay committed
184
185
186
  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
187
    unsigned int basic_header_len = 4;// FIXME How to retrive the BasicHeader length basic_header.get_descriptor()->raw->fieldlength / 8;
garciay's avatar
garciay committed
188
    loggers::get_instance().log("geonetworking_layer::receiveData: basic_header_len = %d", basic_header_len);
garciay's avatar
garciay committed
189
190
    // 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;
191
    OCTETSTRING secured_data = OCTETSTRING(data.lengthof() - basic_header_len, static_cast<const unsigned char*>(data) + basic_header_len);
192
    ///////////////////
garciay's avatar
garciay committed
193
    // FIXME Check what to do with this!
194
    if (*static_cast<const unsigned char*>(secured_data) == 0x02) { // This is the old Security version format, discard it
garciay's avatar
garciay committed
195
      loggers::get_instance().warning("geonetworking_layer::receiveData: Old security format, discard it");
196
197
198
      return;
    }
    ///////////////////
garciay's avatar
garciay committed
199
    if (security_services::get_instance().verify_and_extract_gn_payload(secured_data, _enable_security_checks, ieee_1609dot2_data, unsecured_gn_payload, params) != 0) {
garciay's avatar
garciay committed
200
      loggers::get_instance().warning("geonetworking_layer::receiveData: Security error");
garciay's avatar
garciay committed
201
202
203
204
205
      if (_enable_security_checks) {
        return;
      }
    }
    // Update data
garciay's avatar
garciay committed
206
    loggers::get_instance().log_msg("geonetworking_layer::receiveData: Unsecured payload: ", unsecured_gn_payload);
207
    data = OCTETSTRING(basic_header_len, static_cast<const unsigned char*>(data)) + unsecured_gn_payload;
garciay's avatar
garciay committed
208
209
  }
  
210
  // Decode the payload
garciay's avatar
garciay committed
211
  loggers::get_instance().log_msg("geonetworking_layer::receiveData: Geonetworking payload to decode: ", data);
212
213
  LibItsGeoNetworking__TestSystem::GeoNetworkingInd ind;
  _codec.decode(data, ind.msgIn(), &params);
garciay's avatar
garciay committed
214
  if (ind.msgIn().is_bound()) {
garciay's avatar
garciay committed
215
216
217
218
219
    // Update optional securedMsg field if required
    if (ieee_1609dot2_data.is_bound()) {
      ind.msgIn().gnPacket().securedMsg() = OPTIONAL<IEEE1609dot2::Ieee1609Dot2Data>(ieee_1609dot2_data);
    } // else, nothing to do
    // Update context
220
221
222
223
    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
224
      const LibItsGeoNetworking__TypesAndValues::ExtendedHeader& ex = p.extendedHeader();
225
226
227
228
229
230
231
232
233
234
235
236
237
238
      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
239
240
241
242
243
          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
244
              eh->lsReplyHeader().seqNumber() = _sequence_number++;
245
246
247
248
249
              // Update destination
              eh->lsReplyHeader().dstPosVector().gnAddr() = sopv->gnAddr();
              eh->lsReplyHeader().dstPosVector().latitude() = sopv->latitude();
              eh->lsReplyHeader().dstPosVector().longitude() = sopv->longitude();
              // Update timestamp
250
251
              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);
252
253
              eh->lsReplyHeader().dstPosVector().timestamp__() = eh->lsReplyHeader().srcPosVector().timestamp__();
              
garciay's avatar
garciay committed
254
              loggers::get_instance().log_msg("geonetworking_layer::receiveData: ", *_ls_reply);
255
256
257
258
259
              
              // send it
              // Encode GeoNetworking PDU
              OCTETSTRING os;
              _codec.encode(*_ls_reply, os);
260
              // Apply signature
garciay's avatar
garciay committed
261
              if (_secured_mode) {
262
                if (build_secured_pdu(data, _params) != 0) {
garciay's avatar
garciay committed
263
264
265
                  return;
                }
              }
266
267
268
269
270
271
272
273
274
              // 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 {
garciay's avatar
garciay committed
275
              loggers::get_instance().error("geonetworking_layer::sendData: Wrong cast");
276
277
278
              return;
            }
          }
279
280
281
282
283
284
285
286
287
288
289
290
291
292
        } 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
garciay's avatar
garciay committed
293
      loggers::get_instance().log("geonetworking_layer::receiveData: sopv is bound: %d", sopv->is_bound());
294
295
296
297
298
299
300
301
      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)) {
garciay's avatar
garciay committed
302
      loggers::get_instance().log_msg("geonetworking_layer::receiveData: Pass beaconing filtering", sopv->gnAddr().mid());
303
      if (_pass_beacon_table.empty()) { // Discard beacon
garciay's avatar
garciay committed
304
        loggers::get_instance().log("geonetworking_layer::receiveData: Pass beaconing table empty, discard it");
305
306
307
        return;
      } else { // Check beacon filter for StartPassBeaconing/Stop
        if (!_pass_beacon_table.has_entry(sopv->gnAddr().mid())) { // Discard beacon
garciay's avatar
garciay committed
308
          loggers::get_instance().log_msg("geonetworking_layer::receiveData: Not in pass beaconing table, discard it", *sopv);
309
310
311
312
          return;
        } // else, continue
      }
    } // else, continue
garciay's avatar
garciay committed
313
314
  } else {
    // Inavlid GeoNetworking payload, discard it
garciay's avatar
garciay committed
315
    loggers::get_instance().warning("geonetworking_layer::receiveData: Failed to decode payload, discard it");
garciay's avatar
garciay committed
316
317
    return;
  }
318
319
320
321
322

  // Add lower layers parameters
  // 1. Destination MAC address
  Params::const_iterator it = params.find(Params::mac_dst);
  if (it != params.cend()) {
garciay's avatar
garciay committed
323
    loggers::get_instance().log("geonetworking_layer::receiveData: dst=%s", it->second.c_str());
324
325
326
327
328
329
330
    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()) {
garciay's avatar
garciay committed
331
    loggers::get_instance().log("geonetworking_layer::receiveData: ssp=%s", it->second.c_str());
332
333
334
335
336
337
338
    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()) {
garciay's avatar
garciay committed
339
    loggers::get_instance().log("geonetworking_layer::receiveData: its_aid=%s", it->second.c_str());
340
341
342
343
344
345
346
347
    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()) {
garciay's avatar
garciay committed
348
    loggers::get_instance().log("geonetworking_layer::receiveData: gn_payload=%s", it->second.c_str());
349
350
351
    OCTETSTRING os(str2oct(CHARSTRING(it->second.c_str())));
    receiveToAllLayers(os, params);
  } else {
garciay's avatar
garciay committed
352
    loggers::get_instance().warning("geonetworking_layer::receiveData: No payload to pass to upper layers");
353
  }
354
  
355
356
357
358
  // Pass it to the ports
  toAllUpperPorts(ind, params);
}

garciay's avatar
garciay committed
359
OCTETSTRING geonetworking_layer::trigger_ac_event(OCTETSTRING& data, Params& params)
360
{
garciay's avatar
garciay committed
361
  loggers::get_instance().log_to_hexa(">>> geonetworking_layer::trigger_ac_event: ", data);
362
363
364
365
366


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

garciay's avatar
garciay committed
367
368
369
void geonetworking_layer::start_beaconing() {
  loggers::get_instance().log(">>> geonetworking_layer::start_beaconing");
  //loggers::get_instance().log_msg("geonetworking_layer::start_beaconing: _beacon=", *_beacon);
370
371

  // Establish handler for timer signal
garciay's avatar
garciay committed
372
  loggers::get_instance().log("geonetworking_layer::start_beaconing: Establishing handler for signal %d\n", _signal_id);
373
374
375
  _sa.sa_flags = SA_SIGINFO;
  _sa.sa_sigaction = timer_irq_sigalrm_handler;
  sigemptyset(&_sa.sa_mask);
376
  if (sigaction(_signal_id, &_sa, nullptr) == -1) {
garciay's avatar
garciay committed
377
    loggers::get_instance().error("geonetworking_layer::start_beaconing: Sigaction failure: %d", errno);
378
379
  }
  // Block timer signal temporarily
garciay's avatar
garciay committed
380
  loggers::get_instance().log("geonetworking_layer::start_beaconing: Blocking signal %d\n", _signal_id);
381
382
  sigemptyset(&_mask);
  sigaddset(&_mask, _signal_id);
383
  if (sigprocmask(SIG_SETMASK, &_mask, nullptr) == -1) {
garciay's avatar
garciay committed
384
    loggers::get_instance().error("geonetworking_layer::start_beaconing: Sigprocmask failure: %d", errno);
385
386
387
388
  }	
  // Create the timer 
  _sev.sigev_notify = SIGEV_SIGNAL;
  _sev.sigev_signo = _signal_id; // Use signal alarm
garciay's avatar
garciay committed
389
  _sev.sigev_value.sival_ptr = this; // The geonetworking_layer object address
390
  if (timer_create(CLOCK_REALTIME, &_sev, &_timerid) == -1) {
garciay's avatar
garciay committed
391
    loggers::get_instance().error("geonetworking_layer::start_beaconing: Timer failure: %d", errno);
392
  }
garciay's avatar
garciay committed
393
  loggers::get_instance().log("geonetworking_layer::start_beaconing: timer ID is 0x%x\n", (long)_timerid);
394
395
396
397
  // Start the timer
  unsigned int expiry = 1000; // Default expiry time 1000ms
  Params::const_iterator i = _params.find("expiry");
  if (i != _params.cend()) {
398
    expiry = static_cast<unsigned int>(std::strtoul(i->second.c_str(), nullptr, 10));
399
400
401
402
403
404
  }
  _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;
405
  if (timer_settime(_timerid, 0, &_its, nullptr) == -1) {
garciay's avatar
garciay committed
406
    loggers::get_instance().error("geonetworking_layer::start_beaconing: Sigprocmask failure: %d", errno);
407
408
  }
  // Unlock the timer signal, so that timer notification can be delivered
garciay's avatar
garciay committed
409
  loggers::get_instance().log("geonetworking_layer::start_beaconing: Unblocking signal %d\n", _signal_id);
410
  if (sigprocmask(SIG_UNBLOCK, &_mask, nullptr) == -1) {
garciay's avatar
garciay committed
411
    loggers::get_instance().error("geonetworking_layer::start_beaconing: Sigprocmask failure: %d", errno);
412
413
414
  }
} // End of start_beaconing method

garciay's avatar
garciay committed
415
416
void geonetworking_layer::start_beaconing(const LibItsGeoNetworking__TypesAndValues::GeoNetworkingPdu& p_beacon) {
  loggers::get_instance().log_msg(">>> geonetworking_layer::start_beaconing", p_beacon);
417
418
419
420
421
422
423
424
425
426

  // 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

garciay's avatar
garciay committed
427
428
void geonetworking_layer::stop_beaconing() {
  loggers::get_instance().log(">>> geonetworking_layer::stop_beaconing");
429
430
  
  // Block timer signal temporarily
garciay's avatar
garciay committed
431
  loggers::get_instance().log("geonetworking_layer::stop_beaconing: Blocking signal %d\n", _signal_id);
432
433
  sigemptyset(&_mask);
  sigaddset(&_mask, _signal_id);
434
  if (sigprocmask(SIG_SETMASK, &_mask, nullptr) == -1) {
garciay's avatar
garciay committed
435
    loggers::get_instance().error("geonetworking_layer::stop_beaconing: Sigprocmask failure: %d", errno);
436
437
438
439
440
  }	
  timer_delete(_timerid);
  _timerid = 0;
} // End of stop_beaconing method

garciay's avatar
garciay committed
441
442
void geonetworking_layer::send_beacon() {
  loggers::get_instance().log(">>> geonetworking_layer::send_beacon");
443
444
  
  ExtendedHeader* eh = static_cast<ExtendedHeader *>(_beacon->gnPacket().packet().extendedHeader().get_opt_value());
445
  if (eh == nullptr) {
garciay's avatar
garciay committed
446
    loggers::get_instance().error("geonetworking_layer::send_beacon: Wrong cast");
447
448
449
  }
  // 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()
garciay's avatar
garciay committed
450
  eh->beaconHeader().srcPosVector().timestamp__().set_long_long_val((unsigned int)ms);
garciay's avatar
garciay committed
451
  //loggers::get_instance().log_msg("geonetworking_layer::send_beacon: ", *_beacon);
452
453
454
455
  // 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());
garciay's avatar
garciay committed
456
  Params params(_params);
457
  if (_secured_mode) { // Apply signature
458
    if (build_secured_pdu(data, _params) != 0) {
garciay's avatar
garciay committed
459
460
461
      return;
    }
  }
462
  // Send it
463
464
465
466
467
468
469
  // 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();
470
  
garciay's avatar
garciay committed
471
  //loggers::get_instance().log("<<< geonetworking_layer::send_beacon");
472
473
} // End of send_beacon method

garciay's avatar
garciay committed
474
475
void geonetworking_layer::start_pass_beaconing(const LibItsGeoNetworking__TypesAndValues::BeaconHeader& p_beacon) {
  loggers::get_instance().log_msg(">>> geonetworking_layer::start_pass_beaconing", p_beacon);
476
477
478
479
480
481
482

  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

garciay's avatar
garciay committed
483
484
void geonetworking_layer::stop_pass_beaconing() {
  loggers::get_instance().log(">>> geonetworking_layer::stop_pass_beaconing");
485
486
487
488

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

garciay's avatar
garciay committed
489
490
int geonetworking_layer::enable_secured_mode(const std::string& p_certificate_id, const boolean p_enforce_security) {
  loggers::get_instance().log(">>> geonetworking_layer::enable_secured_mode: '%s' - %x", p_certificate_id.c_str(), p_enforce_security);
491

garciay's avatar
garciay committed
492
  loggers::get_instance().log("geonetworking_layer::enable_secured_mode: _secured_mode = %x", _secured_mode);
493
  if (!_secured_mode) {
garciay's avatar
garciay committed
494
    loggers::get_instance().log("geonetworking_layer::enable_secured_mode: Setup secured mode");
495
496
497
498
499
500
501
502
503
504
    _secured_mode = true;
    setup_secured_mode();
  }
  _enable_security_checks = p_enforce_security;
  Params::const_iterator it = _params.find(Params::certificate);
  if (it == _params.cend()) {
    _params.insert(std::pair<std::string, std::string>(std::string("certificate"), p_certificate_id));
  } else {
    _params[Params::certificate] = p_certificate_id;
  }
garciay's avatar
garciay committed
505
  loggers::get_instance().log("geonetworking_layer::enable_secured_mode: Certificate to be used: '%s'", _params[Params::certificate].c_str());
506
507
  
  return 0;
508
509
}

garciay's avatar
garciay committed
510
511
int geonetworking_layer::disable_secured_mode() {
  loggers::get_instance().log(">>> geonetworking_layer::disable_secured_mode");
garciay's avatar
garciay committed
512
513
514
515
516
  _secured_mode = false;
  _enable_security_checks = false;
  return 0;
}

garciay's avatar
garciay committed
517
const LongPosVector* geonetworking_layer::get_lpv(const GN__Address& p_gn_address)
518
{
garciay's avatar
garciay committed
519
  loggers::get_instance().log_msg(">>> geonetworking_layer::get_lpv", p_gn_address);
520
521
522
523
524
525
526
527
  
  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

garciay's avatar
garciay committed
528
void geonetworking_layer::fill_beacon(const OCTETSTRING& p_ll_address, const INTEGER p_station_type, const INTEGER p_country, const INTEGER type_of_address)
529
530
531
532
533
534
535
536
537
538
539
{
  _beacon = new GeoNetworkingPdu();
  HeaderTST h;
  h.beaconHdr() = BeaconHeaderType(
                                   HeaderType(HeaderType::e__beacon),
                                   0
                                   );
  ExtendedHeader eh;
  eh.beaconHeader() = BeaconHeader(
                                   LongPosVector(
                                                 GN__Address(
540
541
542
                                                             TypeOfAddress((TypeOfAddress)type_of_address),
                                                             StationType((StationType)p_station_type),
                                                             p_country,
543
544
545
                                                             p_ll_address
                                                             ),
                                                 0,
546
547
                                                 _latitude,
                                                 _longitude,
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
                                                 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();
garciay's avatar
garciay committed
587
  //loggers::get_instance().log_msg("geonetworking_layer::fill_beacon: beacon value: ", *_beacon);
588
589
} // End of fill_beacon method
  
garciay's avatar
garciay committed
590
void geonetworking_layer::fill_gbc_packet(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)
591
{
592
  _gbc_packet = new GeoNetworkingPdu();
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
  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,
610
611
                                                           _latitude,
                                                           _longitude,
612
613
614
615
616
617
618
619
620
621
622
                                                           int2bit(1, 1), // PAI
                                                           0,
                                                           0
                                                           ),
                                             p_geoAreaPosLatitude,
                                             p_geoAreaPosLongitude,
                                             p_distanceA,
                                             p_distanceB,
                                             p_angle,
                                             0
                                              );
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
  _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();
garciay's avatar
garciay committed
657
  //loggers::get_instance().log_msg("geonetworking_layer::fill_gbc_packet: packet value: ", *_gbc_packet);
658
659
} // End of fill_gbc_packet method

garciay's avatar
garciay committed
660
void geonetworking_layer::fill_shb_packet(const OCTETSTRING& p_ll_address)
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
{
  _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
676
                                                       ),
677
                                           0,
678
679
                                           _latitude,
                                           _longitude,
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
                                           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();
garciay's avatar
garciay committed
720
  //loggers::get_instance().log_msg("geonetworking_layer::fill_shb_packet: packet value: ", *_shb_packet);
721
722
} // End of fill_shb_packet method

garciay's avatar
garciay committed
723
void geonetworking_layer::fill_tsb_packet(const OCTETSTRING& p_ll_address, const int p_hop_number, const int p_max_hop_limit)
garciay's avatar
garciay committed
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
{
  _tsb_packet = new GeoNetworkingPdu();
  HeaderTST h;
  h.tsbHdr() = TsbHeaderType(
                             HeaderType(HeaderType::e__topologicallyScopedBroadcast),
                             HeaderSubTypeTSB(HeaderSubTypeTSB::e__multiHop)
                             );
  ExtendedHeader eh;
  eh.tsbHeader() = TSBHeader(
                             0,
                             0,
                             LongPosVector(
                                           GN__Address(
                                                       TypeOfAddress(TypeOfAddress::e__manual), // TODO Use Params
                                                       StationType(StationType::e__passengerCar), // TODO Use Params
                                                       33,
                                                       p_ll_address
                                                       ),
                                           0,
743
744
                                           _latitude,
                                           _longitude,
garciay's avatar
garciay committed
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
                                           int2bit(1, 1), // PAI
                                           0,
                                           0
                                           )
                             );
  _tsb_packet->basicHeader() = BasicHeader(
                                           0,
                                           BasicNextHeader(
                                                           BasicNextHeader::e__commonHeader
                                                           ),
                                           0,
                                           Lifetime(
                                                    4,
                                                    LtBase(LtBase::e__50ms)
                                                    ),
                                           p_hop_number
                                           );
  _tsb_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,
                                                                     p_max_hop_limit,
                                                                     0
                                                                     ),
                                                        OPTIONAL<ExtendedHeader>(eh),
                                                        OPTIONAL<GnRawPayload>()
                                                        );
  _tsb_packet->gnPacket().packet().payload().set_to_omit();
  _tsb_packet->gnPacket().securedMsg().set_to_omit();
garciay's avatar
garciay committed
784
  //loggers::get_instance().log_msg("geonetworking_layer::fill_tsb_packet: packet value: ", *_tsb_packet);
garciay's avatar
garciay committed
785
786
} // End of fill_tsb_packet method

garciay's avatar
garciay committed
787
void geonetworking_layer::fill_ls_reply(const OCTETSTRING& p_ll_address)
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
{
  _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,
807
808
                                                   _latitude,
                                                   _longitude,
809
810
811
812
813
814
815
816
817
818
819
820
                                                   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,
821
822
                                                    _latitude,
                                                    _longitude
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
                                                    )
                                     );
  _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();
garciay's avatar
garciay committed
859
  //loggers::get_instance().log_msg("geonetworking_layer::fill_ls_reply: packet value: ", *_ls_reply);
860
} // End of fill_ls_reply method
861

garciay's avatar
garciay committed
862
863
void geonetworking_layer::timer_irq_sigalrm_handler(int p_signal, siginfo_t *p_signal_info, void *p_uc) {
  //loggers::get_instance().log(">>> geonetworking_layer::timer_irq_sigalrm_handler: Caught signal %d", p_signal);
864

garciay's avatar
garciay committed
865
  static_cast<geonetworking_layer *>(p_signal_info->si_value.sival_ptr)->send_beacon();
866
867
} // End of method timer_irq_sigalrm_handler

garciay's avatar
garciay committed
868
869
int geonetworking_layer::build_geonetworking_pdu(OCTETSTRING& data, Params& params) {
  loggers::get_instance().log(">>> geonetworking_layer::build_geonetworking_pdu");
garciay's avatar
garciay committed
870
  //params.log();
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
  
  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();
  }
garciay's avatar
garciay committed
887
  loggers::get_instance().log("geonetworking_layer::build_geonetworking_pdu: %s, %s, %s", next_header.c_str(), header_type.c_str(), header_sub_type.c_str());
888
889
890
891
892
  
  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) {
garciay's avatar
garciay committed
893
        loggers::get_instance().error("geonetworking_layer::build_geonetworking_pdu: Wrong cast");
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
        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));
909
      
garciay's avatar
garciay committed
910
      loggers::get_instance().log_msg("geonetworking_layer::build_geonetworking_pdu: shb: ", *_shb_packet);
911
912
913
914
      // Encode GeoNetworking PDU
      OCTETSTRING os;
      _codec.encode(*_shb_packet, os);
      data = os;
garciay's avatar
garciay committed
915
916
917
    } else { // Use TSB
      ExtendedHeader* eh = static_cast<ExtendedHeader *>(_tsb_packet->gnPacket().packet().extendedHeader().get_opt_value());
      if (eh == nullptr) {
garciay's avatar
garciay committed
918
        loggers::get_instance().error("geonetworking_layer::build_geonetworking_pdu: Wrong cast");
garciay's avatar
garciay committed
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
        return -1;
      }
      // Update sequence number
      eh->tsbHeader().seqNumber() = _sequence_number++;
      // Update NextHeader
      it = params.find(Params::next_header);
      if (next_header.compare("btpB") == 0) {
        _tsb_packet->gnPacket().packet().commonHeader().nextHeader() = NextHeader::e__btpB;
      } else { // Default btp is btpA
        _tsb_packet->gnPacket().packet().commonHeader().nextHeader() = NextHeader::e__btpA;
      }
      // Update payload
      _tsb_packet->gnPacket().packet().commonHeader().plLength() = data.lengthof();
      _tsb_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->tsbHeader().srcPosVector().timestamp__().set_long_long_val(static_cast<unsigned int>(ms));
      
garciay's avatar
garciay committed
937
      loggers::get_instance().log_msg("geonetworking_layer::build_geonetworking_pdu: tsb: ", *_tsb_packet);
garciay's avatar
garciay committed
938
939
940
941
      // Encode GeoNetworking PDU
      OCTETSTRING os;
      _codec.encode(*_tsb_packet, os);
      data = os;
942
943
944
    }
  } else { // TODO To be continued
    // Default: Use GBC
garciay's avatar
garciay committed
945
    ExtendedHeader* eh = static_cast<ExtendedHeader *>(_gbc_packet->gnPacket().packet().extendedHeader().get_opt_value());
946
    if (eh == nullptr) {
garciay's avatar
garciay committed
947
      loggers::get_instance().error("geonetworking_layer::build_geonetworking_pdu: Wrong cast");
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
      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));
965
    
garciay's avatar
garciay committed
966
    loggers::get_instance().log_msg("geonetworking_layer::build_geonetworking_pdu: gbc: ", *_gbc_packet);
967
968
969
970
971
972
973
974
975
976

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

garciay's avatar
garciay committed
977
978
int geonetworking_layer::build_secured_pdu(OCTETSTRING& data, Params& params) {
  loggers::get_instance().log_msg(">>> geonetworking_layer::build_secured_pdu: ", data);
garciay's avatar
garciay committed
979
  //params.log();
garciay's avatar
garciay committed
980
981
982
983
  
  LibItsGeoNetworking__TypesAndValues::BasicHeader basic_header;
  decode_basic_header(data, basic_header);
  // Update security mode
984
  unsigned int basic_header_len = 4;// FIXME How to retrive the BasicHeader length basic_header.get_descriptor()->raw->fieldlength / 8;
garciay's avatar
garciay committed
985
  loggers::get_instance().log("geonetworking_layer::build_secured_pdu: basic_header_len = %d", basic_header_len);
garciay's avatar
garciay committed
986
  basic_header.nextHeader() = BasicNextHeader::e__securedPacket;
garciay's avatar
garciay committed
987
  OCTETSTRING unsecured_gn_payload = OCTETSTRING(data.lengthof() - basic_header_len, static_cast<const unsigned char*>(data) + basic_header_len);
garciay's avatar
garciay committed
988
  OCTETSTRING secured_gn_payload;
garciay's avatar
garciay committed
989
  if (security_services::get_instance().secure_gn_payload(unsecured_gn_payload, secured_gn_payload, params) != 0) {
garciay's avatar
garciay committed
990
    loggers::get_instance().warning("geonetworking_layer::build_secured_pdu: failed to build secured pdu");
garciay's avatar
garciay committed
991
992
993
994
    return -1;
  }

  // Encode the basid header
garciay's avatar
garciay committed
995
  //loggers::get_instance().log_msg("geonetworking_layer::build_secured_pdu: New basic_header = ", basic_header);
garciay's avatar
garciay committed
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
  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;
garciay's avatar
garciay committed
1007
  loggers::get_instance().log_msg("geonetworking_layer::build_secured_pdu: Secured pdu = ", data);
garciay's avatar
garciay committed
1008
1009

  return 0;
garciay's avatar
garciay committed
1010
1011
}

garciay's avatar
garciay committed
1012
1013
int geonetworking_layer::decode_basic_header(const OCTETSTRING& p_data, LibItsGeoNetworking__TypesAndValues::BasicHeader& p_basic_header) {
  loggers::get_instance().log_msg(">>> geonetworking_layer::decode_basic_header: ", p_data);
garciay's avatar
garciay committed
1014
1015
1016
  
  // 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
garciay's avatar
garciay committed
1017
  loggers::get_instance().log_msg("geonetworking_layer::decode_basic_header: bh: ", bh);
garciay's avatar
garciay committed
1018
1019
  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);
garciay's avatar
garciay committed
1020
  loggers::get_instance().log_msg("geonetworking_layer::decode_basic_header: ", p_basic_header);
garciay's avatar
garciay committed
1021
1022
1023
1024
  
  return 0;
}

garciay's avatar
garciay committed
1025
1026
int geonetworking_layer::setup_secured_mode() {
  loggers::get_instance().log(">>> geonetworking_layer::setup_secured_mode");
1027

garciay's avatar
garciay committed
1028
  loggers::get_instance().log("geonetworking_layer::setup_secured_mode: GN Layer address = %p", this);
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052

  Params::const_iterator it = _params.find(Params::certificate);
  if (it == _params.cend()) {
    _params.insert(std::pair<std::string, std::string>(std::string("certificate"), "CERT_TS_A_AT"));
  }
  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("hash"), "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);
  security_services::get_instance().set_position(_latitude, _longitude);

  return 0;
}

garciay's avatar
garciay committed
1053
geonetworking_layer_factory geonetworking_layer_factory::_f;