Added TCP LinkLayer to socktap

tools/socktap/CMakeLists.txt

@@ -23,6 +23,7 @@ add_executable(socktap
     raw_socket_link.cpp
     router_context.cpp
     security.cpp
+    tcp_link.cpp
     time_trigger.cpp
     udp_link.cpp
 )

tools/socktap/link_layer.cpp

@@ -1,5 +1,6 @@
 #include "link_layer.hpp"
 #include "raw_socket_link.hpp"
+#include "tcp_link.hpp"
 #include "udp_link.hpp"
 #include <vanetza/access/ethertype.hpp>
 #include <boost/asio/generic/raw_protocol.hpp>
@@ -9,7 +10,7 @@
 #endif
 
 std::unique_ptr<LinkLayer>
-create_link_layer(boost::asio::io_service& io_service, const EthernetDevice& device, const std::string& name)
+create_link_layer(boost::asio::io_service& io_service, const EthernetDevice& device, const std::string& name, const boost::program_options::variables_map& vm)
 {
     std::unique_ptr<LinkLayer> link_layer;
 
@@ -29,7 +30,43 @@ create_link_layer(boost::asio::io_service& io_service, const EthernetDevice& dev
         namespace ip = boost::asio::ip;
         ip::udp::endpoint multicast(ip::address::from_string("239.118.122.97"), 8947);
         link_layer.reset(new UdpLink { io_service, multicast });
+    } else if (name.substr(0, 3) == "tcp") {
+        namespace ip = boost::asio::ip;
+
+        TcpLink* tcp = new TcpLink { io_service };
+
+        std::string tcp_ip;
+        unsigned short tcp_port, tcp_ip_len;
+
+        if (vm.count("tcp-connect")) {
+            for (const std::string& ip_port : vm["tcp-connect"].as<std::vector<std::string>>()) {
+                tcp_ip_len = ip_port.find(":");
+                tcp_ip = ip_port.substr(0, tcp_ip_len);
+                tcp_port = std::stoi(ip_port.substr(tcp_ip_len + 1));                
+                tcp->connect({ip::address::from_string(tcp_ip), tcp_port});
+            }
+        }
+
+        if (vm.count("tcp-accept")) {
+            for (const std::string& ip_port : vm["tcp-accept"].as<std::vector<std::string>>()) {
+                tcp_ip_len = ip_port.find(":");
+                tcp_ip = ip_port.substr(0, tcp_ip_len);
+                tcp_port = std::stoi(ip_port.substr(tcp_ip_len + 1));
+                tcp->accept({ip::address::from_string(tcp_ip), tcp_port});
+            }
+        }
+
+        link_layer.reset(tcp);
+
     }
 
     return link_layer;
 }
+
+void add_link_layer_options(boost::program_options::options_description& options)
+{
+    options.add_options()
+        ("tcp-connect", boost::program_options::value<std::vector<std::string>>()->multitoken(), "Connect to TCP-Host(s). Comma separated list of [ip]:[port].")
+        ("tcp-accept", boost::program_options::value<std::vector<std::string>>()->multitoken(), "Accept TCP-Connections. Comma separated list of [ip]:[port].")
+    ;
+}

tools/socktap/link_layer.hpp

@@ -6,6 +6,8 @@
 #include <vanetza/net/cohesive_packet.hpp>
 #include <vanetza/net/ethernet_header.hpp>
 #include <boost/asio/io_service.hpp>
+#include <boost/program_options/options_description.hpp>
+#include <boost/program_options/variables_map.hpp>
 #include <memory>
 #include <string>
 
@@ -23,7 +25,9 @@ class LinkLayer : public vanetza::access::Interface, public LinkLayerIndication
 };
 
 std::unique_ptr<LinkLayer>
-create_link_layer(boost::asio::io_service&, const EthernetDevice&, const std::string& name);
+create_link_layer(boost::asio::io_service&, const EthernetDevice&, const std::string& name, const boost::program_options::variables_map& vm);
+
+void add_link_layer_options(boost::program_options::options_description&);
 
 #endif /* LINK_LAYER_HPP_FGEK0QTH */
 

tools/socktap/main.cpp

