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

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

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

10
11
#include "loggers.hh"

garciay's avatar
garciay committed
12
13
#include "security_services.hh"

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

Yann Garcia's avatar
Yann Garcia committed
16
17
#include "registration.hh"

18
19
#include "converter.hh"

20
21
using namespace LibItsGeoNetworking__TypesAndValues;

22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
geonetworking_layer::geonetworking_layer(const std::string & p_type, const std::string & p_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} {
  loggers::get_instance().log(">>> geonetworking_layer::geonetworking_layer: %s, %s", to_string().c_str(), p_param.c_str());

  init(p_type, p_param);
} // End of constructor

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

  if (_timerid != 0) {
    timer_delete(_timerid);
  }
  if (_beacon != nullptr) {
    delete _beacon;
  }
  if (_gbc_packet != nullptr) {
    delete _gbc_packet;
  }
  if (_shb_packet != nullptr) {
    delete _shb_packet;
  }
garciay's avatar
garciay committed
43
44
45
  if (_tsb_packet != nullptr) {
    delete _tsb_packet;
  }
46
47
48
49
50
51
52
  if (_ls_reply != nullptr) {
    delete _ls_reply;
  }
} // End of destructor

void geonetworking_layer::init(const std::string & p_type, const std::string & p_param) {
  loggers::get_instance().log(">>> geonetworking_layer::init: %s, %s", to_string().c_str(), p_param.c_str());
53
54
  
  // Setup parameters
55
  params::convert(_params, p_param);
56
  // Sanity checks
garciay's avatar
garciay committed
57
  params::const_iterator it = _params.find(params::latitude);
58
  if (it != _params.cend()) {
59
    _latitude = converter::get_instance().string_to_int(it->second);
60
  }
garciay's avatar
garciay committed
61
  it = _params.find(params::longitude);
62
  if (it != _params.cend()) {
63
    _longitude = converter::get_instance().string_to_int(it->second);
64
65
  }
  OCTETSTRING ll_address;
garciay's avatar
garciay committed
66
  it = _params.find(params::ll_address);
67
68
69
70
  if (it != _params.cend()) {
    ll_address = str2oct(CHARSTRING(it->second.c_str()));
  }
  INTEGER distanceA = 1000; // 1km
garciay's avatar
garciay committed
71
  it = _params.find(params::distanceA);
72
  if (it != _params.cend()) {
73
    distanceA = converter::get_instance().string_to_int(it->second);
74
75
  }
  INTEGER distanceB = 1000; // 1Km
garciay's avatar
garciay committed
76
  it = _params.find(params::distanceB);
77
  if (it != _params.cend()) {
78
    distanceB = converter::get_instance().string_to_int(it->second);
79
80
  }
  INTEGER angle = 0;
garciay's avatar
garciay committed
81
  it = _params.find(params::angle);
82
  if (it != _params.cend()) {
83
    angle = converter::get_instance().string_to_int(it->second);
84
  }
85
  INTEGER station_type = 5; // passangerCar
garciay's avatar
garciay committed
86
  it = _params.find(params::station_type);
87
88
89
90
  if (it != _params.cend()) {
    station_type = converter::get_instance().string_to_int(it->second);
  }
  INTEGER country = 0;
garciay's avatar
garciay committed
91
  it = _params.find(params::country);
92
93
94
95
  if (it != _params.cend()) {
    country = converter::get_instance().string_to_int(it->second);
  }
  INTEGER type_of_address = 1; // Manual
garciay's avatar
garciay committed
96
  it = _params.find(params::type_of_address);
97
98
99
100
  if (it != _params.cend()) {
    type_of_address = converter::get_instance().string_to_int(it->second);
  }
  
garciay's avatar
garciay committed
101
  it = _params.find(params::device_mode);
102
  if (it != _params.cend()) {
103
    _device_mode = (1 == converter::get_instance().string_to_int(it->second));
104
  }
garciay's avatar
garciay committed
105
  it = _params.find(params::secured_mode);
garciay's avatar
garciay committed
106
107
  if (it != _params.cend()) {
    _secured_mode = (1 == converter::get_instance().string_to_int(it->second));
108
109
  } else {
    _params.insert(std::pair<std::string, std::string>(std::string("secured_mode"), "0"));
garciay's avatar
garciay committed
110
  }
garciay's avatar
garciay committed
111
  it = _params.find(params::encrypted_mode);
112
113
  if (it != _params.cend()) {
    _encrypted_mode = (1 == converter::get_instance().string_to_int(it->second));
114
115
  } else {
    _params.insert(std::pair<std::string, std::string>(std::string("encrypted_mode"), "0"));
116
  }
garciay's avatar
garciay committed
117
  it = _params.find(params::enable_security_checks);
garciay's avatar
garciay committed
118
119
120
  if (it != _params.cend()) {
    _enable_security_checks = (1 == converter::get_instance().string_to_int(it->second));
  }
garciay's avatar
garciay committed
121
  
122
  // Add broadcast address if needed
garciay's avatar
garciay committed
123
  it = _params.find(params::its_aid);
garciay's avatar
garciay committed
124
  if (it == _params.cend()) {
garciay's avatar
garciay committed
125
    _params.insert(std::pair<std::string, std::string>(std::string("its_aid"), "141"));
garciay's avatar
garciay committed
126
  }
garciay's avatar
garciay committed
127
  it = _params.find(params::mac_bc);
128
129
130
  if (it == _params.cend()) {
    _params.insert(std::pair<std::string, std::string>(std::string("mac_bc"), "FFFFFFFFFFFF"));
  }
garciay's avatar
garciay committed
131
  
garciay's avatar
garciay committed
132
  // Set up default security parameters value
133
  if (_secured_mode || _encrypted_mode) {
garciay's avatar
garciay committed
134
    loggers::get_instance().log("geonetworking_layer::geonetworking_layer: Setup secured mode");
135
    setup_secured_mode();
garciay's avatar
garciay committed
136
  }
garciay's avatar
garciay committed
137
138

  // Automatic beaconing mode
139
  fill_beacon(ll_address, station_type, country, type_of_address);
garciay's avatar
garciay committed
140
  params::const_iterator i = _params.find(params::beaconing);
141
142
143
144
145
  if ((i != _params.cend()) && (i->second.compare("1") == 0)) { // Immediate beaconing was requested
    // Prepare beaconing operation
    start_beaconing();
  }

146
  // Fill packet templates
147
148
149
150
151
152
  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
153
154
  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);
