CommsigniaLayer.cc

#include <arpa/inet.h>

#include "CommsigniaLayerFactory.hh"

#include "loggers.hh"

CommsigniaLayer::CommsigniaLayer(const std::string & p_type, const std::string & param) : Layer(p_type), _params(), _c2p_recv{0}, _802_11p_hdr{0}, _c2p_llc_hdr{0} {
  loggers::get_instance().log(">>> CommsigniaLayer::CommsigniaLayer: %s, %s", to_string().c_str(), param.c_str());
  
  // Setup parameters
  Params::convert(_params, param);
  Params::const_iterator it = _params.find("mac_src");
  if (it == _params.cend()) {
    _params.insert(std::pair<std::string, std::string>(std::string("mac_src"), "000000000000"));
  }
  it = _params.find("mac_bc");
  if (it == _params.cend()) {
    _params.insert(std::pair<std::string, std::string>(std::string("mac_bc"), "FFFFFFFFFFFF"));
  }
  it = _params.find(Params::eth_type);
  if (it == _params.cend()) {
    _params.insert(std::pair<std::string, std::string>(std::string("eth_type"), "8947"));
  }
  it = _params.find(Params::interface_id);
  if (it == _params.cend()) {
    _params.insert(std::pair<std::string, std::string>(std::string("interface_id"), "1"));
  }
  //_params.log();
}

void CommsigniaLayer::sendData(OCTETSTRING& data, Params& params) {
  loggers::get_instance().log_msg(">>> CommsigniaLayer::sendData: ", data);

}

void CommsigniaLayer::receiveData(OCTETSTRING& data, Params& params) {
  loggers::get_instance().log_msg(">>> CommsigniaLayer::receiveData: ", data);
  
  const unsigned char* p = static_cast<const unsigned char *>(data);

  const CommsigniaLayer::c2p_recv* r = (const CommsigniaLayer::c2p_recv*)p;
  //loggers::get_instance().log("CommsigniaLayer::receiveData: version=%02x", r->version);
  //loggers::get_instance().log("CommsigniaLayer::receiveData: timestamp1=%08x", ntohl(r->timestamp_sec));
  //loggers::get_instance().log("CommsigniaLayer::receiveData: timestamp2=%08x", ntohl(r->timestamp_msec));
  //loggers::get_instance().log("CommsigniaLayer::receiveData: timestamp2=%08x", ntohl(r->timestamp_msec));
  //loggers::get_instance().log("CommsigniaLayer::receiveData: antenna=%02x", r->antenna);
  //loggers::get_instance().log("CommsigniaLayer::receiveData: rssi_ant2=%02x", r->rssi_ant2);
  //loggers::get_instance().log("CommsigniaLayer::receiveData: noise_ant1=%02x", r->noise_ant1);
  //loggers::get_instance().log("CommsigniaLayer::receiveData: noise_ant2=%02x", r->noise_ant2);
  // Filtering on antenna index
  if (r->antenna != static_cast<unsigned char>(std::strtoul(_params[Params::interface_id].c_str(), NULL, 10))) {
    // Discard packet
    return;
  } // else, continue

  const CommsigniaLayer::c2p_802_11p_hdr* h = (const CommsigniaLayer::c2p_802_11p_hdr*)(p + sizeof(CommsigniaLayer::c2p_recv));
  //loggers::get_instance().log("CommsigniaLayer::receiveData: frame_ctrl=%04x", ntohs(h->frame_ctrl));
  OCTETSTRING dst = OCTETSTRING(6, (const unsigned char*)&h->dst_addr);
  //loggers::get_instance().log_msg("CommsigniaLayer::receiveData: dst: ", dst);
  OCTETSTRING src = OCTETSTRING(6, (const unsigned char*)&h->src_addr);
  //loggers::get_instance().log_msg("CommsigniaLayer::receiveData: src: ", src);

  // Filtering on source MAC address of the packet
  std::string str;
  Params::const_iterator it = params.find(Params::mac_src); // Find in provided parameters, params
  if (it != params.cend()) {
    str = it->second;
  } else {
    str = _params[Params::mac_src];
  }
  loggers::get_instance().log("CommsigniaLayer::receiveData: compare %s with %s", str.c_str(), static_cast<const char *>(oct2str(src)));
  if (str.compare(static_cast<const char *>(oct2str(src))) == 0) {
    // Discard packet
    return;
  } // else, continue
  const CommsigniaLayer::c2p_llc_hdr* l;
  int length;
  if ((ntohs(h->frame_ctrl) & 0xf000) == 0x8000) {
    l = (const CommsigniaLayer::c2p_llc_hdr*)(p + sizeof(CommsigniaLayer::c2p_recv) + sizeof(CommsigniaLayer::c2p_802_11p_hdr) + sizeof(CommsigniaLayer::c2p_qos_ctrl));
    length = sizeof(CommsigniaLayer::c2p_recv) + sizeof(CommsigniaLayer::c2p_802_11p_hdr) + sizeof(CommsigniaLayer::c2p_qos_ctrl) + sizeof(CommsigniaLayer::c2p_llc_hdr);
  } else {
    l = (const CommsigniaLayer::c2p_llc_hdr*)(p + sizeof(CommsigniaLayer::c2p_recv) + sizeof(CommsigniaLayer::c2p_802_11p_hdr));
    length = sizeof(CommsigniaLayer::c2p_recv) + sizeof(CommsigniaLayer::c2p_802_11p_hdr) + sizeof(CommsigniaLayer::c2p_llc_hdr);
  }
  //loggers::get_instance().log("CommsigniaLayer::receiveData: dsap=%02x", l->dsap);
  //loggers::get_instance().log("CommsigniaLayer::receiveData: ssap=%02x", l->ssap);
  //loggers::get_instance().log("CommsigniaLayer::receiveData: type=%04x", l->type);
  // Check ether type
  OCTETSTRING et = str2oct(_params[Params::eth_type].c_str());
  //loggers::get_instance().log_msg("CommsigniaLayer::receiveData: ether type convert: ", et);
  //loggers::get_instance().log("CommsigniaLayer::receiveData: ether type convert: et[1]=%02x et[0]=%02x", et[1].get_octet(), et[0].get_octet());
  if ((et[1].get_octet() == (unsigned char)((l->type & 0xff00) >> 8)) && (et[0].get_octet() == (unsigned char)(l->type & 0xff))) { // Warning: Network ordered bytes
    // Extract payload
    data = OCTETSTRING(data.lengthof() - length, length + static_cast<const unsigned char *>(data));
    //loggers::get_instance().log_msg("CommsigniaLayer::receiveData: payload for upper layer:", data);
    
    // Update params
    CHARSTRING s = oct2str(dst);
    params[Params::mac_dst] = std::string(static_cast<const char *>(s));
    s = oct2str(src);
    params[Params::mac_src] = std::string(static_cast<const char *>(s));
    
    receiveToAllLayers(data, params);
  } // else, nothing to do
}

CommsigniaLayerFactory CommsigniaLayerFactory::_f;