Commit 86fa225f authored by Garcia's avatar Garcia
Browse files

Merge with STF 525

parent ebbbb52c
Loading
Loading
Loading
Loading
+1 −0
Original line number Diff line number Diff line
@@ -353,4 +353,5 @@ public:
   */
  std::vector<std::string> split_arguments_line(const std::string & p_value);
                  
  static const std::string lut;
}; // End of class converter
+32 −46
Original line number Diff line number Diff line
#include "converter.hh"

#include <stdexcept>
converter * converter::instance = NULL;
    
uint16_t converter::swap(const uint16_t p_value) {
@@ -12,8 +12,8 @@ uint32_t converter::swap(const uint32_t p_value) {
  return (ptr[0] << 24) | (ptr[1] << 16) | (ptr[2] << 8) | ptr[3];
}
    
const std::string converter::lut = "0123456789ABCDEF";
std::string converter::string_to_hexa(const std::string & p_value) {
  static const char * const lut = "0123456789ABCDEF";

  std::string input(p_value);
  std::for_each(
@@ -37,55 +37,41 @@ std::string converter::string_to_hexa(const std::string & p_value) {
}
    
std::string converter::bytes_to_hexa(const std::vector<uint8_t> & p_value) {
  std::ostringstream oss;
  oss << std::setfill('0');
  std::for_each(
                p_value.begin(), 
                p_value.end(),
                [&oss](uint8_t ch) {
                  oss << std::hex
                      << std::setw(2)
                      << static_cast<int>(ch);
  std::string ret;
  ret.assign(p_value.size()*2, ' ');
  for(size_t i=0; i<p_value.size(); i++){
    uint8_t c = p_value[i];
    ret[i*2] = lut[c>>4]; 
    ret[i*2+1] = lut[c&0xF];
  }
                );
  return oss.str();
  return ret;
}

std::vector<uint8_t> converter::hexa_to_bytes(const std::string & p_value) {
  static const char * const lut = "0123456789ABCDEF";
inline uint8_t char2byte(const char ch) {
  size_t s = converter::lut.find(ch);
  if(s == std::string::npos)
    throw (std::length_error(""));
  return s;
}

std::vector<uint8_t> converter::hexa_to_bytes(const std::string & p_value) {
  // Sanity check
  std::vector<uint8_t> output;
  if (p_value.length() & 1) {
    return output;
  }
  size_t i=0, idx = 0, outlen=(p_value.length()+1) / 2;

  std::string input(p_value);
  std::for_each(
                input.begin(), 
                input.end(), 
                [](char & c) { 
                  c = std::toupper(c);
  output.assign(outlen, 0x00);
  try{
    if (p_value.length() & 1)
      output[idx++] = char2byte(p_value[i++]);
    for(;idx<outlen;idx++){
      uint8_t b0 = char2byte(p_value[i++]);
      uint8_t b1 = char2byte(p_value[i++]);
      output[idx] = (b0<<4)|b1;
    }
                );
        
  output.reserve(p_value.length() / 2);
  for (uint32_t i = 0; i < p_value.length(); i += 2) {
    char msbDigit = input[i];
    const char *msbIndex = std::lower_bound(lut, lut + 16, msbDigit);
    if (*msbIndex != msbDigit) {
      output.clear();
      return output;
  }
    char lsbDigit = input[i + 1];
    const char* lsbIndex = std::lower_bound(lut, lut + 16, lsbDigit);
    if (*lsbIndex != lsbDigit) {
  catch (const std::length_error& le) {
    output.clear();
      return output;
  }
    output.push_back(((msbIndex - lut) << 4) | (lsbIndex - lut));
  } // End of 'for' statement
    
  return output;
}

+263 −0
Original line number Diff line number Diff line
#if defined (__CYGWIN__)

#define _GNU_SOURCE

#include <unistd.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <errno.h>
#include <chrono>
#include <sstream>
#include <iomanip>

#include <Port.hh>

#include "pcap_layer_factory.hh"

#include "loggers.hh"

#include <pcap.h>

typedef struct {
    bpf_int32 tv_sec;		/* seconds */
    bpf_int32 tv_usec;		/* microseconds */
}pcap_o_timeval;

typedef struct pcap_o_pkthdr { 
    pcap_o_timeval ts;	/* time stamp */
    bpf_u_int32 caplen;		/* length of portion present */
    bpf_u_int32 len;		/* length this packet (off wire) */
}pcap_o_pkthdr;

extern "C" int pcap_oid_get_request(pcap_t *p, bpf_u_int32 oid, void *data, size_t *lenp);

static const char* _hexDigits = "0123456789ABCDEF";
static char * _bin2hex(char * hex, size_t hlen, const char * bin, size_t blen)
{
	const unsigned char *b, *e;
	char * s;

	// sanity check
	if (hlen >= 0 && hlen < blen * 2) return NULL;

	b = (const unsigned char *)bin;
	e = b + blen - 1;
	s = hex + blen * 2;
	if (s < hex + hlen) *s = 0;
	for (; b <= e; e--){
		*(--s) = _hexDigits[(*e) & 0xF];
		*(--s) = _hexDigits[(*e) >> 4];
	}
	return hex + blen * 2;
}

pcap_layer::pcap_layer(const std::string& p_type, const std::string& param) : 
	layer(p_type), PORT(p_type.c_str()), _params(), _device(NULL), _running(FALSE), _time_key("pcap_layer::Handle_Fd_Event_Readable") {
  loggers::get_instance().log(">>> pcap_layer::pcap_layer: %s, %s", p_type.c_str(), param.c_str());
  params::convert(_params, param);

  char error_buffer[PCAP_ERRBUF_SIZE];
  params::const_iterator it;
  std::string nic;  //network interface name
  bpf_u_int32 mask; // subnet mask
  bpf_u_int32 net;  // ip address

  it = _params.find(params::nic); 
  if ((it == _params.end()) || it->second.empty()) { 
    loggers::get_instance().error("pcap_layer::pcap_layer: NIC must be specified");
    return;
  }

  nic = std::string("\\Device\\NPF_{") +  it->second + "}";

  if (pcap_lookupnet(nic.c_str(), &net, &mask, error_buffer) != 0) {
    loggers::get_instance().error("pcap_layer::pcap_layer: pcap_layer::pcap_layer: Failed to fetch newtork address for device %s", nic.c_str());
  }
  loggers::get_instance().log("pcap_layer::pcap_layer: Device %s Network address: %d", nic.c_str(), net);
  
  // Open the device
  _device = pcap_open_live(nic.c_str(), 65536, 1, 200, error_buffer);
  if (_device == NULL) {
    loggers::get_instance().error("pcap_layer::pcap_layer: Failed to open device %s", nic.c_str());
    return;
  }
  
  // Setup filter
  std::string filter = "";
  std::string mac_src;
  it = _params.find(params::mac_src);
  if (it != _params.end() && !it->second.empty()) {
    mac_src = it->second;
  } else {
    // Not found
    // determine it automatically
#ifndef OID_802_3_CURRENT_ADDRESS
#define OID_802_3_CURRENT_ADDRESS 0x01010102
#endif
    char mac[6] = {0};
    size_t l = sizeof(mac);
    pcap_oid_get_request(_device, OID_802_3_CURRENT_ADDRESS, mac, &l);
    char buf[13];
    *_bin2hex(buf, sizeof(buf), mac, 6) = 0;
    mac_src = buf;
    loggers::get_instance().user("pcap_layer::pcap_layer: local MAC is %s", mac_src.c_str());
    _params[params::mac_src] = mac_src;
  }
  
  std::string mac_bc;
  it = _params.find(params::mac_bc);
  if (it != _params.end() && !it->second.empty())
    mac_bc = it->second;
  else
    mac_bc = "ffffffffffff";

  if(mac_bc == mac_src || mac_src.empty())
    filter = "ether dst " + mac_bc;
  else
    filter = "( ether dst " + mac_bc + " or ether dst " + mac_src + " )";

  if(! mac_src.empty())
    // Reject ITS messages sent by this component
    filter += " and not ether src " + mac_src;

  // Add user defined filter
  it = _params.find(std::string("filter"));
  if ((it != _params.end()) && !it->second.empty()) {
    filter += std::string(" ") + it->second;
  }
  
  // Log final PCAP filter
  loggers::get_instance().user("pcap_layer::pcap_layer: Filter: %s", filter.c_str());
  
  // setup filter
  {
    struct bpf_program f = {0};
    if (pcap_compile(_device, &f, filter.c_str(), 1, PCAP_NETMASK_UNKNOWN) != 0) {
      loggers::get_instance().error("pcap_layer::pcap_layer: Failed to compile PCAP filter");
    } else {
      if (pcap_setfilter(_device, &f) != 0) {
        loggers::get_instance().error("pcap_layer::pcap_layer: Failed to set PCAP filter");
      }
    }
    pcap_freecode(&f);
  }

  _o_params.insert(std::pair<std::string, std::string>(std::string("timestamp"), std::string()));

  // create pipe and run thread
  if (pipe2(_fd, O_NONBLOCK) == -1) {
    loggers::get_instance().error("pcap_layer::pcap_layer: Failed to create a pipe: %s", ::strerror(errno));
  }
  // Pass the pipe handler to the polling procedure
  loggers::get_instance().log("pcap_layer::pcap_layer: Call handler with descriptor %d", _fd[0]);
  Handler_Add_Fd_Read(_fd[0]);
  
  // Create the reader thread
  _thread = new std::thread(&pcap_layer::run, (void *)this);
  if (_thread == NULL) {
    loggers::get_instance().error("pcap_layer::pcap_layer: Failed to start offline thread");
  }
  while (_running == FALSE) {
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
  }
  // Thread was started
  loggers::get_instance().log("<<< pcap_layer::pcap_layer");
} // End of ctor

pcap_layer::~pcap_layer() {
  loggers::get_instance().log(">>> pcap_layer::~pcap_layer");
  
  if (_device != NULL) {
    if (_thread != NULL) {
      _running = FALSE;
      // Wait for the working thread to terminate
      _thread->join();
      loggers::get_instance().log("pcap_layer::~pcap_layer: Thread were stops");
      // Cleanup
      delete _thread;
      close(_fd[0]);
      close(_fd[1]);
    }
    pcap_close(_device);
  }
} // End of dtor

void* pcap_layer::run(void* p_this) {
  pcap_layer& p = *static_cast<pcap_layer *>(p_this);
  return p.thread();
}

void* pcap_layer::thread() {
  pcap_o_pkthdr *pkt_header;
  const u_char *pkt_data;
  unsigned char pkt_count = 0;

  loggers::get_instance().log(">>> pcap_layer::run");

  _running = TRUE;

  // wait a bit before sending first packet
  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  while (_running) { // Loop while _running flag is up
    // get next frame
    int result = pcap_next_ex(_device, (struct pcap_pkthdr**)&pkt_header, &pkt_data);
    if(result == 0){
      continue;
    }
    if(result < 0){
      loggers::get_instance().log("<<< pcap_layer::run: error %s", pcap_geterr(_device));
      break;
    }

    while(_running && !_resume.try_lock()) {
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
#if 0
    {
      char buf[128];
      std::time_t t = pkt_header->ts.tv_sec;
      std::tm * pt = std::localtime( &t );
      t = std::strftime(buf, sizeof(buf), "%Y-%m-%d %H:%M:%S", pt);
      std::sprintf(buf+t, ".%06ld", pkt_header->ts.tv_usec);
      _o_params["timestamp"] = std::string(buf);
    }
#else
    _o_params["timestamp"] = std::to_string(pkt_header->ts.tv_usec);
#endif
    _o_data = OCTETSTRING(pkt_header->len, pkt_data);
    write(_fd[1], &pkt_count, 1);pkt_count++;
  }
  _running = FALSE;
  loggers::get_instance().log("<<< pcap_layer::run");
  return NULL;
}

void pcap_layer::send_data(OCTETSTRING& data, params& params) {
  loggers::get_instance().log_msg(">>> pcap_layer::send_data: ", data);
  if (pcap_sendpacket(_device, static_cast<const unsigned char *>(data), data.lengthof()) == -1) {
    loggers::get_instance().error("pcap_layer::send_data: Failed to send packet: %s", pcap_geterr(_device));
  }
}

void pcap_layer::receive_data(OCTETSTRING& data, params& params) {
  loggers::get_instance().log(">>> pcap_layer::receive_data: Received %d bytes", data.lengthof());
  loggers::get_instance().log_to_hexa("Packet dump", data);
  
  // Pass the packet to the upper layers
  receive_to_all_layers(data, params);
}

void pcap_layer::Handle_Fd_Event_Readable(int fd) {
  char c[2];
  float duration;
  loggers::get_instance().set_start_time(_time_key);
  this->receive_data(_o_data, _o_params);
  loggers::get_instance().set_stop_time(_time_key, duration);
  read(_fd[0], &c, 1);
  _resume.unlock();
}

pcap_layer_factory pcap_layer_factory::_f;

#endif //__CYGWIN__
+78 −0
Original line number Diff line number Diff line
/*!
 * \file      pcap_layer.hh
 * \brief     Header file for ITS Offline Pcap port layer.
 * \author    ETSI STF525
 * \copyright ETSI Copyright Notification
 *            No part may be reproduced except as authorized by written permission.
 *            The copyright and the foregoing restriction extend to reproduction in all media.
 *            All rights reserved.
 * \version   0.1
 */
#pragma once

#include <thread>
#include <mutex>

#include "t_layer.hh"
#include "params.hh"

#include <Octetstring.hh>

class PORT; //! Forward declaration of TITAN class

typedef struct pcap pcap_t;

/*!
 * \class pcap_layer
 * \brief  This class provides description of ITS PCAP port protocol layer
 */
class pcap_layer : public layer, public PORT {

  params _params;            //! Layer parameters
  pcap_t* _device;           //! Device handle
  std::thread* _thread;      //! Thread handle, used to read PCAP file instead of NIC, used in file mode
  std::mutex _resume;
  bool _running;             //! Set to true when the thread is running, used in file mode
  int _fd[2];                //! pipe to signal to Titan


  OCTETSTRING _o_data;
  params      _o_params;

  std::string _time_key;

  static void* run(void* p_this);
public: 
  void* thread(void);
public: //! \publicsection
  /*!
   * \brief Specialised constructor
   *        Create a new instance of the pcap_layer class
   * \param[in] p_type \todo
   * \param[in] p_param \todo
   */
  pcap_layer(const std::string& p_type, const std::string& param);
  /*!
   * \brief Default destructor
   */
  virtual ~pcap_layer();

  /*!
   * \virtual
   * \fn void send_data(OCTETSTRING& data, params& params);
   * \brief Send bytes formated data to the lower layers
   * \param[in] p_data The data to be sent
   * \param[in] p_params Some parameters to overwrite default value of the lower layers parameters
   */
  virtual void send_data(OCTETSTRING& data, params& params);
  /*!
   * \virtual
   * \fn void receive_data(OCTETSTRING& data, params& params);
   * \brief Receive bytes formated data from the lower layers
   * \param[in] p_data The bytes formated data received
   * \param[in] p_params Some lower layers parameters values when data was received
   */
  virtual void receive_data(OCTETSTRING& data, params& info);
  
  void Handle_Fd_Event_Readable(int fd);
};
+37 −25
Original line number Diff line number Diff line
#if (OSTYPE == linux)

#if 0
#include <unistd.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <errno.h>
#include <chrono>

#include "Port.hh"
#include <Port.hh>

#include "pcap_layer_factory.hh"

typedef struct pcap_pkthdr pcap_o_pkthdr;
typedef struct timeval pcap_o_timeval;

#include "loggers.hh"

pcap_layer::pcap_layer(const std::string& p_type, const std::string& param) : layer(p_type), PORT(p_type.c_str()), _params(), _device(NULL), _pcap_h(-1), _thread(NULL), _running(FALSE), _resume(), _sent_file(NULL), _time_key("pcap_layer::Handle_Fd_Event_Readable") {
  bool online = false;
  loggers::get_instance().log(">>> pcap_layer::pcap_layer: %s, %s", to_string().c_str(), param.c_str());
  _fd[0] = -1; _fd[1] = -1;
  // Setup parameters
@@ -25,14 +28,17 @@ pcap_layer::pcap_layer(const std::string& p_type, const std::string& param) : la
    // Fetch the network address and network mask
    bpf_u_int32 mask; // subnet mask
    bpf_u_int32 net; // ip address
    if (pcap_lookupnet(_params[params::nic].c_str(), &net, &mask, error_buffer) != 0) {
      loggers::get_instance().error("pcap_layer::pcap_layer: pcap_layer::pcap_layer: Failed to fetch newtork address for device %s", _params[params::nic].c_str());
    std::string nic;
    online = true;
    nic = _params[params::nic];
    if (pcap_lookupnet(nic.c_str(), &net, &mask, error_buffer) != 0) {
      loggers::get_instance().error("pcap_layer::pcap_layer: pcap_layer::pcap_layer: Failed to fetch newtork address for device %s", nic.c_str());
    }
    loggers::get_instance().log("pcap_layer::pcap_layer: Device %s Network address: %d", _params[params::nic].c_str(), net);
    loggers::get_instance().log("pcap_layer::pcap_layer: Device %s Network address: %d", nic.c_str(), net);
    // Open the device
    _device = pcap_open_live(_params[params::nic].c_str(), 65536, 1, 1000, error_buffer); // TODO Replace hard coded values by pcap_layer::<constants>
    _device = pcap_open_live(nic.c_str(), 65536, 1, 1000, error_buffer); // TODO Replace hard coded values by pcap_layer::<constants>
    if (_device == NULL) {
      loggers::get_instance().error("pcap_layer::pcap_layer: Failed to open device %s", _params[params::nic].c_str());
      loggers::get_instance().error("pcap_layer::pcap_layer: Failed to open device %s", nic.c_str());
    } // else, continue
    // Set non-blocking flag for the polling procedure
    if (pcap_setnonblock(_device, 1, error_buffer) != 0) {
@@ -74,6 +80,7 @@ pcap_layer::pcap_layer(const std::string& p_type, const std::string& param) : la
      loggers::get_instance().error("pcap_layer::pcap_layer: Failed to open PCAP file %s", error_buffer);
    }
  }

  // Setup filter
  std::string filter = "";
  it = _params.find(params::mac_src);
@@ -109,14 +116,18 @@ pcap_layer::pcap_layer(const std::string& p_type, const std::string& param) : la
    }
    pcap_freecode(&f);
  }

  // Pass the device file handler to the polling procedure
  if (_pcap_h != -1) { // Live capture
    Handler_Add_Fd_Read(_pcap_h);
  } else { // Offline capture
  } else { // Offline capture or cygwin
    // Create a pipe
    if (pipe2(_fd, O_NONBLOCK) == -1) {
      loggers::get_instance().error("pcap_layer::pcap_layer: Failed to create a pipe: %s", ::strerror(errno));
    }
    if(online){
      _pcap_h = _fd[0];
    }
    // Pass the pipe handler to the polling procedure
    loggers::get_instance().log("pcap_layer::pcap_layer: Call handler with descriptor %d", _fd[0]);
    Handler_Add_Fd_Read(_fd[0]);
@@ -156,14 +167,17 @@ pcap_layer::~pcap_layer() {
} // End of dtor

void* pcap_layer::run(void* p_this) {
  pcap_layer& p = *static_cast<pcap_layer *>(p_this);
  return p.thread();
}

void* pcap_layer::thread() {
  loggers::get_instance().log(">>> pcap_layer::run");

  // Pointer the main object
  pcap_layer& p = *static_cast<pcap_layer *>(p_this);
  // Wait a little bit before to start sending packet
  std::this_thread::sleep_for(std::chrono::milliseconds(500));
  params::const_iterator it = p._params.find("frame_offset");
  if ((it != p._params.cend()) && (it->second.compare("0") != 0)) {
  params::const_iterator it = _params.find("frame_offset");
  if ((it != _params.cend()) && (it->second.compare("0") != 0)) {
    // TODO Try t use PCAP filter to start directly to the correct frame offset
    /*try {
      unsigned int offset = std::stoul(str_dec, &s);
@@ -180,10 +194,10 @@ void* pcap_layer::run(void* p_this) {
      }*/
  }
  // Let's go
  p._running = TRUE;
  while (p._running) { // Loop while _running flag is up
    if (p._resume.try_lock() == TRUE) { // Previous packet was consumed, lock for the next one
      write(p._fd[1], "\n", 1); // Any character will do the job
  _running = TRUE;
  while (_running) { // Loop while _running flag is up
    if (_resume.try_lock() == TRUE) { // Previous packet was consumed, lock for the next one
      write(_fd[1], "\n", 1); // Any character will do the job
    } else { // not ready yet
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
@@ -225,9 +239,9 @@ void pcap_layer::receive_data(OCTETSTRING& data, params& params) {
void pcap_layer::Handle_Fd_Event_Readable(int fd) {
  //loggers::get_instance().log(">>> pcap_layer::Handle_Fd_Event_Readable: %d", fd);
  
  struct pcap_pkthdr *pkt_header;
  pcap_o_pkthdr *pkt_header;
  const u_char *pkt_data;
  int result = pcap_next_ex(_device, &pkt_header, &pkt_data);
  int result = pcap_next_ex(_device, (struct pcap_pkthdr**)&pkt_header, &pkt_data);
  if (result == 1) { // Succeed
    if (pkt_header->caplen > 14) { // Reject too small packet
      //loggers::get_instance().log("pcap_layer::Handle_Fd_Event_Readable: %.6d - %d", pkt_header->ts.tv_usec, pkt_header->len);
@@ -251,14 +265,12 @@ void pcap_layer::Handle_Fd_Event_Readable(int fd) {
    read(_fd[0], &c, 1);
    if (result == -2) { // End of file, therminate worker thread
      _running = FALSE;
    } else { // Get next packet
    }
      //loggers::get_instance().log("pcap_layer::Handle_Fd_Event_Readable: pcap_next_ex failed: result=%d", result);
    _resume.unlock();
    }
  } // else, nothing to do
}

pcap_layer_factory pcap_layer_factory::_f;

#endif
#endif // !CYGWIN
Loading