155
156
157

  // Add 4 leap seconds to convert to TAI (as Feb 2019)
  base_time::get_instance().set_leap_delay_us(4 * 1000000); // TODO Set it as parameter
158
} // End of init_params
159

garciay's avatar
garciay committed
160
void geonetworking_layer::sendMsg(const LibItsGeoNetworking__TestSystem::GeoNetworkingReq& p, params& params) {
garciay's avatar
garciay committed
161
  loggers::get_instance().log(">>> geonetworking_layer::sendMsg");
162
163
164
165

  // Encode GeoNetworking PDU
  OCTETSTRING data;
  _codec.encode(p.msgOut(), data);
garciay's avatar
garciay committed
166
  send_data(data, _params);
167
168
}

garciay's avatar
garciay committed
169
void geonetworking_layer::send_data(OCTETSTRING& data, params& params) {
garciay's avatar
garciay committed
170
  loggers::get_instance().log_msg(">>> geonetworking_layer::send_data: ", data);
171
  params.log();
172
173

  if (_device_mode) { // Need to build a GN packet
garciay's avatar
garciay committed
174
175
    params[params::certificate] = _params[params::certificate];
    params[params::hash] = _params[params::hash];
176
    params[params::signature] = _params[params::signature]; // TODO Should be removed
177
    if (build_geonetworking_pdu(data, params) != 0) {
178
179
180
      return;
    }
  }
Yann Garcia's avatar
Yann Garcia committed
181
  if (_secured_mode) { // Add Security support
garciay's avatar
garciay committed
182
    if (build_secured_pdu(data, params) != 0) {
garciay's avatar
garciay committed
183
184
185
      return;
    }
  }
186
187
188
189
190
191
  
  // 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
192
  send_to_all_layers(data, params);
193
  _sendData.unlock();
garciay's avatar
garciay committed
194
  loggers::get_instance().log("<<< geonetworking_layer::send_data");
195
196
}

filatov's avatar
filatov committed
197
198
199
const TTCN_RAWdescriptor_t _intx_raw_= {RAW_INTX,SG_NO,ORDER_MSB,ORDER_MSB,ORDER_LSB,ORDER_MSB,EXT_BIT_NO,ORDER_LSB,ORDER_LSB,TOP_BIT_INHERITED,0,0,0,8,0,NULL,-1,CharCoding::UNKNOWN};
const TTCN_Typedescriptor_t _intx_descr_={"IntX", NULL, &_intx_raw_, NULL, NULL, NULL, NULL, NULL, TTCN_Typedescriptor_t::DONTCARE};

