ASN.1 Changes due to latest asn1c github version

Enhance UdpLayer
Bug fixed on CAM/DENM/BTP layers (TODO GeoNetworking LAYER)
Validation basic test cases for CAM/DENM/GeoNetworking

ccsrc/Asn1c/Asn1cEncDec.cc

@@ -107,16 +107,9 @@ BITSTRING   asn1c2titan(const BIT_STRING_t& a) {
 
 extern "C" int asn1c_collect_encoded_data(const void *buffer, size_t size, void *application_specific_key)
 {
-	unsigned char* ptr;
-	size_t s = size;
 	TTCN_Buffer * tb = (TTCN_Buffer *)application_specific_key;
   tb->put_s(size, (unsigned char *)buffer);
   loggers::get_instance().log_to_hexa("asn1c_collect_encoded_data: Encoding=", *tb);
-	/*tb->get_end(ptr, s);
-	if(s >= size){
-		memcpy(ptr, buffer, size);
-		tb->increase_length(size);
-    }*/
 	return 0;
 }
 
@@ -132,16 +125,16 @@ int asn1c_per2ber(asn_TYPE_descriptor_t &td, const TTCN_Buffer & per, TTCN_Buffe
 	asn_dec_rval_t rc_d;
 
 	rc_d = uper_decode_complete(NULL, 
-				    &td, ctx, 
-				    per.get_read_data(), per.get_read_len());
+                              &td, ctx, 
+                              per.get_read_data(), per.get_read_len());
 
 	if(rc_d.code == RC_OK){
 		asn_enc_rval_t rc_e;
 		ber.clear();
 		rc_e = der_encode(&td,
-			ctx, 
-			asn1c_collect_encoded_data,
-			&ber);
+                      (const asn_per_constraints_t*)ctx,
+                      asn1c_collect_encoded_data,
+                      &ber);
 		rc = rc_e.encoded;
 	}
 	/*if(obj) {
@@ -156,16 +149,17 @@ int asn1c_ber2per(asn_TYPE_descriptor_t &td, const TTCN_Buffer & ber, TTCN_Buffe
 	int rc = -1;
 	asn_dec_rval_t rc_d;
 	rc_d = ber_decode( NULL, &td,
-			   ctx,
-			   ber.get_data(), ber.get_len());
+                     ctx,
+                     ber.get_data(), ber.get_len());
 	if(rc_d.code == RC_OK){
     loggers::get_instance().log("asn1c_ber2per: BER decoding succeed");
 		asn_enc_rval_t rc_e;
 		//per.clear();
 		rc_e = uper_encode(&td,
-		  ctx, 
-			asn1c_collect_encoded_data,
-			&per);
+                       NULL,
+                       ctx, 
+                       asn1c_collect_encoded_data,
+                       &per);
 		rc = rc_e.encoded;
     if(rc < 0) {
       loggers::get_instance().warning("asn1c_ber2per: PER encoding failed");

ccsrc/Framework/Params.hh

@@ -29,6 +29,7 @@ public:
   static const std::string& longitude;
   static const std::string& expiry;
   
+  static const std::string& packetize; /** Indicate to the lower layer to build packet */
   
   Params() : std::map<std::string, std::string>() {};
   Params(const Params& p_params) : std::map<std::string, std::string>(p_params.begin(), p_params.end()) {};

ccsrc/Framework/src/Params.cc

@@ -28,9 +28,9 @@ const std::string& Params::longitude = std::string("longitude");
 const std::string& Params::ll_address = std::string("ll_address");
 const std::string& Params::expiry = std::string("expiry");
 
