geonetworking_layer.cc 54.4 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
} // End of init_params
156

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

  // Encode GeoNetworking PDU
  OCTETSTRING data;
  _codec.encode(p.msgOut(), data);
garciay's avatar
garciay committed
163
  send_data(data, _params);
164
165
}

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

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

filatov's avatar
filatov committed
194
195
196
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
197
void geonetworking_layer::receive_data(OCTETSTRING& data, params& params) { 
garciay's avatar
garciay committed
198
  loggers::get_instance().log_msg(">>> geonetworking_layer::receive_data: ", data);
garciay's avatar
garciay committed
199
200

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

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

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


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

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

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

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

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

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

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

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

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

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

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

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

garciay's avatar
garciay committed
556
const LongPosVector* geonetworking_layer::get_lpv(const GN__Address& p_gn_address)
557
{
garciay's avatar
garciay committed
558
  loggers::get_instance().log_msg(">>> geonetworking_layer::get_lpv", p_gn_address);
559
560
561
562
563
564
565
566
  
  const LongPosVector* lpv = nullptr;
  if (_location_table.has_entry(p_gn_address.mid())) {
    lpv = _location_table.get_entry(p_gn_address.mid());
  }
  return lpv;
} // End of get_lpv

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

garciay's avatar
garciay committed
699
void geonetworking_layer::fill_shb_packet(const OCTETSTRING& p_ll_address)
700
701
702
703
704
705
706
707
708
709
710
{
  _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
711
712
                                                       TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                       StationType(StationType::e__passengerCar), // TODO Use params
713
714
                                                       33,
                                                       p_ll_address
715
                                                       ),
716
                                           0,
717
718
                                           _latitude,
                                           _longitude,
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
                                           int2bit(1, 1), // PAI
                                           0,
                                           0
                                           ),
                             0
                             );
  _shb_packet->basicHeader() = BasicHeader(
                                           0,
                                           BasicNextHeader(
                                                           BasicNextHeader::e__commonHeader
                                                           ),
                                           0,
                                           Lifetime(
                                                    4,
                                                    LtBase(LtBase::e__50ms)
                                                    ),
                                           1
                                           );
  _shb_packet->gnPacket().packet() = GnNonSecuredPacket(
                                                        CommonHeader(
                                                                     NextHeader(
                                                                                NextHeader::e__btpA
                                                                                ),
                                                                     0,
                                                                     h,
                                                                     TrafficClass(
                                                                                  SCF(SCF::e__scfDisabled),
                                                                                  ChannelOffload(ChannelOffload::e__choffDisabled),
                                                                                  0
                                                                                  ),
                                                                     int2bit(128, 8), // Mobile stationnary flag set
                                                                     0,
                                                                     1,
                                                                     0
                                                                     ),
                                                        OPTIONAL<ExtendedHeader>(eh),
                                                        OPTIONAL<GnRawPayload>()
                                                        );
  _shb_packet->gnPacket().packet().payload().set_to_omit();
  _shb_packet->gnPacket().securedMsg().set_to_omit();
garciay's avatar
garciay committed
759
  //loggers::get_instance().log_msg("geonetworking_layer::fill_shb_packet: packet value: ", *_shb_packet);
760
761
} // End of fill_shb_packet method

garciay's avatar
garciay committed
762
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
763
764
765
766
767
768
769
770
771
772
773
774
775
{
  _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
776
777
                                                       TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                       StationType(StationType::e__passengerCar), // TODO Use params
garciay's avatar
garciay committed
778
779
780
781
                                                       33,
                                                       p_ll_address
                                                       ),
                                           0,
782
783
                                           _latitude,
                                           _longitude,