garciay's avatar
garciay committed
200
void geonetworking_layer::receive_data(OCTETSTRING& data, params& params) { 
garciay's avatar
garciay committed
201
  loggers::get_instance().log_msg(">>> geonetworking_layer::receive_data: ", data);
garciay's avatar
garciay committed
202
203

  // Check security mode
garciay's avatar
garciay committed
204
  IEEE1609dot2::Ieee1609Dot2Data ieee_1609dot2_data;
garciay's avatar
garciay committed
205
206
207
  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
208
    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
209
    loggers::get_instance().log("geonetworking_layer::receive_data: basic_header_len = %d", basic_header_len);
garciay's avatar
garciay committed
210
211
    // 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;
212
    OCTETSTRING secured_data = OCTETSTRING(data.lengthof() - basic_header_len, static_cast<const unsigned char*>(data) + basic_header_len);
213
    ///////////////////
garciay's avatar
garciay committed
214
    // FIXME Check what to do with this!
215
    if (*static_cast<const unsigned char*>(secured_data) == 0x02) { // This is the old Security version format, discard it
YannGarcia's avatar
YannGarcia committed
216
217
218
219
        loggers::get_instance().warning("geonetworking_layer::receive_data: Security error");
        if (_enable_security_checks) {
          return;
        }
filatov's avatar
filatov committed
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
      INTEGER n; int r;
      TTCN_Buffer buf(secured_data);
      loggers::get_instance().warning("geonetworking_layer::receive_data: Old security format, parse manualy");

      buf.increase_pos(1); // skip version
      n.decode(_intx_descr_, buf, TTCN_EncDec::CT_RAW);
      if(n > (int)buf.get_read_len()){
        loggers::get_instance().warning("geonetworking_layer::receive_data: Broken security headers");
    	return;
      }
      buf.increase_pos((int)n); // skip headers

      // payload type
      r = *buf.get_read_data();
      buf.increase_pos(1);
      if(r != 3){
        n.decode(_intx_descr_, buf, TTCN_EncDec::CT_RAW);
        //payload length
          if(n > (int)buf.get_read_len()){
            loggers::get_instance().warning("geonetworking_layer::receive_data: Broken security payload");
        	return;
          }
          unsecured_gn_payload = OCTETSTRING(n, buf.get_read_data());
      }
YannGarcia's avatar
YannGarcia committed
244
    } else {
filatov's avatar
filatov committed
245
246
247
248
249
250
      ///////////////////
      if (security_services::get_instance().verify_and_extract_gn_payload(secured_data, _enable_security_checks, ieee_1609dot2_data, unsecured_gn_payload, params) != 0) {
        loggers::get_instance().warning("geonetworking_layer::receive_data: Security error");
        if (_enable_security_checks) {
          return;
        }
garciay's avatar
garciay committed
251
252
253
      }
    }
    // Update data
garciay's avatar
garciay committed
254
    loggers::get_instance().log_msg("geonetworking_layer::receive_data: Unsecured payload: ", unsecured_gn_payload);
255
    data = OCTETSTRING(basic_header_len, static_cast<const unsigned char*>(data)) + unsecured_gn_payload;
garciay's avatar
garciay committed
256
257
  }
  
258
  // Decode the payload
garciay's avatar
garciay committed
259
  loggers::get_instance().log_msg("geonetworking_layer::receive_data: Geonetworking payload to decode: ", data);
260
261
  LibItsGeoNetworking__TestSystem::GeoNetworkingInd ind;
  _codec.decode(data, ind.msgIn(), &params);
garciay's avatar
garciay committed
262
  if (ind.msgIn().is_bound()) {
garciay's avatar
garciay committed
263
264
265
266
267
    // 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
268
269
270
271
    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
272
      const LibItsGeoNetworking__TypesAndValues::ExtendedHeader& ex = p.extendedHeader();
273
274
275
276
277
278
279
280
281
282
283
284
285
286
      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
287
288
289
290
291
          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
292
              eh->lsReplyHeader().seqNumber() = _sequence_number++;
293
294
295
296
297
              // Update destination
              eh->lsReplyHeader().dstPosVector().gnAddr() = sopv->gnAddr();
              eh->lsReplyHeader().dstPosVector().latitude() = sopv->latitude();
              eh->lsReplyHeader().dstPosVector().longitude() = sopv->longitude();
              // Update timestamp
Yann Garcia's avatar
Yann Garcia committed
298
              eh->lsReplyHeader().srcPosVector().timestamp__().set_long_long_val(base_time::get_instance().get_its_current_time_mod_ms());
299
300
              eh->lsReplyHeader().dstPosVector().timestamp__() = eh->lsReplyHeader().srcPosVector().timestamp__();
              
garciay's avatar
garciay committed
301
              loggers::get_instance().log_msg("geonetworking_layer::receive_data: ", *_ls_reply);
302
303
304
305
306
              
              // send it
              // Encode GeoNetworking PDU
              OCTETSTRING os;
              _codec.encode(*_ls_reply, os);
307
              // Apply signature
garciay's avatar
garciay committed
308
              if (_secured_mode) {
309
                if (build_secured_pdu(data, _params) != 0) {
garciay's avatar
garciay committed
310
311
312
                  return;
                }
              }
313
314
315
316
317
318
              // 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
319
              send_to_all_layers(os, params);
320
321
              _sendData.unlock();
            } else {
garciay's avatar
garciay committed
322
              loggers::get_instance().error("geonetworking_layer::send_data: Wrong cast");
323
324
325
              return;
            }
          }
