geonetworking_layer.cc 54.9 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

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

garciay's avatar
garciay committed
702
void geonetworking_layer::fill_shb_packet(const OCTETSTRING& p_ll_address)
703
704
705
706
707
708
709
710
711
712
713
{
  _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
714
715
                                                       TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                       StationType(StationType::e__passengerCar), // TODO Use params
716
717
                                                       33,
                                                       p_ll_address
718
                                                       ),
719
                                           0,
720
721
                                           _latitude,
                                           _longitude,
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
759
760
761
                                           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
762
  //loggers::get_instance().log_msg("geonetworking_layer::fill_shb_packet: packet value: ", *_shb_packet);
763
764
} // End of fill_shb_packet method

garciay's avatar
garciay committed
765
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
766
767
768
769
770
771
772
773
774
775
776
777
778
{
  _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
779
780
                                                       TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                       StationType(StationType::e__passengerCar), // TODO Use params
garciay's avatar
garciay committed
781
782
783
784
                                                       33,
                                                       p_ll_address
                                                       ),
                                           0,
785
786
                                           _latitude,
                                           _longitude,
garciay's avatar
garciay committed
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
823
824
825
                                           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
826
  //loggers::get_instance().log_msg("geonetworking_layer::fill_tsb_packet: packet value: ", *_tsb_packet);
garciay's avatar
garciay committed
827
828
} // End of fill_tsb_packet method

garciay's avatar
garciay committed
829
void geonetworking_layer::fill_ls_reply(const OCTETSTRING& p_ll_address)
830
831
832
833
834
835
836
837
838
839
840
841
842
{
  _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
843
844
                                                               TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                               StationType(StationType::e__passengerCar), // TODO Use params
845
846
847
848
                                                               33,
                                                               p_ll_address
                                                               ),
                                                   0,
849
850
                                                   _latitude,
                                                   _longitude,
851
852
853
854
855
856
                                                   int2bit(1, 1), // PAI
                                                   0,
                                                   0
                                                   ),
                                     ShortPosVector(
                                                    GN__Address(
garciay's avatar
garciay committed
857
858
                                                                TypeOfAddress(TypeOfAddress::e__manual), // TODO Use params
                                                                StationType(StationType::e__passengerCar), // TODO Use params
859
860
861
862
                                                                33,
                                                                p_ll_address
                                                                ),
                                                    0,
863
864
                                                    _latitude,
                                                    _longitude
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
898
899
900
                                                    )
                                     );
  _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
901
  //loggers::get_instance().log_msg("geonetworking_layer::fill_ls_reply: packet value: ", *_ls_reply);
902
} // End of fill_ls_reply method
903

garciay's avatar
garciay committed
904
905
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);
906

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

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