garciay's avatar
garciay committed
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
                                           int2bit(1, 1), // PAI
                                           0,
                                           0
                                           )
                             );
  _tsb_packet->basicHeader() = BasicHeader(
                                           0,
                                           BasicNextHeader(
                                                           BasicNextHeader::e__commonHeader
                                                           ),
                                           0,
                                           Lifetime(
                                                    4,
                                                    LtBase(LtBase::e__50ms)
                                                    ),
                                           p_hop_number
                                           );
  _tsb_packet->gnPacket().packet() = GnNonSecuredPacket(
                                                        CommonHeader(
                                                                     NextHeader(
                                                                                NextHeader::e__btpA
                                                                                ),
                                                                     0,
                                                                     h,
                                                                     TrafficClass(
                                                                                  SCF(SCF::e__scfDisabled),
                                                                                  ChannelOffload(ChannelOffload::e__choffDisabled),
                                                                                  0
                                                                                  ),
                                                                     int2bit(128, 8), // Mobile stationnary flag set
                                                                     0,
                                                                     p_max_hop_limit,
                                                                     0
                                                                     ),
                                                        OPTIONAL<ExtendedHeader>(eh),
                                                        OPTIONAL<GnRawPayload>()
                                                        );
  _tsb_packet->gnPacket().packet().payload().set_to_omit();
  _tsb_packet->gnPacket().securedMsg().set_to_omit();
garciay's avatar
garciay committed
823
  //loggers::get_instance().log_msg("geonetworking_layer::fill_tsb_packet: packet value: ", *_tsb_packet);
garciay's avatar
garciay committed
824
825
} // End of fill_tsb_packet method

garciay's avatar
garciay committed
826
void geonetworking_layer::fill_ls_reply(const OCTETSTRING& p_ll_address)
827
828
829
830
831
832
833
834
835
836
837
838
839
{
  _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
840
841
                                                               TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                               StationType(StationType::e__passengerCar), // TODO Use params
842
843
844
845
                                                               33,
                                                               p_ll_address
                                                               ),
                                                   0,
846
847
                                                   _latitude,
                                                   _longitude,
848
849
850
851
852
853
                                                   int2bit(1, 1), // PAI
                                                   0,
                                                   0
                                                   ),
                                     ShortPosVector(
                                                    GN__Address(
garciay's avatar
garciay committed
854
855
                                                                TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                                StationType(StationType::e__passengerCar), // TODO Use params
856
857
858
859
                                                                33,
                                                                p_ll_address
                                                                ),
                                                    0,
860
861
                                                    _latitude,
                                                    _longitude
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
                                                    )
                                     );
  _ls_reply->basicHeader() = BasicHeader(
                                         0,
                                         BasicNextHeader(
                                                         BasicNextHeader::e__commonHeader
                                                         ),
                                         0,
                                         Lifetime(
                                                  4,
                                                  LtBase(LtBase::e__50ms)
                                                  ),
                                         5
                                         );
  _ls_reply->gnPacket().packet() = GnNonSecuredPacket(
                                                      CommonHeader(
                                                                   NextHeader(
                                                                              NextHeader::e__any
                                                                              ),
                                                                   0,
                                                                   h,
                                                                   TrafficClass(
                                                                                SCF(SCF::e__scfDisabled),
                                                                                ChannelOffload(ChannelOffload::e__choffDisabled),
                                                                                0
                                                                                ),
                                                                   int2bit(128, 8), // Mobile stationnary flag set
                                                                   0,
                                                                   5,
                                                                   0
                                                                   ),
                                                      OPTIONAL<ExtendedHeader>(eh),
                                                      OPTIONAL<GnRawPayload>()
                                                      );
  _ls_reply->gnPacket().packet().payload().set_to_omit();
  _ls_reply->gnPacket().securedMsg().set_to_omit();
garciay's avatar
garciay committed
898
  //loggers::get_instance().log_msg("geonetworking_layer::fill_ls_reply: packet value: ", *_ls_reply);
899
} // End of fill_ls_reply method
900

garciay's avatar
garciay committed
901
902
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);
903

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