@@ -36,6 +36,7 @@ int main(int argc, const char** argv)
     ;
     add_positioning_options(options);
     add_security_options(options);
+    add_link_layer_options(options);
 
     po::positional_options_description positional_options;
     positional_options.add("interface", 1);
@@ -80,7 +81,7 @@ int main(int argc, const char** argv)
         }
 
         const std::string link_layer_name = vm["link-layer"].as<std::string>();
-        auto link_layer =  create_link_layer(io_service, device, link_layer_name);
+        auto link_layer =  create_link_layer(io_service, device, link_layer_name, vm);
         if (!link_layer) {
             std::cerr << "No link layer '" << link_layer_name << "' found." << std::endl;
             return 1;

tools/socktap/tcp_link.cpp

@@ -0,0 +1,169 @@
+#include "tcp_link.hpp"
+#include <vanetza/access/data_request.hpp>
+#include <vanetza/net/ethernet_header.hpp>
+#include <boost/asio/ip/multicast.hpp>
+#include <boost/bind.hpp>
+#include <iostream>
+
+namespace ip = boost::asio::ip;
+using namespace vanetza;
+
+TcpLink::TcpLink(boost::asio::io_service& io_service) :
+    io_service_(&io_service)
+{
+
+}
+
+TcpLink::~TcpLink()
+{
+    for (auto &ts : sockets_) {
+        delete ts;
+    }
+    for (auto &acceptor : acceptors_) {
+        delete acceptor.second;
+    }
+}
+
+void TcpLink::indicate(IndicationCallback cb)
+{
+    callback_ = cb;
+}
+
+void TcpLink::request(const access::DataRequest& request, std::unique_ptr<ChunkPacket> packet)
+{
+    packet->layer(OsiLayer::Link) = create_ethernet_header(request.destination_addr, request.source_addr, request.ether_type);
+
+    std::array<boost::asio::const_buffer, layers_> const_buffers;
+    for (auto& layer : osi_layer_range<OsiLayer::Link, OsiLayer::Application>()) {
+        const auto index = distance(OsiLayer::Link, layer);
+        packet->layer(layer).convert(tx_buffers_[index]);
+        const_buffers[index] = boost::asio::buffer(tx_buffers_[index]);
+    }
+
+    std::list<TcpSocket*>::iterator i = sockets_.begin();
+
+    while (i != sockets_.end()) {
+        if ((*i)->connected()) {
+            (*i)->request(const_buffers);
+            i++;
+        } else {
+            sockets_.erase(i++);
+            std::cerr << "Socket removed" << std::endl;
+        }
+    }
+}
+
+void TcpLink::connect(ip::tcp::endpoint ep)
+{
+    TcpSocket* ts = new TcpSocket(*io_service_, callback_);
+    ts->connect(ep);
+    sockets_.push_back(ts);
+}
+
+void TcpLink::accept(ip::tcp::endpoint ep)
+{
+    if (!acceptors_[ep]) {
+        acceptors_[ep] = new ip::tcp::acceptor(*io_service_, ep);
+    }
+
+    TcpSocket* ts = new TcpSocket(*io_service_, callback_);
+    boost::system::error_code ec;
+
+    acceptors_[ep]->async_accept(
+        ts->socket(),
+        boost::bind(
+            &TcpLink::accept_handler,
+            this,
+            ec,
+            ts,
+            ep
+        )
+    );
+
+}
+
+void TcpLink::accept_handler(boost::system::error_code& ec, TcpSocket* ts, ip::tcp::endpoint ep)
+{
+    sockets_.push_back(ts);
+    ts->do_receive();
+    ts->connected(true);
+    accept(ep);
+}
+
+
+/**
+ * TcpSocket
+ */
+
+TcpSocket::TcpSocket(boost::asio::io_service& io_service, IndicationCallback& cb) :
+    socket_(io_service),
+    callback_(&cb),
+    rx_buffer_(2560, 0x00)
+{
+
+}
+
+void TcpSocket::connect(ip::tcp::endpoint ep)
+{
+    boost::system::error_code ec;
+    socket_.connect(ep, ec);
+    if (!ec) {
+        is_connected_ = true;
+        do_receive();
+    }
+
+}
+
+void TcpSocket::request(std::array<boost::asio::const_buffer, layers_> const_buffers)
+{
+    boost::system::error_code ec;
+    socket_.write_some(const_buffers, ec);
+
+    if (ec) {
+        is_connected_ = false;
+    }
+}
+
+void TcpSocket::do_receive()
+{
+    socket_.async_read_some(boost::asio::buffer(rx_buffer_),
+        [this](boost::system::error_code ec, std::size_t length) {
+            if (!ec) {
+                is_connected_ = true;
+                ByteBuffer buffer(rx_buffer_.begin(), rx_buffer_.begin() + length);
+                CohesivePacket packet(std::move(buffer), OsiLayer::Link);
+                if (packet.size(OsiLayer::Link) < EthernetHeader::length_bytes) {
+                    std::cerr << "Dropped TCP packet too short to contain Ethernet header\n";
+                } else {
+                    packet.set_boundary(OsiLayer::Link, EthernetHeader::length_bytes);
+                    auto link_range = packet[OsiLayer::Link];
+                    EthernetHeader eth = decode_ethernet_header(link_range.begin(), link_range.end());
+                    if (callback_) {
+                        (*callback_)(std::move(packet), eth);
+                    }
+                }
+                do_receive();
+            } else {
+                is_connected_ = false;
+            }
+        });
+
+}
+
+ip::tcp::socket& TcpSocket::socket()
+{
+    return socket_;
+}
+
+bool TcpSocket::connected()
+{
+    return is_connected_;
+}
+
+void TcpSocket::connected(bool b)
+{
+    is_connected_ = b;
+}
+
+
+

tools/socktap/tcp_link.hpp

@@ -0,0 +1,64 @@
+#ifndef TCP_LINK_HPP_A16QFBX3
+#define TCP_LINK_HPP_A16QFBX3
+
+#include "link_layer.hpp"
+#include <vanetza/common/byte_buffer.hpp>
+#include <boost/asio/ip/tcp.hpp>
+#include <boost/asio/io_service.hpp>
+#include <array>
+#include <list>
+
+static constexpr std::size_t layers_ = num_osi_layers(vanetza::OsiLayer::Link, vanetza::OsiLayer::Application);
+
+
+class TcpSocket
+{
+public:
+    using IndicationCallback = std::function<void(vanetza::CohesivePacket&&, const vanetza::EthernetHeader&)>;
+
+    TcpSocket(boost::asio::io_service&, IndicationCallback&);
+
+    void connect(boost::asio::ip::tcp::endpoint);
+    void request(std::array<boost::asio::const_buffer, layers_>);
+    void do_receive();
+
+    boost::asio::ip::tcp::socket& socket();
+    bool connected();
+    void connected(bool);
+
+private:
+
+    bool is_connected_;
+    boost::asio::io_service* io_service_;
+    boost::asio::ip::tcp::endpoint endpoint_;
+    boost::asio::ip::tcp::socket socket_;
+    vanetza::ByteBuffer rx_buffer_;
+    IndicationCallback* callback_;
+
+};
+
+
+class TcpLink : public LinkLayer
+{
+public:
+    TcpLink(boost::asio::io_service&);
+    ~TcpLink();
+
+    void indicate(IndicationCallback) override;
+    void request(const vanetza::access::DataRequest&, std::unique_ptr<vanetza::ChunkPacket>) override;
+    void connect(boost::asio::ip::tcp::endpoint);
+    void accept(boost::asio::ip::tcp::endpoint);
+    void accept_handler(boost::system::error_code& ec, TcpSocket* ts, boost::asio::ip::tcp::endpoint ep);
+
+private:
+    std::list<TcpSocket*> sockets_;
+    std::map<boost::asio::ip::tcp::endpoint, boost::asio::ip::tcp::acceptor*> acceptors_;
+    IndicationCallback callback_;
+    boost::asio::io_service* io_service_;
+    std::array<vanetza::ByteBuffer, layers_> tx_buffers_;
+};
+
+
+
+#endif /* TCP_LINK_HPP_A16QFBX3 */
+