+const std::string& Params::packetize = std::string("packetize");
 
 void Params::convert(Params& p_param, const std::string p_parameters) {
-  //loggers::get_instance().log(">>> Params::convert: '%s'", p_parameters.c_str());
   // Sanity checks
   if (p_parameters.length() == 0) {
     return;

ccsrc/Ports/LibIts_ports/CAM_ports/UpperTesterPort_CAM.partC

+#include "UpperTesterCamLayer.hh"
+#include "loggers.hh"
+
 //=============================================================================
 namespace LibItsCam__TestSystem {
 
-UpperTesterPort::UpperTesterPort(const char *par_port_name)
-	: UpperTesterPort_BASE(par_port_name)
-{
-
-}
-
-UpperTesterPort::~UpperTesterPort()
-{
-
-}
-
-void UpperTesterPort::set_parameter(const char * /*parameter_name*/,
-	const char * /*parameter_value*/)
-{
-
-}
-
-/*void UpperTesterPort::Handle_Fd_Event(int fd, boolean is_readable,
-	boolean is_writable, boolean is_error) {}*/
-
-void UpperTesterPort::Handle_Fd_Event_Error(int /*fd*/)
-{
-
-}
-
-void UpperTesterPort::Handle_Fd_Event_Writable(int /*fd*/)
-{
-
-}
-
-void UpperTesterPort::Handle_Fd_Event_Readable(int /*fd*/)
-{
-
-}
-
-/*void UpperTesterPort::Handle_Timeout(double time_since_last_call) {}*/
-
-void UpperTesterPort::user_map(const char * /*system_port*/)
-{
-
-}
-
-void UpperTesterPort::user_unmap(const char * /*system_port*/)
-{
-
-}
-
-void UpperTesterPort::user_start()
-{
-
-}
-
-void UpperTesterPort::user_stop()
-{
-
-}
-
-void UpperTesterPort::outgoing_send(const LibItsCam__TypesAndValues::UtCamInitialize& /*send_par*/)
-{
-
-}
-
-void UpperTesterPort::outgoing_send(const LibItsCam__TypesAndValues::UtCamTrigger& /*send_par*/)
-{
-
-}
-
-void UpperTesterPort::outgoing_send(const LibItsCam__TypesAndValues::UtCamChangePosition& /*send_par*/)
-{
-
-}
-
-void UpperTesterPort::outgoing_send(const LibItsCam__TypesAndValues::UtActivatePositionTime& /*send_par*/)
-{
-
-}
-
-void UpperTesterPort::outgoing_send(const LibItsCam__TypesAndValues::UtDeactivatePositionTime& /*send_par*/)
-{
-
-}
+  UpperTesterPort::UpperTesterPort(const char *par_port_name)
+    : UpperTesterPort_BASE(par_port_name), _cfg_params(), _layer_params(), _layer(NULL), _time_key("UpperTesterPort_Cam::outgoing_send")
+  {
+    loggers::get_instance().log("UpperTesterPort_Cam::UpperTesterPort_Cam");
+  }
+
+  UpperTesterPort::~UpperTesterPort()
+  {
+    if (_layer != NULL) {
+      delete _layer;
+    }
+  }
+
+  void UpperTesterPort::set_parameter(const char * parameter_name, const char * parameter_value)
+  {
+    loggers::get_instance().log("UpperTesterPort_Cam::set_parameter: %s=%s", parameter_name, parameter_value);
+    _cfg_params.insert(std::pair<std::string, std::string>(std::string(parameter_name), std::string(parameter_value)));
+    _cfg_params.log();
+  }
+
+  /*void UpperTesterPort::Handle_Fd_Event(int fd, boolean is_readable,
+    boolean is_writable, boolean is_error) {}*/
+
+  void UpperTesterPort::Handle_Fd_Event_Error(int /*fd*/)
+  {
+
+  }
+
+  void UpperTesterPort::Handle_Fd_Event_Writable(int /*fd*/)
+  {
+
+  }
+
+  void UpperTesterPort::Handle_Fd_Event_Readable(int /*fd*/)
+  {
+
+  }
+
+  /*void UpperTesterPort::Handle_Timeout(double time_since_last_call) {}*/
+
+  void UpperTesterPort::user_map(const char * system_port)
+  {
+    loggers::get_instance().log(">>> UpperTesterPort_Cam::user_map: %s", system_port);
+    // Build layer stack
+    Params::iterator it = _cfg_params.find(std::string("params"));
+    if (it != _cfg_params.end()) {
+      loggers::get_instance().log("UpperTesterPort_Cam::user_map: %s", it->second.c_str());
+      _layer = LayerStackBuilder::GetInstance()->createLayerStack(it->second.c_str());
+      if (static_cast<UpperTesterCamLayer *>(_layer) == NULL) {
+        loggers::get_instance().error("UpperTesterPort_Cam::user_map: Invalid stack configuration: %s", it->second.c_str());
+      }
+      static_cast<UpperTesterCamLayer *>(_layer)->addUpperPort(this);
+    } else {
+      loggers::get_instance().error("UpperTesterPort_Cam::user_map: No layers defined in configuration file");
+    }
+  }
+
+  void UpperTesterPort::user_unmap(const char * system_port)
+  {
+    loggers::get_instance().log(">>> UpperTesterPort_Cam::user_unmap: %s", system_port);
+    if (_layer != NULL) {
+      delete _layer;
+      _layer = NULL;
+    }
+
+  }
+
+  void UpperTesterPort::user_start()
+  {
+
+  }
+
+  void UpperTesterPort::user_stop()
+  {
+
+  }
+
+  void UpperTesterPort::outgoing_send(const LibItsCam__TypesAndValues::UtCamInitialize& send_par)
+  {
+    loggers::get_instance().log_msg(">>> UppertesterPort::outgoing_send: ", send_par);
+    
+    float duration;
+    loggers::get_instance().set_start_time(_time_key);
+    static_cast<UpperTesterCamLayer *>(_layer)->sendMsg(send_par, _layer_params);
+    loggers::get_instance().set_stop_time(_time_key, duration);
+  }
+
+  void UpperTesterPort::outgoing_send(const LibItsCam__TypesAndValues::UtCamChangePosition& send_par)
+  {
+    loggers::get_instance().log_msg(">>> UppertesterPort::outgoing_send: ", send_par);
+    
+    float duration;
+    loggers::get_instance().set_start_time(_time_key);
+    static_cast<UpperTesterCamLayer *>(_layer)->sendMsg(send_par, _layer_params);
+    loggers::get_instance().set_stop_time(_time_key, duration);
+  }
+
+  void UpperTesterPort::outgoing_send(const LibItsCam__TypesAndValues::UtCamTrigger& send_par)
+  {
+    loggers::get_instance().log_msg(">>> UppertesterPort::outgoing_send: ", send_par);
+    
+    float duration;
+    loggers::get_instance().set_start_time(_time_key);
+    static_cast<UpperTesterCamLayer *>(_layer)->sendMsg(send_par, _layer_params);
+    loggers::get_instance().set_stop_time(_time_key, duration);
+  }
+
+  void UpperTesterPort::outgoing_send(const LibItsCam__TypesAndValues::UtActivatePositionTime& /*send_par*/)
+  {
+
+  }
+
+  void UpperTesterPort::outgoing_send(const LibItsCam__TypesAndValues::UtDeactivatePositionTime& /*send_par*/)
+  {
+
+  }
+
+  void UpperTesterPort::receiveMsg (const Base_Type& p_ind, const Params& p_params) {
+    loggers::get_instance().log_msg(">>> UpperTesterPort_Cam::receive_msg: ", p_ind);
+    // Sanity check
+    if (!p_ind.is_bound()) {
+      return;
+    }
+
+    loggers::get_instance().log("UpperTesterPort_Cam::receive_msg: %s", p_ind.get_descriptor()->name);
+    if (std::string(p_ind.get_descriptor()->name).compare("@LibItsCam_TypesAndValues.UtCamResults") == 0) { // TODO To be refined
+      incoming_message((LibItsCam__TypesAndValues::UtCamResults&)p_ind);
+    } // TODO Implement UtCamEventInd part
+  }
 
 } /* end of namespace */

ccsrc/Ports/LibIts_ports/CAM_ports/UpperTesterPort_CAM.partH

+#include "Layer.hh"
+#include "Params.hh"
+
 //=============================================================================
 #include "LibItsCam_TestSystem.hh"
 namespace LibItsCam__TestSystem {
 
-class UpperTesterPort : public UpperTesterPort_BASE {
-public:
-	UpperTesterPort(const char *par_port_name = NULL);
-	~UpperTesterPort();
-
-	void set_parameter(const char *parameter_name,
-		const char *parameter_value);
-
-private:
-	/* void Handle_Fd_Event(int fd, boolean is_readable,
-		boolean is_writable, boolean is_error); */
-	void Handle_Fd_Event_Error(int fd);
-	void Handle_Fd_Event_Writable(int fd);
-	void Handle_Fd_Event_Readable(int fd);
-	/* void Handle_Timeout(double time_since_last_call); */
-protected:
-	void user_map(const char *system_port);
-	void user_unmap(const char *system_port);
-
-	void user_start();
-	void user_stop();
-
-	void outgoing_send(const LibItsCam__TypesAndValues::UtCamInitialize& send_par);
+  class UpperTesterPort : public UpperTesterPort_BASE {
+    Params _cfg_params;
+    Params _layer_params;
+    Layer * _layer;
+    std::string _time_key;
+    
+  public:
+    UpperTesterPort(const char *par_port_name = NULL);
+    ~UpperTesterPort();
+
+    void set_parameter(const char *parameter_name,
+                       const char *parameter_value);
+
+    void receiveMsg (const Base_Type&, const Params&);
+
+  private:
+    /* void Handle_Fd_Event(int fd, boolean is_readable,
+       boolean is_writable, boolean is_error); */
+    void Handle_Fd_Event_Error(int fd);
+    void Handle_Fd_Event_Writable(int fd);
+    void Handle_Fd_Event_Readable(int fd);
+    /* void Handle_Timeout(double time_since_last_call); */
+  protected:
+    void user_map(const char *system_port);
+    void user_unmap(const char *system_port);
+
+    void user_start();
+    void user_stop();
+
+    void outgoing_send(const LibItsCam__TypesAndValues::UtCamInitialize& send_par);
 	
-	void outgoing_send(const LibItsCam__TypesAndValues::UtCamTrigger& send_par);
+    void outgoing_send(const LibItsCam__TypesAndValues::UtCamChangePosition& send_par);
 	
-	void outgoing_send(const LibItsCam__TypesAndValues::UtCamChangePosition& send_par);
+    void outgoing_send(const LibItsCam__TypesAndValues::UtCamTrigger& send_par);
 	
     void outgoing_send(const LibItsCam__TypesAndValues::UtActivatePositionTime& send_par);
-    
+  
     void outgoing_send(const LibItsCam__TypesAndValues::UtDeactivatePositionTime& send_par);
     
-};
+  };
 
 } /* end of namespace */

ccsrc/Ports/LibIts_ports/DENM_ports/UpperTesterPort_DENM.partC

@@ -8,7 +8,6 @@ namespace LibItsDenm__TestSystem {
     : UpperTesterPort_BASE(par_port_name), _cfg_params(), _layer_params(), _layer(NULL), _time_key("UpperTesterPort_Denm::outgoing_send")
   {
     loggers::get_instance().log("UpperTesterPort_Denm::UpperTesterPort_Denm");
-
   }
 
   UpperTesterPort::~UpperTesterPort()
@@ -16,7 +15,6 @@ namespace LibItsDenm__TestSystem {
     if (_layer != NULL) {
       delete _layer;
     }
-
   }
 
   void UpperTesterPort::set_parameter(const char * parameter_name, const char * parameter_value)

ccsrc/Protocols/BTP/BTPLayer.cc

@@ -7,22 +7,60 @@ BTPLayer::BTPLayer(const std::string & p_type, const std::string & param) : TLay
   loggers::get_instance().log(">>> BTPLayer::BTPLayer: %s, %s", to_string().c_str(), param.c_str());
   // Setup parameters
   Params::convert(_params, param);
+  
+  // Sanity check
+  Params::const_iterator it = _params.find(Params::btp_type);
+  if (it == _params.cend()) {
+    _params[Params::btp_type] = std::string("btpA");
+  }
+  it = _params.find(Params::btp_destination_port);
+  if (it == _params.cend()) {
+    _params[Params::btp_destination_port] = std::to_string(2001);
+  }
+  it = _params.find(Params::btp_info);
+  if (it == _params.cend()) {
+    _params[Params::btp_info] = std::to_string(0);
+  }
 }
 
 void BTPLayer::sendMsg(const LibItsBtp__TestSystem::BtpReq& p, Params& params){
   loggers::get_instance().log(">>> BTPLayer::sendMsg");
+  //params.log();
 
   // Encode BTP PDU
   OCTETSTRING data;
   _codec.encode(p.msgOut(), data);
-  // Update parameters
-  //  Params par(params); // FIXME Review all const Param& in method declarations
   sendData(data, params);
 }
 
 void BTPLayer::sendData(OCTETSTRING& data, Params& params) {
   loggers::get_instance().log_msg(">>> BTPLayer::sendData: ", data);
-  //params.log();  
+  params.log();
+
+  Params::const_iterator it = params.find(Params::packetize);
+  if (it != params.cend()) {
+    LibItsBtp__TypesAndValues::BtpHeader header;
+    if (_params[Params::btp_type].compare("btpA") == 0) {
+      header.btpAHeader() = LibItsBtp__TypesAndValues::BtpAHeader(
+                                                                  std::stoi(_params[Params::btp_destination_port]),
+                                                                  std::stoi(_params[Params::btp_info])
+                                                                  );
+    } else {
+      header.btpBHeader() = LibItsBtp__TypesAndValues::BtpBHeader(
+                                                                  std::stoi(_params[Params::btp_destination_port]),
+                                                                  std::stoi(_params[Params::btp_info])
+                                                                  );
+    }
+    LibItsBtp__TypesAndValues::BtpPacket p(
+                                           header,
+                                           data
+                                           );
+    loggers::get_instance().log_msg("BTPLayer::sendData: ", p);
+    // Encode BTP PDU
+    OCTETSTRING os;
+    _codec.encode(p, os);
+    data = os;
+  }
   sendToAllLayers(data, params);
 }
 

ccsrc/Protocols/CAM/CAMLayer.cc

@@ -16,8 +16,12 @@ void CAMLayer::sendMsg(const LibItsCam__TestSystem::CamReq& p, Params& params){
   OCTETSTRING data;
   _codec.encode(p.msgOut(), data);
   // Update parameters
-  //  Params par(params); // FIXME Review all const Param& in method declarations
-  sendData(data, params);
+  Params pars(params);
+  pars.insert(std::pair<std::string, std::string>(
+                                                  Params::packetize,
+                                                  std::string("")
+                                                  ));
+  sendData(data, pars);
 }
 
 void CAMLayer::sendData(OCTETSTRING& data, Params& params) {

ccsrc/Protocols/DENM/DENMLayer.cc

@@ -16,7 +16,11 @@ void DENMLayer::sendMsg(const LibItsDenm__TestSystem::DenmReq& p, Params& params
   OCTETSTRING data;
   _codec.encode(p.msgOut(), data);
   // Update parameters
-  //  Params par(params); // FIXME Review all const Param& in method declarations
+  Params pars(params);
+  pars.insert(std::pair<std::string, std::string>(
+                                                  Params::packetize,
+                                                  std::string("")
+                                                  ));
   sendData(data, params);
 }
 

ccsrc/Protocols/GeoNetworking/GeoNetworkingLayer.cc

@@ -48,7 +48,7 @@ GeoNetworkingLayer::GeoNetworkingLayer(const std::string & p_type, const std::st
     fill_beacon(latitude, longitude, ll_address);
     start_beaconing();
   }
-                                                                                                                                                                                                                                                                                                                             } // End of constructor
+ } // End of constructor
 
 GeoNetworkingLayer::~GeoNetworkingLayer() {
   loggers::get_instance().log(">>> GeoNetworkingLayer::~GeoNetworkingLayer");
@@ -73,6 +73,45 @@ void GeoNetworkingLayer::sendMsg(const LibItsGeoNetworking__TestSystem::GeoNetwo
 
 void GeoNetworkingLayer::sendData(OCTETSTRING& data, Params& params) {
   loggers::get_instance().log_msg(">>> GeoNetworkingLayer::sendData: ", data);
+  //params.log();
+
+  // TODO Take into account the Security
+  Params::const_iterator it = params.find(Params::packetize);
+  /*if (it != params.cend()) {
+    LibItsGeoNetworking__TypesAndValues::GeoNetworkingPacket gnp;
+    if (_params[Params::btp_type].compare("btpA") == 0) {
+      header.btpAHeader() = LibItsGeoNetworking__TypesAndValues::GeoNetworkingAHeader(
+                                                                  std::stoi(_params[Params::btp_destination_port]),
+                                                                  std::stoi(_params[Params::btp_info])
+                                                                  );
+    } else {
+      header.btpBHeader() = LibItsGeoNetworking__TypesAndValues::GeoNetworkingBHeader(
+                                                                  std::stoi(_params[Params::btp_destination_port]),
+                                                                  std::stoi(_params[Params::btp_info])
+                                                                  );
+    }
+    LibItsGeoNetworking__TypesAndValues::GeoNetworkingPacket p(
+                                                               LibItsGeoNetworking__TypesAndValues::BasicHeader(
+                                                                                                                1,
+                                                                                                                LibItsGeoNetworking__TypesAndValues::BasicNextHeader(LibItsGeoNetworking__TypesAndValues::BasicNextHeader::e__commonHeader), // TODO Take into acount the Security
+                                                                                                                0,
+                                                                                                                LibItsGeoNetworking__TypesAndValues::Lifetime(
+                                                                                                                                                              LibItsGeoNetworking__TypesAndValues::Ltbase(LibItsGeoNetworking__TypesAndValues::Ltbase::e__50ms),
+                                                                                                                                                              std::stoi(_params[Params::expity]) / 50
+                                                                                                                                                              ),
+                                                                                                                10
+),
+                                                               LibItsGeoNetworking__TypesAndValues::GeoNetworkingPacket(gnp)
+                                                               );
+    loggers::get_instance().log_msg("GeoNetworkingLayer::sendData: ", p);
+    
+    // Encode GeoNetworking PDU
+    OCTETSTRING os;
+    _codec.encode(p, os);
+    data = os;
+    }*/
+  
+  // TODO To be removed
   while (_sendData.try_lock() == FALSE) {
     // not ready yet
     std::this_thread::sleep_for(std::chrono::milliseconds(1));

ccsrc/Protocols/UDP/UdpLayer.cc

@@ -45,7 +45,7 @@ void UdpLayer::sendData(OCTETSTRING& data, Params& params) {
   inet_pton(AF_INET, it->second.c_str(), (struct in_addr *)&_saddr.sin_addr.s_addr);
   _iphdr->ihl = 5;
   _iphdr->version = 4;
-  _iphdr->tos = 0x0;
+  _iphdr->tos = IPTOS_LOWDELAY;
   _iphdr->id = 0;
   _iphdr->frag_off = htons(0x4000); /* Don't fragment */
   _iphdr->ttl = 64;
@@ -54,7 +54,7 @@ void UdpLayer::sendData(OCTETSTRING& data, Params& params) {
   _iphdr->saddr = _saddr.sin_addr.s_addr;
   _iphdr->daddr = _daddr.sin_addr.s_addr;
   _iphdr->check = 0;
-  _iphdr->check = inetCsum((const void *)_iphdr, sizeof(struct iphdr));// The checksum should be calculated over the entire header with the checksum field set to 0, so that's what we do
+  _iphdr->check = inet_check_sum((const void *)_iphdr, sizeof(struct iphdr));
   // Set udp header
   _udphdr = (struct udphdr *)(buffer + sizeof(struct iphdr));
   _udphdr->source = _saddr.sin_port;
@@ -64,7 +64,20 @@ void UdpLayer::sendData(OCTETSTRING& data, Params& params) {
   // Set payload
   unsigned char *payload = buffer + sizeof(struct iphdr) + sizeof(struct udphdr);
   memcpy(payload, static_cast<const unsigned char *>(data), data.lengthof());
-  // TODO Calculate UDP checksum
+  // Calculate UDP checksum
+  _udphdr->check = inet_check_sum(
+                                  (const void *)_udphdr,
+                                  sizeof(struct udphdr),
+                                  inet_check_sum(
+                                                 static_cast<const unsigned char*>(data),
+                                                 data.lengthof(),
+                                                 inet_check_sum(
+                                                                (const unsigned char*)(&(_iphdr->saddr)),
+                                                                2 * sizeof(_iphdr->saddr),
+                                                                IPPROTO_UDP + static_cast<unsigned int>(ntohs(_udphdr->len))
+                                                                )
+                                                 )
+                                  );
   // Send data lower layers
   OCTETSTRING udp(len, buffer);
   sendToAllLayers(udp, params);
@@ -87,23 +100,25 @@ void UdpLayer::receiveData(OCTETSTRING& data, Params& params) {
   receiveToAllLayers(data, params);
 }
 
-unsigned short UdpLayer::inetCsum(const void *buf, size_t hdr_len) {
-  unsigned long sum = 0;
-  const unsigned short *ip1;
-
-  ip1 = (const unsigned short *) buf;
-  while (hdr_len > 1)
-  {
-    sum += *ip1++;
-    if (sum & 0x80000000)
-      sum = (sum & 0xFFFF) + (sum >> 16);
-    hdr_len -= 2;
+unsigned short UdpLayer::inet_check_sum(const void *buf, size_t len, const unsigned short p_initial_sum) {
+  unsigned long sum = p_initial_sum;
+  unsigned int i;
+  
+  // Checksum all the pairs of bytes first...
+  for (i = 0; i < (len & ~1U); i += 2) {
+    sum += (u_int16_t)ntohs(*((u_int16_t *)((unsigned char*)buf + i)));
+    if (sum > 0xFFFF)
+      sum -= 0xFFFF;
+  } // End of 'for' statement