326
327
328
329
330
331
332
333
        } 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, nothing to do
garciay's avatar
garciay committed
334
      loggers::get_instance().log("geonetworking_layer::receive_data: sopv is bound: %d", sopv->is_bound());
335
336
337
338
339
340
341
342
      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)) {
YannGarcia's avatar
YannGarcia committed
343
      loggers::get_instance().log_msg("geonetworking_layer::receive_data: Pass beaconing filtering: ", sopv->gnAddr().mid());
344
      if (_pass_beacon_table.empty()) { // Discard beacon
garciay's avatar
garciay committed
345
        loggers::get_instance().log("geonetworking_layer::receive_data: Pass beaconing table empty, discard it");
346
347
348
        return;
      } else { // Check beacon filter for StartPassBeaconing/Stop
        if (!_pass_beacon_table.has_entry(sopv->gnAddr().mid())) { // Discard beacon
garciay's avatar
garciay committed
349
          loggers::get_instance().log_msg("geonetworking_layer::receive_data: Not in pass beaconing table, discard it", *sopv);
350
351
352
353
          return;
        } // else, continue
      }
    } // else, continue
garciay's avatar
garciay committed
354
355
  } else {
    // Inavlid GeoNetworking payload, discard it
garciay's avatar
garciay committed
356
    loggers::get_instance().warning("geonetworking_layer::receive_data: Failed to decode payload, discard it");
garciay's avatar
garciay committed
357
358
    return;
  }
359
360
361

  // Add lower layers parameters
  // 1. Destination MAC address
garciay's avatar
garciay committed
362
  params::const_iterator it = params.find(params::mac_dst);
