geonetworking_layer.cc 54.3 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
12
#include "registration.hh"
#include "loggers.hh"

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

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

17
18
#include "converter.hh"

19
20
using namespace LibItsGeoNetworking__TypesAndValues;

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

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

145
  // Fill packet templates
146
147
148
149
150
151
  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
152
153
  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);
154
} // End of init_params
155

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

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

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

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

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

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

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

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


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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