Add cohda support

Signed-off-by: Tiago Garcia <tiago.rgarcia@av.it.pt>

CMakeLists.txt

@@ -34,7 +34,7 @@ include_directories(
   ${CMAKE_CURRENT_BINARY_DIR}/src
 )
 
-ament_auto_add_library(autoware_v2x SHARED
+set(SOURCES
   src/v2x_node.cpp
   src/v2x_app.cpp
   src/application.cpp
@@ -51,6 +51,34 @@ ament_auto_add_library(autoware_v2x SHARED
   src/security.cpp
 )
 
+if(DEFINED BUILD_COHDA)
+  set(SEARCH_PATHS
+    "/usr/include/linux/cohda/llc"
+    "/usr/include/cohda/llc"
+  )
+  find_file(COHDA_LLC_API_HEADER llc-api.h
+    PATHS ${SEARCH_PATHS}
+    CMAKE_FIND_ROOT_PATH_BOTH
+    DOC "Cohda LLC API header"
+  )
+  mark_as_advanced(COHDA_LLC_API_HEADER)
+  if(COHDA_LLC_API_HEADER)
+    get_filename_component(COHDA_LLC_INCLUDE_DIR ${COHDA_LLC_API_HEADER} DIRECTORY)
+    include_directories(
+      include
+      ${CMAKE_CURRENT_BINARY_DIR}/src
+      ${COHDA_LLC_INCLUDE_DIR}
+    )
+    list(APPEND SOURCES src/cohda_link.cpp)
+    add_compile_definitions(BUILD_COHDA)
+  else()
+    string(REPLACE ";" " " SEARCH_PATHS_STR "${SEARCH_PATHS}")
+    message(WARNING "Cohda LLC API header not found, could not enable Cohda support.\nSearched in: ${SEARCH_PATHS_STR}")
+  endif()
+endif()
+
+ament_auto_add_library(autoware_v2x SHARED ${SOURCES})
+
 protobuf_generate(TARGET autoware_v2x PROTOS src/cube_evk_radio.proto)
 
 target_link_libraries(autoware_v2x Vanetza::vanetza ${GeographicLib_LIBRARIES} Boost::thread Boost::program_options sqlite3 etsi_its_cam_ts_coding::etsi_its_cam_ts_coding etsi_its_cam_ts_conversion::etsi_its_cam_ts_conversion protobuf::libprotobuf)

config/autoware_v2x.param.yaml

@@ -1,8 +1,7 @@
 /**:
   ros__parameters:
-    link_layer: "cube-evk"
+    link_layer: "ethernet" # "ethernet" | "cube-evk" | "cohda"
-    network_interface: "v2x_testing"
+    target_device: "lo" # Should be the IP address of the target device or the network interface name
-    cube_ip: "192.168.94.84"
     is_sender: true
     publish_own_cams: false
     security: "none"

include/autoware_v2x/cohda_link.hpp

@@ -0,0 +1,17 @@
+#ifndef COHDA_LINK_HPP_IXOCQ5RH
+#define COHDA_LINK_HPP_IXOCQ5RH
+
+#include "raw_socket_link.hpp"
+
+class CohdaLink : public RawSocketLink
+{
+public:
+    using RawSocketLink::RawSocketLink;
+
+    void request(const vanetza::access::DataRequest&, std::unique_ptr<vanetza::ChunkPacket>) override;
+
+protected:
+    boost::optional<vanetza::EthernetHeader> parse_ethernet_header(vanetza::CohesivePacket&) const override;
+};
+
+#endif /* COHDA_LINK_HPP_IXOCQ5RH */

include/autoware_v2x/link_layer.hpp

@@ -22,7 +22,7 @@ class LinkLayer : public vanetza::access::Interface, public LinkLayerIndication
 {
 };
 
-std::unique_ptr<LinkLayer>
+std::unique_ptr<LinkLayer> create_link_layer(boost::asio::io_service &io_service, const std::string &name, const EthernetDevice &target_device);
-create_link_layer(boost::asio::io_service&, const EthernetDevice&, const std::string&, const std::string&);
+std::unique_ptr<LinkLayer> create_link_layer(boost::asio::io_service &io_service, const std::string &name, const std::string &target_device);
 
 #endif /* LINK_LAYER_HPP_FGEK0QTH */

launch/v2x.launch.xml

@@ -1,8 +1,8 @@
 <launch>
   <arg name="link_layer" default="ethernet"/>
-  <arg name="network_interface" default="v2x_testing"/>
+  <arg name="target_device" default="lo"/>
-  <arg name="cube_ip" default="127.0.0.1"/>
   <arg name="is_sender" default="true"/>
+  <arg name="publish_own_cams" default="false"/>
   <arg name="security" default="none"/>
   <node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen">
     <param from="$(find-pkg-share autoware_v2x)/config/autoware_v2x.param.yaml"/>

src/cohda_link.cpp

@@ -0,0 +1,81 @@
+#include <memory>
+#include <boost/optional.hpp>
+#include "autoware_v2x/cohda_link.hpp"
+#include <boost/optional/optional.hpp>
+#include <vanetza/access/access_category.hpp>
+#include <vanetza/access/data_request.hpp>
+#include <vanetza/access/g5_link_layer.hpp>
+#include <vanetza/common/byte_buffer.hpp>
+#include <vanetza/common/serialization_buffer.hpp>
+#include <vanetza/common/clock.hpp>
+#include <vanetza/dcc/mapping.hpp>
+#include <vanetza/net/chunk_packet.hpp>
+#include <vanetza/net/cohesive_packet.hpp>
+#include <vanetza/net/ethernet_header.hpp>
+#include <cassert>
+#include <llc-api.h>
+
+using namespace vanetza;
+
+void CohdaLink::request(const vanetza::access::DataRequest& request, std::unique_ptr<vanetza::ChunkPacket> packet)
+{
+  access::G5LinkLayer link_layer;
+  access::ieee802::dot11::QosDataHeader& mac_header = link_layer.mac_header;
+  mac_header.destination = request.destination_addr;
+  mac_header.source = request.source_addr;
+  mac_header.qos_control.user_priority(request.access_category);
+
+  ByteBuffer link_layer_buffer;
+  serialize_into_buffer(link_layer, link_layer_buffer);
+  assert(link_layer_buffer.size() == access::G5LinkLayer::length_bytes);
+  packet->layer(OsiLayer::Link) = std::move(link_layer_buffer);
+
+  const std::size_t payload_size = packet->size();
+  const std::size_t total_size = sizeof(tMKxTxPacket) + payload_size;
+
+  tMKxTxPacket phy = { 0 };
+  phy.Hdr.Type = MKXIF_TXPACKET;
+  phy.Hdr.Len = total_size;
+  phy.TxPacketData.TxAntenna = MKX_ANT_DEFAULT;
+  phy.TxPacketData.TxFrameLength = payload_size;
+  auto phy_ptr = reinterpret_cast<const uint8_t*>(&phy);
+  packet->layer(OsiLayer::Physical) = ByteBuffer { phy_ptr, phy_ptr + sizeof(tMKxTxPacket) };
+
+  transmit(std::move(packet));
+}
+
+boost::optional<vanetza::EthernetHeader> CohdaLink::parse_ethernet_header(vanetza::CohesivePacket& packet) const
+{
+  static const std::size_t min_length = sizeof(tMKxRxPacket) + access::G5LinkLayer::length_bytes + access::ieee802::dot11::fcs_length_bytes;
+  if (packet.size(OsiLayer::Physical) < min_length) {
+      return boost::none;
+  }
+
+  packet.set_boundary(OsiLayer::Physical, sizeof(tMKxRxPacket));
+  auto phy = reinterpret_cast<const tMKxRxPacket*>(&*packet[OsiLayer::Physical].begin());
+  if (phy->Hdr.Type != MKXIF_RXPACKET) {
+      return boost::none;
+  }
+
+  if (phy->Hdr.Len != packet.size() || phy->RxPacketData.RxFrameLength != packet.size() - sizeof(tMKxRxPacket)) {
+      return boost::none;
+  }
+
+  if (!phy->RxPacketData.FCSPass) {
+      return boost::none;
+  }
+
+  packet.trim(OsiLayer::Link, packet.size() - access::ieee802::dot11::fcs_length_bytes);
+  packet.set_boundary(OsiLayer::Link, access::G5LinkLayer::length_bytes);
+  access::G5LinkLayer link_layer;
+  deserialize_from_range(link_layer, packet[OsiLayer::Link]);
+  if (!access::check_fixed_fields(link_layer)) {
+      return boost::none;
+  }
+
+  EthernetHeader eth;
+  eth.destination = link_layer.mac_header.destination;
+  eth.source = link_layer.mac_header.source;
+  eth.type = link_layer.llc_snap_header.protocol_id;
+  return eth;
+}

src/link_layer.cpp

@@ -1,25 +1,46 @@
 #include "autoware_v2x/link_layer.hpp"
+#ifdef BUILD_COHDA
+#include "autoware_v2x/cohda_link.hpp"
+#endif
 #include "autoware_v2x/cube_evk_link.hpp"
-#include <boost/asio/generic/raw_protocol.hpp>
-#include <vanetza/access/ethertype.hpp>
 #include "autoware_v2x/raw_socket_link.hpp"
 
-std::unique_ptr<LinkLayer> create_link_layer(
+#include <vanetza/access/ethertype.hpp>
-  boost::asio::io_service &io_service, const EthernetDevice &device, const std::string &name, const std::string &cube_ip)
+
+#include <boost/asio/generic/raw_protocol.hpp>
+
+std::unique_ptr<LinkLayer> create_link_layer(boost::asio::io_service &io_service, const std::string &name, const EthernetDevice &target_device)
 {
   std::unique_ptr<LinkLayer> link_layer;
 
-  if (name == "ethernet") {
   boost::asio::generic::raw_protocol raw_protocol(
     AF_PACKET, vanetza::access::ethertype::GeoNetworking.net());
   boost::asio::generic::raw_protocol::socket raw_socket(io_service, raw_protocol);
-    raw_socket.bind(device.endpoint(AF_PACKET));
+  raw_socket.bind(target_device.endpoint(AF_PACKET));
+  if (name == "ethernet") {
     link_layer.reset(new RawSocketLink{std::move(raw_socket)});
-  } else if (name == "cube-evk") {
-    link_layer.reset(new CubeEvkLink{ io_service, boost::asio::ip::address_v4::from_string(cube_ip) });
   }
-
+#ifdef BUILD_COHDA
-  // Removed Cohda and UDP Support
+  else if (name == "cohda") {
+    link_layer.reset(new CohdaLink{std::move(raw_socket)});
+  }
+#endif
+  else {
+    throw std::runtime_error("Unknown link layer: " + name);
+  }
+
+  return link_layer;
+}
+
+std::unique_ptr<LinkLayer> create_link_layer(boost::asio::io_service &io_service, const std::string &name, const std::string &target_device)
+{
+  std::unique_ptr<LinkLayer> link_layer;
+
+  if (name == "cube-evk") {
+    link_layer.reset(new CubeEvkLink{ io_service, boost::asio::ip::address_v4::from_string(target_device) });
+  } else {
+    throw std::runtime_error("Unknown link layer: " + name);
+  }
 
   return link_layer;
 }

src/v2x_app.cpp

@@ -16,6 +16,7 @@
 
 #include <vanetza/asn1/cpm.hpp>
 #include <vanetza/asn1/cam.hpp>
+#include <regex>
 #include <sstream>
 #include <memory>
 #include <GeographicLib/UTMUPS.hpp>
@@ -133,17 +134,46 @@ namespace v2x
     node_->get_parameter("link_layer", link_layer_name);
     RCLCPP_INFO(node_->get_logger(), "Link Layer: %s", link_layer_name.c_str());
 
-    std::string network_interface;
+#ifdef BUILD_COHDA
-    node_->get_parameter("network_interface", network_interface);
+    std::vector<std::string> valid_link_layers = {"ethernet", "cube-evk", "cohda"};
-    RCLCPP_INFO(node_->get_logger(), "Network Interface: %s", network_interface.c_str());
+#else
+    std::vector<std::string> valid_link_layers = {"ethernet", "cube-evk"};
+#endif
+    if (std::find(valid_link_layers.begin(), valid_link_layers.end(), link_layer_name) == valid_link_layers.end()) {
+      throw std::runtime_error("Invalid link layer: " + link_layer_name);
+    }
 
-    EthernetDevice device(network_interface.c_str());
+    std::string target_device;
-    vanetza::MacAddress mac_address = device.address();
+    node_->get_parameter("target_device", target_device);
+    const std::regex ip_pattern(R"(^((25[0-5]|(2[0-4]|1\d|[1-9]|)\d)\.?\b){4}$)");
+    bool is_ip = std::regex_match(target_device, ip_pattern);
+    RCLCPP_INFO(node_->get_logger(), "IS_IP: %d, Target Device: %s", is_ip, target_device.c_str());
+    if (!is_ip && !(link_layer_name == "ethernet" || link_layer_name == "cohda")) {
+      throw std::runtime_error("Invalid target device: " + target_device + "\nMust use ethernet link layer for network interface");
+    }
+
+    vanetza::MacAddress mac_address;
+    std::unique_ptr<LinkLayer> link_layer;
+    if (!is_ip) {
+      EthernetDevice device(target_device.c_str());
+      mac_address = device.address();
 
       std::stringstream sout;
       sout << mac_address;
       RCLCPP_INFO(node_->get_logger(), "MAC Address: '%s'", sout.str().c_str());
 
+      link_layer = create_link_layer(io_service, link_layer_name, device);
+      RCLCPP_INFO(node_->get_logger(), "Ethernet Device: %s", target_device.c_str());
+    } else {
+      if (link_layer_name == "cube-evk") {
+        link_layer = create_link_layer(io_service, link_layer_name, target_device);
+        RCLCPP_INFO(node_->get_logger(), "CubeEVK IP: %s", target_device.c_str());
+      } else {
+        throw std::runtime_error("Invalid link layer: " + link_layer_name);
+      }
+      mac_address = parse_mac_address("00:00:00:00:00:00").value();
+    }
+
     // Geonetworking Management Infirmation Base (MIB) defines the GN protocol constants.
     gn::MIB mib;
     mib.itsGnLocalGnAddr.mid(mac_address);
@@ -152,12 +182,6 @@ namespace v2x
     mib.itsGnSecurity = false;
     mib.itsGnProtocolVersion = 1;
 
-    std::string cube_ip;
-    node_->get_parameter("cube_ip", cube_ip);
-    RCLCPP_INFO(node_->get_logger(), "CubeEVK IP: %s", cube_ip.c_str());
-
-    // Create raw socket on device and LinkLayer object
-    auto link_layer =  create_link_layer(io_service, device, link_layer_name, cube_ip);
     auto positioning = create_position_provider(io_service, trigger.runtime());
 
     po::variables_map security_options;

src/v2x_node.cpp

@@ -61,8 +61,7 @@ namespace v2x
 
     // Declare Parameters
     this->declare_parameter<std::string>("link_layer", "ethernet");
-    this->declare_parameter<std::string>("network_interface", "v2x_testing");
+    this->declare_parameter<std::string>("target_device", "lo");
-    this->declare_parameter<std::string>("cube_ip", "127.0.0.1");
     this->declare_parameter<bool>("is_sender", true);
     this->declare_parameter<bool>("publish_own_cams", true);
     this->declare_parameter<std::string>("security", "none");