363
  if (it != params.cend()) {
garciay's avatar
garciay committed
364
    loggers::get_instance().log("geonetworking_layer::receive_data: dst=%s", it->second.c_str());
365
366
367
368
369
    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
370
  it = params.find(_params[params::its_aid]);
371
  if (it != params.cend()) {
garciay's avatar
garciay committed
372
    loggers::get_instance().log("geonetworking_layer::receive_data: ssp=%s", it->second.c_str());
garciay's avatar
garciay committed
373
    ind.ssp() = oct2bit(str2oct(CHARSTRING(it->second.c_str())));
374
375
376
377
  } else {
    ind.ssp().set_to_omit();
  }
  // 3. its_aid
garciay's avatar
garciay committed
378
  it = params.find(params::its_aid);
379
  if (it != params.cend()) {
garciay's avatar
garciay committed
380
    loggers::get_instance().log("geonetworking_layer::receive_data: its_aid=%s", it->second.c_str());
381
382
383
384
385
386
    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
387
  it = params.find(params::gn_payload);
388
  if (it != params.cend()) {
garciay's avatar
garciay committed
389
    loggers::get_instance().log("geonetworking_layer::receive_data: gn_payload=%s", it->second.c_str());
390
    OCTETSTRING os(str2oct(CHARSTRING(it->second.c_str())));
garciay's avatar
garciay committed
391
    receive_to_all_layers(os, params);
392
  } else {
garciay's avatar
garciay committed
393
    loggers::get_instance().warning("geonetworking_layer::receive_data: No payload to pass to upper layers");
394
  }
395
  
396
  // Pass it to the ports
garciay's avatar
garciay committed
397
  to_all_upper_ports(ind, params);
398
399
}

garciay's avatar
garciay committed
400
OCTETSTRING geonetworking_layer::trigger_ac_event(OCTETSTRING& data, params& params)
401
{
garciay's avatar
garciay committed
402
  loggers::get_instance().log_to_hexa(">>> geonetworking_layer::trigger_ac_event: ", data);
403
404
405
406
407


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

garciay's avatar
garciay committed
408
409
410
void geonetworking_layer::start_beaconing() {
  loggers::get_instance().log(">>> geonetworking_layer::start_beaconing");
  //loggers::get_instance().log_msg("geonetworking_layer::start_beaconing: _beacon=", *_beacon);
411
412

  // Establish handler for timer signal
garciay's avatar
garciay committed
413
  loggers::get_instance().log("geonetworking_layer::start_beaconing: Establishing handler for signal %d\n", _signal_id);
414
415
416
  _sa.sa_flags = SA_SIGINFO;
  _sa.sa_sigaction = timer_irq_sigalrm_handler;
  sigemptyset(&_sa.sa_mask);
417
  if (sigaction(_signal_id, &_sa, nullptr) == -1) {
garciay's avatar
garciay committed
418
    loggers::get_instance().error("geonetworking_layer::start_beaconing: Sigaction failure: %d", errno);
419
420
  }
  // Block timer signal temporarily
garciay's avatar
garciay committed
421
  loggers::get_instance().log("geonetworking_layer::start_beaconing: Blocking signal %d\n", _signal_id);
422
423
  sigemptyset(&_mask);
  sigaddset(&_mask, _signal_id);
424
  if (sigprocmask(SIG_SETMASK, &_mask, nullptr) == -1) {
garciay's avatar
garciay committed
425
    loggers::get_instance().error("geonetworking_layer::start_beaconing: Sigprocmask failure: %d", errno);
426
427
428
429
  }	
  // Create the timer 
  _sev.sigev_notify = SIGEV_SIGNAL;
  _sev.sigev_signo = _signal_id; // Use signal alarm
garciay's avatar
garciay committed
430
  _sev.sigev_value.sival_ptr = this; // The geonetworking_layer object address
431
  if (timer_create(CLOCK_REALTIME, &_sev, &_timerid) == -1) {
garciay's avatar
garciay committed
432
    loggers::get_instance().error("geonetworking_layer::start_beaconing: Timer failure: %d", errno);
433
  }
garciay's avatar
garciay committed
434
  loggers::get_instance().log("geonetworking_layer::start_beaconing: timer ID is 0x%x\n", (long)_timerid);
435
436
  // Start the timer
  unsigned int expiry = 1000; // Default expiry time 1000ms
garciay's avatar
garciay committed
437
  params::const_iterator i = _params.find("expiry");
438
  if (i != _params.cend()) {
439
    expiry = static_cast<unsigned int>(std::strtoul(i->second.c_str(), nullptr, 10));
440
441
442
443
444
445
  }
  _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;
446
  if (timer_settime(_timerid, 0, &_its, nullptr) == -1) {
garciay's avatar
garciay committed
447
    loggers::get_instance().error("geonetworking_layer::start_beaconing: Sigprocmask failure: %d", errno);
448
449
  }
  // Unlock the timer signal, so that timer notification can be delivered
garciay's avatar
garciay committed
450
  loggers::get_instance().log("geonetworking_layer::start_beaconing: Unblocking signal %d\n", _signal_id);
451
  if (sigprocmask(SIG_UNBLOCK, &_mask, nullptr) == -1) {
garciay's avatar
garciay committed
452
    loggers::get_instance().error("geonetworking_layer::start_beaconing: Sigprocmask failure: %d", errno);
453
454
455
  }
} // End of start_beaconing method

garciay's avatar
garciay committed
456
457
void geonetworking_layer::start_beaconing(const LibItsGeoNetworking__TypesAndValues::GeoNetworkingPdu& p_beacon) {
  loggers::get_instance().log_msg(">>> geonetworking_layer::start_beaconing", p_beacon);
458
459
460
461
462
463
464
465
466
467

  // 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
468
469
void geonetworking_layer::stop_beaconing() {
  loggers::get_instance().log(">>> geonetworking_layer::stop_beaconing");
470
471
  
  // Block timer signal temporarily
garciay's avatar
garciay committed
472
  loggers::get_instance().log("geonetworking_layer::stop_beaconing: Blocking signal %d\n", _signal_id);
473
474
  sigemptyset(&_mask);
  sigaddset(&_mask, _signal_id);
475
  if (sigprocmask(SIG_SETMASK, &_mask, nullptr) == -1) {
garciay's avatar
garciay committed
476
    loggers::get_instance().error("geonetworking_layer::stop_beaconing: Sigprocmask failure: %d", errno);
477
478
479
480
481
  }	
  timer_delete(_timerid);
  _timerid = 0;
} // End of stop_beaconing method

garciay's avatar
garciay committed
482
483
void geonetworking_layer::send_beacon() {
  loggers::get_instance().log(">>> geonetworking_layer::send_beacon");
484
485
  
  ExtendedHeader* eh = static_cast<ExtendedHeader *>(_beacon->gnPacket().packet().extendedHeader().get_opt_value());
486
  if (eh == nullptr) {
garciay's avatar
garciay committed
487
    loggers::get_instance().error("geonetworking_layer::send_beacon: Wrong cast");
488
489
  }
  // Update timestamp
Yann Garcia's avatar
Yann Garcia committed
490
  eh->beaconHeader().srcPosVector().timestamp__().set_long_long_val((unsigned int)base_time::get_instance().get_its_current_time_mod_ms());
garciay's avatar
garciay committed
491
  //loggers::get_instance().log_msg("geonetworking_layer::send_beacon: ", *_beacon);
492
493
494
495
  // 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
496
  params params(_params);
Yann Garcia's avatar
Yann Garcia committed
497
  if (_secured_mode) { // Apply Security
498
    if (build_secured_pdu(data, _params) != 0) {
garciay's avatar
garciay committed
499
500
501
      return;
    }
  }
502
  // Send it
503
504
505
506
507
  // 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
508
  send_to_all_layers(data, params);
509
  _sendData.unlock();
510
  
garciay's avatar
garciay committed
511
  //loggers::get_instance().log("<<< geonetworking_layer::send_beacon");
512
513
} // End of send_beacon method

garciay's avatar
garciay committed
514
515
void geonetworking_layer::start_pass_beaconing(const LibItsGeoNetworking__TypesAndValues::BeaconHeader& p_beacon) {
  loggers::get_instance().log_msg(">>> geonetworking_layer::start_pass_beaconing", p_beacon);
516
517
518
519
520
521
522

  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
523
524
void geonetworking_layer::stop_pass_beaconing() {
  loggers::get_instance().log(">>> geonetworking_layer::stop_pass_beaconing");
525
526
527
528

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

garciay's avatar
garciay committed
529
530
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);
531

garciay's avatar
garciay committed
532
  loggers::get_instance().log("geonetworking_layer::enable_secured_mode: _secured_mode = %x", _secured_mode);
533
  if (!_secured_mode) {
garciay's avatar
garciay committed
534
    loggers::get_instance().log("geonetworking_layer::enable_secured_mode: Setup secured mode");
535
536
537
538
    _secured_mode = true;
    setup_secured_mode();
  }
  _enable_security_checks = p_enforce_security;
garciay's avatar
garciay committed
539
  params::const_iterator it = _params.find(params::certificate);
540
541
542
  if (it == _params.cend()) {
    _params.insert(std::pair<std::string, std::string>(std::string("certificate"), p_certificate_id));
  } else {
garciay's avatar
garciay committed
543
    _params[params::certificate] = p_certificate_id;
544
  }
garciay's avatar
garciay committed
545
  loggers::get_instance().log("geonetworking_layer::enable_secured_mode: Certificate to be used: '%s'", _params[params::certificate].c_str());
546
547
  
  return 0;
548
549
}

garciay's avatar
garciay committed
550
551
int geonetworking_layer::disable_secured_mode() {
  loggers::get_instance().log(">>> geonetworking_layer::disable_secured_mode");
garciay's avatar
garciay committed
552
553
554
555
556
  _secured_mode = false;
  _enable_security_checks = false;
  return 0;
}

garciay's avatar
garciay committed
557
const LongPosVector* geonetworking_layer::get_lpv(const GN__Address& p_gn_address)
558
{
garciay's avatar
garciay committed
559
  loggers::get_instance().log_msg(">>> geonetworking_layer::get_lpv", p_gn_address);
560
561
562
563
564
565
566
567
  
  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

568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
const LibItsGeoNetworking__TypesAndValues::BasicHeader geonetworking_layer::fill_basic_header() const {
  return LibItsGeoNetworking__TypesAndValues::BasicHeader(
                                                          1, // GeoNetworking version
                                                          BasicNextHeader(
                                                                          BasicNextHeader::e__commonHeader
                                                                          ),
                                                          0,
                                                          Lifetime(
                                                                   4,
                                                                   LtBase(LtBase::e__50ms)
                                                                   ),
                                                          1
                                                          );
}

garciay's avatar
garciay committed
583
void geonetworking_layer::fill_beacon(const OCTETSTRING& p_ll_address, const INTEGER p_station_type, const INTEGER p_country, const INTEGER type_of_address)
584
585
586
587
588
589
590
591
592
593
594
{
  _beacon = new GeoNetworkingPdu();
  HeaderTST h;
  h.beaconHdr() = BeaconHeaderType(
                                   HeaderType(HeaderType::e__beacon),
                                   0
                                   );
  ExtendedHeader eh;
  eh.beaconHeader() = BeaconHeader(
                                   LongPosVector(
                                                 GN__Address(
595
596
597
                                                             TypeOfAddress((TypeOfAddress)type_of_address),
                                                             StationType((StationType)p_station_type),
                                                             p_country,
598
599
600
                                                             p_ll_address
                                                             ),
                                                 0,
601
602
                                                 _latitude,
                                                 _longitude,
603
604
605
606
607
                                                 int2bit(0, 1),
                                                 0,
                                                 0
                                                 )
                                   );
608
  _beacon->basicHeader() = fill_basic_header();
609
610
611
612
613
614
615
616
  _beacon->gnPacket().packet() = GnNonSecuredPacket(
                                                    CommonHeader(
                                                                 NextHeader(
                                                                            NextHeader::e__any
                                                                            ),
                                                                 0,
                                                                 h,
                                                                 TrafficClass(
YannGarcia's avatar
YannGarcia committed
617
                                                                              SCF(SCF::e__scfEnabled),
618
619
620
621
622
623
624
625
626
627
628
629
630
                                                                              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
631
  //loggers::get_instance().log_msg("geonetworking_layer::fill_beacon: beacon value: ", *_beacon);
632
633
} // End of fill_beacon method
  
garciay's avatar
garciay committed
634
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)
635
{
636
  _gbc_packet = new GeoNetworkingPdu();
637
638
639
640
641
642
643
644
645
646
647
  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
648
649
                                                                       TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                                       StationType(StationType::e__passengerCar), // TODO Use params
650
651
652
653
                                                                       33,
                                                                       p_ll_address
                                                                       ),
                                                           0,
654
655
                                                           _latitude,
                                                           _longitude,
656
657
658
659
660
661
662
663
664
665
666
                                                           int2bit(1, 1), // PAI
                                                           0,
                                                           0
                                                           ),
                                             p_geoAreaPosLatitude,
                                             p_geoAreaPosLongitude,
                                             p_distanceA,
                                             p_distanceB,
                                             p_angle,
                                             0
                                              );
667
  _gbc_packet->basicHeader() = fill_basic_header();
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
  _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
690
  //loggers::get_instance().log_msg("geonetworking_layer::fill_gbc_packet: packet value: ", *_gbc_packet);
691
692
} // End of fill_gbc_packet method

garciay's avatar
garciay committed
693
void geonetworking_layer::fill_shb_packet(const OCTETSTRING& p_ll_address)
694
695
696
697
698
699
700
701
702
703
704
{
  _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
705
706
                                                       TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                       StationType(StationType::e__passengerCar), // TODO Use params
707
708
                                                       33,
                                                       p_ll_address
709
                                                       ),
710
                                           0,
711
712
                                           _latitude,
                                           _longitude,
713
714
715
716
717
718
                                           int2bit(1, 1), // PAI
                                           0,
                                           0
                                           ),
                             0
                             );
719
  _shb_packet->basicHeader() = fill_basic_header();
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
  _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
742
  //loggers::get_instance().log_msg("geonetworking_layer::fill_shb_packet: packet value: ", *_shb_packet);
743
744
} // End of fill_shb_packet method

