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

  // Automatic beaconing mode
garciay's avatar
garciay committed
108
  params::const_iterator i = _params.find(params::beaconing);
109
110
  if ((i != _params.cend()) && (i->second.compare("1") == 0)) { // Immediate beaconing was requested
    // Prepare beaconing operation
111
    fill_beacon(ll_address, station_type, country, type_of_address);
112
113
114
    start_beaconing();
  }

115
  // Fill packet templates
116
117
118
119
120
121
  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
122
123
  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);
124
} // End of constructor
125

garciay's avatar
garciay committed
126
127
geonetworking_layer::~geonetworking_layer() {
  loggers::get_instance().log(">>> geonetworking_layer::~geonetworking_layer");
128
129
130
131
132
133
134

  if (_timerid != 0) {
    timer_delete(_timerid);
  }
  if (_beacon != nullptr) {
    delete _beacon;
  }
135
136
137
138
139
  if (_gbc_packet != nullptr) {
    delete _gbc_packet;
  }
  if (_shb_packet != nullptr) {
    delete _shb_packet;
140
  }
141
142
143
  if (_ls_reply != nullptr) {
    delete _ls_reply;
  }
144
145
} // End of destructor

garciay's avatar
garciay committed
146
void geonetworking_layer::sendMsg(const LibItsGeoNetworking__TestSystem::GeoNetworkingReq& p, params& params) {
garciay's avatar
garciay committed
147
  loggers::get_instance().log(">>> geonetworking_layer::sendMsg");
148
149
150
151

  // Encode GeoNetworking PDU
  OCTETSTRING data;
  _codec.encode(p.msgOut(), data);
garciay's avatar
garciay committed
152
  send_data(data, _params);
153
154
}

