geonetworking_layer.cc 52.8 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
filatov's avatar
filatov committed
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
      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());
      }
    }else{
      ///////////////////
      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
247
248
249
      }
    }
    // Update data
garciay's avatar
garciay committed
250
    loggers::get_instance().log_msg("geonetworking_layer::receive_data: Unsecured payload: ", unsecured_gn_payload);
251
    data = OCTETSTRING(basic_header_len, static_cast<const unsigned char*>(data)) + unsecured_gn_payload;
garciay's avatar
garciay committed
252
253
  }
  
254
  // Decode the payload
garciay's avatar
garciay committed
255
  loggers::get_instance().log_msg("geonetworking_layer::receive_data: Geonetworking payload to decode: ", data);
256
257
  LibItsGeoNetworking__TestSystem::GeoNetworkingInd ind;
  _codec.decode(data, ind.msgIn(), &params);
garciay's avatar
garciay committed
258
  if (ind.msgIn().is_bound()) {
garciay's avatar
garciay committed
259
260
261
262
263
    // 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
264
265
266
267
    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
268
      const LibItsGeoNetworking__TypesAndValues::ExtendedHeader& ex = p.extendedHeader();
269
270
271
272
273
274
275
276
277
278
279
280
281
282
      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
283
284
285
286
287
          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
288
              eh->lsReplyHeader().seqNumber() = _sequence_number++;
289
290
291
292
293
              // 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
294
              eh->lsReplyHeader().srcPosVector().timestamp__().set_long_long_val(base_time::get_instance().get_its_current_time_mod_ms());
295
296
              eh->lsReplyHeader().dstPosVector().timestamp__() = eh->lsReplyHeader().srcPosVector().timestamp__();
              
garciay's avatar
garciay committed
297
              loggers::get_instance().log_msg("geonetworking_layer::receive_data: ", *_ls_reply);
298
299
300
301
302
              
              // send it
              // Encode GeoNetworking PDU
              OCTETSTRING os;
              _codec.encode(*_ls_reply, os);
303
              // Apply signature
garciay's avatar
garciay committed
304
              if (_secured_mode) {
305
                if (build_secured_pdu(data, _params) != 0) {
garciay's avatar
garciay committed
306
307
308
                  return;
                }
              }
309
310
311
312
313
314
              // 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
315
              send_to_all_layers(os, params);
316
317
              _sendData.unlock();
            } else {
garciay's avatar
garciay committed
318
              loggers::get_instance().error("geonetworking_layer::send_data: Wrong cast");
319
320
321
              return;
            }
          }
322
323
324
325
326
327
328
329
330
331
332
333
334
335
        } 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
336
      loggers::get_instance().log("geonetworking_layer::receive_data: sopv is bound: %d", sopv->is_bound());
337
338
339
340
341
342
343
344
      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
345
      loggers::get_instance().log_msg("geonetworking_layer::receive_data: Pass beaconing filtering", sopv->gnAddr().mid());
346
      if (_pass_beacon_table.empty()) { // Discard beacon
garciay's avatar
garciay committed
347
        loggers::get_instance().log("geonetworking_layer::receive_data: Pass beaconing table empty, discard it");
348
349
350
        return;
      } else { // Check beacon filter for StartPassBeaconing/Stop
        if (!_pass_beacon_table.has_entry(sopv->gnAddr().mid())) { // Discard beacon
garciay's avatar
garciay committed
351
          loggers::get_instance().log_msg("geonetworking_layer::receive_data: Not in pass beaconing table, discard it", *sopv);
352
353
354
355
          return;
        } // else, continue
      }
    } // else, continue
garciay's avatar
garciay committed
356
357
  } else {
    // Inavlid GeoNetworking payload, discard it
garciay's avatar
garciay committed
358
    loggers::get_instance().warning("geonetworking_layer::receive_data: Failed to decode payload, discard it");
garciay's avatar
garciay committed
359
360
    return;
  }
361
362
363

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

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


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

garciay's avatar
garciay committed
800
void geonetworking_layer::fill_ls_reply(const OCTETSTRING& p_ll_address)
801
802
803
804
805
806
807
808
809
810
811
812
813
{
  _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
814
815
                                                               TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                               StationType(StationType::e__passengerCar), // TODO Use params
816
817
818
819
                                                               33,
                                                               p_ll_address
                                                               ),
                                                   0,
820
821
                                                   _latitude,
                                                   _longitude,
822
823
824
825
826
827
                                                   int2bit(1, 1), // PAI
                                                   0,
                                                   0
                                                   ),
                                     ShortPosVector(
                                                    GN__Address(
garciay's avatar
garciay committed
828
829
                                                                TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                                StationType(StationType::e__passengerCar), // TODO Use params
830
831
832
833
                                                                33,
                                                                p_ll_address
                                                                ),
                                                    0,
834
835
                                                    _latitude,
                                                    _longitude
836
837
                                                    )
                                     );
838
  _ls_reply->basicHeader() = fill_basic_header();
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
  _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
861
  //loggers::get_instance().log_msg("geonetworking_layer::fill_ls_reply: packet value: ", *_ls_reply);
862
} // End of fill_ls_reply method
863

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

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

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