garciay's avatar
garciay committed
907
int geonetworking_layer::build_geonetworking_pdu(OCTETSTRING& data, params& params) {
garciay's avatar
garciay committed
908
  loggers::get_instance().log(">>> geonetworking_layer::build_geonetworking_pdu");
garciay's avatar
garciay committed
909
  //params.log();
910
911
  
  std::string next_header;
garciay's avatar
garciay committed
912
  params::const_iterator it = params.find(params::next_header);
913
914
915
916
  if (it != params.cend()) {
    next_header = it->second.c_str();
  }
  std::string header_type;
garciay's avatar
garciay committed
917
  it = params.find(params::header_type);
918
919
920
921
  if (it != params.cend()) {
    header_type = it->second.c_str();
  }
  std::string header_sub_type;
garciay's avatar
garciay committed
922
  it = params.find(params::header_sub_type);
923
924
925
  if (it != params.cend()) {
    header_sub_type = it->second.c_str();
  }
garciay's avatar
garciay committed
926
  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());
927
928
929
930
931
  
  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
932
        loggers::get_instance().error("geonetworking_layer::build_geonetworking_pdu: Wrong cast");
933
934
935
        return -1;
      }
      // Update NextHeader
garciay's avatar
garciay committed
936
      it = params.find(params::next_header);
937
938
939
940
941
942
943
944
945
      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
garciay's avatar
garciay committed
946
      eh->shbHeader().srcPosVector().timestamp__().set_long_long_val(static_cast<unsigned int>(base_time::get_instance().get_its_current_time()));
947
      
garciay's avatar
garciay committed
948
      loggers::get_instance().log_msg("geonetworking_layer::build_geonetworking_pdu: shb: ", *_shb_packet);
949
950
951
952
      // Encode GeoNetworking PDU
      OCTETSTRING os;
      _codec.encode(*_shb_packet, os);
      data = os;
garciay's avatar
garciay committed
953
954
955
    } else { // Use TSB
      ExtendedHeader* eh = static_cast<ExtendedHeader *>(_tsb_packet->gnPacket().packet().extendedHeader().get_opt_value());
      if (eh == nullptr) {
garciay's avatar
garciay committed
956
        loggers::get_instance().error("geonetworking_layer::build_geonetworking_pdu: Wrong cast");
garciay's avatar
garciay committed
957
958
959
960
961
        return -1;
      }
      // Update sequence number
      eh->tsbHeader().seqNumber() = _sequence_number++;
      // Update NextHeader
garciay's avatar
garciay committed
962
      it = params.find(params::next_header);
garciay's avatar
garciay committed
963
964
965
966
967
968
969
970
971
      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
garciay's avatar
garciay committed
972
      eh->tsbHeader().srcPosVector().timestamp__().set_long_long_val(static_cast<unsigned int>(base_time::get_instance().get_its_current_time()));
garciay's avatar
garciay committed
973
      
garciay's avatar
garciay committed
974
      loggers::get_instance().log_msg("geonetworking_layer::build_geonetworking_pdu: tsb: ", *_tsb_packet);
garciay's avatar
garciay committed
975
976
977
978
      // Encode GeoNetworking PDU
      OCTETSTRING os;
      _codec.encode(*_tsb_packet, os);
      data = os;
979
980
981
    }
  } else { // TODO To be continued
    // Default: Use GBC
garciay's avatar
garciay committed
982
    ExtendedHeader* eh = static_cast<ExtendedHeader *>(_gbc_packet->gnPacket().packet().extendedHeader().get_opt_value());
983
    if (eh == nullptr) {
garciay's avatar
garciay committed
984
      loggers::get_instance().error("geonetworking_layer::build_geonetworking_pdu: Wrong cast");
985
986
987
      return -1;
    }
    // Update NextHeader
garciay's avatar
garciay committed
988
    it = params.find(params::next_header);
989
990
991
992
993
994
995
996
997
998
999
    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
garciay's avatar
garciay committed
1000
    eh->geoBroadcastHeader().srcPosVector().timestamp__().set_long_long_val(static_cast<unsigned int>(base_time::get_instance().get_its_current_time()));
1001
    
garciay's avatar
garciay committed
1002
    loggers::get_instance().log_msg("geonetworking_layer::build_geonetworking_pdu: gbc: ", *_gbc_packet);
garciay's avatar