garciay's avatar
garciay committed
745
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
746
747
748
749
750
751
752
753
754
755
756
757
758
{
  _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
759
760
                                                       TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                       StationType(StationType::e__passengerCar), // TODO Use params
garciay's avatar
garciay committed
761
762
763
764
                                                       33,
                                                       p_ll_address
                                                       ),
                                           0,
765
766
                                           _latitude,
                                           _longitude,
garciay's avatar
garciay committed
767
768
769
770
771
                                           int2bit(1, 1), // PAI
                                           0,
                                           0
                                           )
                             );
772
  _tsb_packet->basicHeader() = fill_basic_header();
garciay's avatar
garciay committed
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
  _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
795
  //loggers::get_instance().log_msg("geonetworking_layer::fill_tsb_packet: packet value: ", *_tsb_packet);
garciay's avatar
garciay committed
796
797
} // End of fill_tsb_packet method

garciay's avatar
garciay committed
798
void geonetworking_layer::fill_ls_reply(const OCTETSTRING& p_ll_address)
799
800
801
802
803
804
805
806
807
808
809
810
811
{
  _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
812
813
                                                               TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                               StationType(StationType::e__passengerCar), // TODO Use params
814
815
816
817
                                                               33,
                                                               p_ll_address
                                                               ),
                                                   0,