garciay's avatar
garciay committed
155
void geonetworking_layer::send_data(OCTETSTRING& data, params& params) {
garciay's avatar
garciay committed
156
  loggers::get_instance().log_msg(">>> geonetworking_layer::send_data: ", data);
157
  params.log();
158
159

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

garciay's avatar
garciay committed
183
void geonetworking_layer::receive_data(OCTETSTRING& data, params& params) { 
garciay's avatar
garciay committed
184
  loggers::get_instance().log_msg(">>> geonetworking_layer::receive_data: ", data);
garciay's avatar
garciay committed
185
186

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

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

garciay's avatar
garciay committed
363
OCTETSTRING geonetworking_layer::trigger_ac_event(OCTETSTRING& data, params& params)
364
{
garciay's avatar
garciay committed
365
  loggers::get_instance().log_to_hexa(">>> geonetworking_layer::trigger_ac_event: ", data);
366
367
368
369
370


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

garciay's avatar
garciay committed
371
372
373
void geonetworking_layer::start_beaconing() {
  loggers::get_instance().log(">>> geonetworking_layer::start_beaconing");
  //loggers::get_instance().log_msg("geonetworking_layer::start_beaconing: _beacon=", *_beacon);
374
375

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

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

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

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

garciay's avatar
garciay committed
478
479
void geonetworking_layer::start_pass_beaconing(const LibItsGeoNetworking__TypesAndValues::BeaconHeader& p_beacon) {
  loggers::get_instance().log_msg(">>> geonetworking_layer::start_pass_beaconing", p_beacon);
480
481
482
483
484
485
486

  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
487
488
void geonetworking_layer::stop_pass_beaconing() {
  loggers::get_instance().log(">>> geonetworking_layer::stop_pass_beaconing");
489
490
491
492

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

garciay's avatar
garciay committed
493
494
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);
495

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

garciay's avatar
garciay committed
514
515
int geonetworking_layer::disable_secured_mode() {
  loggers::get_instance().log(">>> geonetworking_layer::disable_secured_mode");
garciay's avatar
garciay committed
516
517
518
519
520
  _secured_mode = false;
  _enable_security_checks = false;
  return 0;
}

garciay's avatar
garciay committed
521
const LongPosVector* geonetworking_layer::get_lpv(const GN__Address& p_gn_address)
522
{
garciay's avatar
garciay committed
523
  loggers::get_instance().log_msg(">>> geonetworking_layer::get_lpv", p_gn_address);
524
525
526
527
528
529
530
531
  
  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
532
void geonetworking_layer::fill_beacon(const OCTETSTRING& p_ll_address, const INTEGER p_station_type, const INTEGER p_country, const INTEGER type_of_address)
533
534
535
536
537
538
539
540
541
542
543
{
  _beacon = new GeoNetworkingPdu();
  HeaderTST h;
  h.beaconHdr() = BeaconHeaderType(
                                   HeaderType(HeaderType::e__beacon),
                                   0
                                   );
  ExtendedHeader eh;
  eh.beaconHeader() = BeaconHeader(
                                   LongPosVector(
                                                 GN__Address(
544
545
546
                                                             TypeOfAddress((TypeOfAddress)type_of_address),
                                                             StationType((StationType)p_station_type),
                                                             p_country,
547
548
549
                                                             p_ll_address
                                                             ),
                                                 0,
550
551
                                                 _latitude,
                                                 _longitude,
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
587
588
589
590
                                                 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
591
  //loggers::get_instance().log_msg("geonetworking_layer::fill_beacon: beacon value: ", *_beacon);
592
593
} // End of fill_beacon method
  
garciay's avatar
garciay committed
594
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)
595
{
596
  _gbc_packet = new GeoNetworkingPdu();
597
598
599
600
601
602
603
604
605
606
607
  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(
garciay's avatar
garciay committed
608
609
                                                                       TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                                       StationType(StationType::e__passengerCar), // TODO Use params
610
611
612
613
                                                                       33,
                                                                       p_ll_address
                                                                       ),
                                                           0,
614
615
                                                           _latitude,
                                                           _longitude,
616
617
618
619
620
621
622
623
624
625
626
                                                           int2bit(1, 1), // PAI
                                                           0,
                                                           0
                                                           ),
                                             p_geoAreaPosLatitude,
                                             p_geoAreaPosLongitude,
                                             p_distanceA,
                                             p_distanceB,
                                             p_angle,
                                             0
                                              );
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
657
658
659
660
  _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
661
  //loggers::get_instance().log_msg("geonetworking_layer::fill_gbc_packet: packet value: ", *_gbc_packet);
662
663
} // End of fill_gbc_packet method

garciay's avatar
garciay committed
664
void geonetworking_layer::fill_shb_packet(const OCTETSTRING& p_ll_address)
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(
garciay's avatar
garciay committed
676
677
                                                       TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                       StationType(StationType::e__passengerCar), // TODO Use params
678
679
                                                       33,
                                                       p_ll_address
680
                                                       ),
681
                                           0,
682
683
                                           _latitude,
                                           _longitude,
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
720
721
722
723
                                           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
724
  //loggers::get_instance().log_msg("geonetworking_layer::fill_shb_packet: packet value: ", *_shb_packet);
725
726
} // End of fill_shb_packet method

garciay's avatar
garciay committed
727
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
728
729
730
731
732
733
734
735
736
737
738
739
740
{
  _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(
garciay's avatar
garciay committed
741
742
                                                       TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                       StationType(StationType::e__passengerCar), // TODO Use params
garciay's avatar
garciay committed
743
744
745
746
                                                       33,
                                                       p_ll_address
                                                       ),
                                           0,
747
748
                                           _latitude,
                                           _longitude,
garciay's avatar
garciay committed
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
784
785
786
787
                                           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
788
  //loggers::get_instance().log_msg("geonetworking_layer::fill_tsb_packet: packet value: ", *_tsb_packet);
garciay's avatar
garciay committed
789
790
} // End of fill_tsb_packet method

garciay's avatar
garciay committed
791
void geonetworking_layer::fill_ls_reply(const OCTETSTRING& p_ll_address)
792
793
794
795
796
797
798
799
800
801
802
803
804
{
  _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(
garciay's avatar
garciay committed
805
806
                                                               TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                               StationType(StationType::e__passengerCar), // TODO Use params
807
808
809
810
                                                               33,
                                                               p_ll_address
                                                               ),
                                                   0,
811
812
                                                   _latitude,
                                                   _longitude,
813
814
815
816
817
818
                                                   int2bit(1, 1), // PAI
                                                   0,
                                                   0
                                                   ),
                                     ShortPosVector(
                                                    GN__Address(
garciay's avatar
garciay committed
819
820
                                                                TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                                StationType(StationType::e__passengerCar), // TODO Use params
821
822
823
824
                                                                33,
                                                                p_ll_address
                                                                ),
                                                    0,
825
826
                                                    _latitude,
                                                    _longitude
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
859
860
861
862
                                                    )
                                     );
  _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
863
  //loggers::get_instance().log_msg("geonetworking_layer::fill_ls_reply: packet value: ", *_ls_reply);
864
} // End of fill_ls_reply method
865

garciay's avatar
garciay committed
866
867
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);
868

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

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

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

garciay's avatar
garciay committed
981
int geonetworking_layer::build_secured_pdu(OCTETSTRING& data, params& params) {
garciay's avatar
garciay committed
982
  loggers::get_instance().log_msg(">>> geonetworking_layer::build_secured_pdu: ", data);
garciay's avatar
garciay committed
983
  //params.log();
garciay's avatar
garciay committed
984
985
986
987
  
  LibItsGeoNetworking__TypesAndValues::BasicHeader basic_header;