818
819
                                                   _latitude,
                                                   _longitude,
820
821
822
823
824
825
                                                   int2bit(1, 1), // PAI
                                                   0,
                                                   0
                                                   ),
                                     ShortPosVector(
                                                    GN__Address(
garciay's avatar
garciay committed
826
827
                                                                TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                                StationType(StationType::e__passengerCar), // TODO Use params
828
829
830
831
                                                                33,
                                                                p_ll_address
                                                                ),
                                                    0,
832
833
                                                    _latitude,
                                                    _longitude
834
835
                                                    )
                                     );
836
  _ls_reply->basicHeader() = fill_basic_header();
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
  _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
int geonetworking_layer::build_geonetworking_pdu(OCTETSTRING& data, params& params) {
garciay's avatar
garciay committed
869
  loggers::get_instance().log(">>> geonetworking_layer::build_geonetworking_pdu");
garciay's avatar
garciay committed
870
  //params.log();
871
872
  
  std::string next_header;
garciay's avatar
garciay committed
873
  params::const_iterator it = params.find(params::next_header);
874
875
876
877
  if (it != params.cend()) {
    next_header = it->second.c_str();
  }
  std::string header_type;
garciay's avatar
garciay committed
878
  it = params.find(params::header_type);
879
880
881
882
  if (it != params.cend()) {
    header_type = it->second.c_str();
  }
  std::string header_sub_type;
garciay's avatar
garciay committed
883
  it = params.find(params::header_sub_type);
884
885
886
  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
        return -1;
      }
      // Update NextHeader
garciay's avatar
garciay committed
897
      it = params.find(params::next_header);
898
899
900
901
902
903
904
905
906
      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
Yann Garcia's avatar
Yann Garcia committed
907
      eh->shbHeader().srcPosVector().timestamp__().set_long_long_val(static_cast<unsigned int>(base_time::get_instance().get_its_current_time_mod_ms()));
908
      
garciay's avatar
garciay committed
909
      loggers::get_instance().log_msg("geonetworking_layer::build_geonetworking_pdu: shb: ", *_shb_packet);
910
911
912
913
      // Encode GeoNetworking PDU
      OCTETSTRING os;
      _codec.encode(*_shb_packet, os);
      data = os;
garciay's avatar
garciay committed
914
915
916
    } else { // Use TSB
      ExtendedHeader* eh = static_cast<ExtendedHeader *>(_tsb_packet->gnPacket().packet().extendedHeader().get_opt_value());
      if (eh == nullptr) {
garciay's avatar
garciay committed
917
        loggers::get_instance().error("geonetworking_layer::build_geonetworking_pdu: Wrong cast");
garciay's avatar
garciay committed
918
919
920
921
922
        return -1;
      }
      // Update sequence number
      eh->tsbHeader().seqNumber() = _sequence_number++;
      // Update NextHeader
garciay's avatar
garciay committed
923
      it = params.find(params::next_header);
garciay's avatar
garciay committed
924
925
926
927
928
929
930
931
932
      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
Yann Garcia's avatar
Yann Garcia committed
933
      eh->tsbHeader().srcPosVector().timestamp__().set_long_long_val(static_cast<unsigned int>(base_time::get_instance().get_its_current_time_mod_ms()));
garciay's avatar
garciay committed
934
      
garciay's avatar
garciay committed
935
      loggers::get_instance().log_msg("geonetworking_layer::build_geonetworking_pdu: tsb: ", *_tsb_packet);
garciay's avatar
garciay committed
936
937
938
939
      // Encode GeoNetworking PDU
      OCTETSTRING os;
      _codec.encode(*_tsb_packet, os);
      data = os;
940
941
942
    }
  } else { // TODO To be continued
    // Default: Use GBC
garciay's avatar
garciay committed
943
    ExtendedHeader* eh = static_cast<ExtendedHeader *>(_gbc_packet->gnPacket().packet().extendedHeader().get_opt_value());
944
    if (eh == nullptr) {
garciay's avatar
garciay committed
945
      loggers::get_instance().error("geonetworking_layer::build_geonetworking_pdu: Wrong cast");
946
947
948
      return -1;
    }
    // Update NextHeader
garciay's avatar
garciay committed
949
    it = params.find(params::next_header);
950
951
952
953
954
955
956
957
958
959
960
    if (next_header.compare("btpB") == 0) {
      _gbc_packet->gnPacket().packet().commonHeader().nextHeader() = NextHeader::e__btpB;