Add PacketLossManager

Those packets are probably packets truncated because the buffer is not
large enough.

TunDevice.cpp

@@ -72,6 +72,31 @@ TunDevice::~TunDevice() {
     close(_fd);
 }
 
+uint16_t TunDevice::get_mtu() const {
+    int sockfd = socket(AF_INET, SOCK_DGRAM, 0);  // Any socket will do
+
+    struct ifreq ifr;
+    ifr.ifr_addr.sa_family = AF_INET6;
+    strncpy(ifr.ifr_name, _dev_name.c_str(), sizeof(ifr.ifr_name)-1);
+    if (ioctl(sockfd, SIOCGIFMTU, (caddr_t)&ifr) < 0)
+        return 0;
+    close(sockfd);
+    return ifr.ifr_mtu;
+}
+
+bool TunDevice::set_mtu(uint16_t mtu) {
+    int sockfd = socket(AF_INET, SOCK_DGRAM, 0);  // Any socket will do
+
+    struct ifreq ifr;
+    ifr.ifr_addr.sa_family = AF_INET6;
+    strncpy(ifr.ifr_name, _dev_name.c_str(), sizeof(ifr.ifr_name)-1);
+    ifr.ifr_mtu = mtu;
+    if (ioctl(sockfd, SIOCSIFMTU, (caddr_t)&ifr) < 0)
+        return false;
+    close(sockfd);
+    return true;
+}
+
 size_t TunDevice::poll_packet(char* read_buffer, size_t buf_size, int timeout) {
     int poll_rc = poll(&_poll_fd, 1, timeout);
     if(poll_rc < 0) {

TunDevice.hpp

@@ -29,6 +29,12 @@ class TunDevice {
         const std::string& get_dev_name() const { return _dev_name; }
         int get_fd() const { return _fd; }
 
+        /** Get the interface's MTU */
+        uint16_t get_mtu() const;
+
+        /** Set the interface's MTU */
+        bool set_mtu(uint16_t mtu);
+
         /* Reads a packet from the device.
          * Timeouts after `timeout` ms, or never if `timeout < 0`.
          * Upon timeout, returns 0.

UdpVpn.cpp

@@ -14,8 +14,9 @@
 static const size_t VPN_MTU = 1460; // TODO determine this -- issue #3
 
 UdpVpn::UdpVpn()
-    : _stopped(false), _vpn_mtu(VPN_MTU), _tun_dev("cvpn%d")
+    : _stopped(false), _vpn_mtu(VPN_MTU), _tun_dev("cvpn%d"), _peer(nullptr)
 {
+    _tun_dev.set_mtu(VpnPacket::get_tunnelled_mtu(_vpn_mtu));
     _socket = socket(AF_INET6, SOCK_DGRAM, 0);
     if(_socket < 0)
         throw UdpVpn::InitializationError("Cannot create socket", errno, true);
@@ -74,14 +75,19 @@ size_t UdpVpn::read_from_tun(char* buffer, size_t len) {
     return _tun_dev.read(buffer, len);
 }
 
-size_t UdpVpn::read_from_tun(VpnPacket& packet) {
-    size_t nread =
-        read_from_tun(packet.get_payload(), packet.get_payload_space());
+size_t UdpVpn::read_from_tun(TunnelledPacket& packet) {
+    size_t payload_space = packet.get_payload_space();
+    size_t nread = read_from_tun(packet.get_payload(), payload_space);
     packet.set_payload_size(nread);
     if(!packet.parse_as_ipv6()) {
         debugf("Ignoring packet with invalid header\n");
         return 0;
     }
+    if(nread != packet.get_ipv6_header().packet_length()) {
+        debugf("Ignoring packet with bad size (expected %d, got %d, buffer %d)\n",
+               packet.get_ipv6_header().packet_length(), nread, payload_space);
+        return 0;
+    }
     return nread;
 }
 
@@ -118,10 +124,75 @@ size_t UdpVpn::read_from_udp(VpnPacket& packet, sockaddr_in6& peer_addr) {
     size_t nread =
         read_from_udp(packet.get_data(), packet.get_data_space(), peer_addr);
     packet.set_data_size(nread);
-    if(!packet.parse_as_ipv6()) {
-        debugf("Ignoring packet with invalid header\n");
-        return 0;
-    }
     return nread;
 }
 
+size_t UdpVpn::transmit_to_peer(VpnPacket& packet) {
+    if(!_peer) {
+        debugf("Dropping packet: no peer yet.\n");
+        return 0;
+    }
+    return _peer->write(packet);
+}
+
+void UdpVpn::receive_from_tun() {
+    VpnPacket packet(_vpn_mtu);
+    TunnelledPacket tunnelled = TunnelledPacket::create(packet);
+    size_t nread = read_from_tun(tunnelled);
+    if(nread == 0)
+        return;
+
+    if(!_peer) {
+        debugf("Dropping packet: no peer yet.\n");
+        return;
+    }
+    packet.set_peer(_peer.get());
+
+    kdebugf("Transmitting %s -> %s, size %d\n",
+            format_address(tunnelled.get_ipv6_header().source.s6_addr),
+            format_address(tunnelled.get_ipv6_header().dest.s6_addr),
+            nread);
+
+    packet.prepare_for_sending();
+    transmit_to_peer(packet);
+}
+
+void UdpVpn::receive_from_udp() {
+    VpnPacket packet(_vpn_mtu);
+    sockaddr_in6 peer_ext_addr;
+    size_t nread = read_from_udp(packet, peer_ext_addr);
+    if(nread == 0)
+        return;
+
+    // If we don't have a peer yet -- we're just setting the peer to nullptr.
+    packet.set_peer(_peer.get());
+
+    for(VpnPacketTLV tlv=packet.first_tlv();
+            !tlv.past_the_end();
+            tlv.seek_next_tlv())
+    {
+        switch(tlv.get_type()) {
+            case VpnPacket::PAYLOAD_TYPE_TUNNELLED:
+                {
+                    TunnelledPacket tunnelled(tlv);
+                    acquire_peer(tunnelled, peer_ext_addr);
+                    receive_tunnelled_tlv(tunnelled);
+                }
+                break;
+
+            case VpnPacket::PAYLOAD_TYPE_UNDEF:
+            default:
+                debugf("#%d+%lu: ignoring TLV with bad type %d.\n",
+                        packet.get_seqno(), tlv.get_offset(),
+                        tlv.get_type());
+                break;
+        }
+    }
+}
+
+void UdpVpn::receive_tunnelled_tlv(TunnelledPacket& packet) {
+    // Reinject into tun
+    kdebugf("Reinjecting tunnelled packet of size %d\n",
+            packet.get_payload_size());
+    _tun_dev.write(packet.get_payload(), packet.get_payload_size());
+}

UdpVpn.hpp

@@ -42,18 +42,27 @@ class UdpVpn {
         void stop() { _stopped = true; }
 
     protected:
-        virtual void receive_from_tun() = 0;
-        virtual void receive_from_udp() = 0;
+        virtual void acquire_peer(
+                TunnelledPacket& packet,
+                const sockaddr_in6& peer_ext_addr) = 0;
 
         size_t read_from_tun(char* buffer, size_t len);
-        size_t read_from_tun(VpnPacket& packet);
+        size_t read_from_tun(TunnelledPacket& packet);
         size_t read_from_udp(char* buffer, size_t len, sockaddr_in6& peer_addr);
         size_t read_from_udp(VpnPacket& packet, sockaddr_in6& peer_addr);
 
+        size_t transmit_to_peer(VpnPacket& packet);
+
+        void receive_from_tun();
+        void receive_from_udp();
+
+        void receive_tunnelled_tlv(TunnelledPacket& packet);
+
         int _socket;
         bool _stopped;
 
         size_t _vpn_mtu;
 
         TunDevice _tun_dev;
+        std::unique_ptr<VpnPeer> _peer;
 };

UdpVpnClient.cpp

@@ -4,55 +4,14 @@
 #include "ip_header.hpp"
 
 UdpVpnClient::UdpVpnClient(const struct sockaddr_in6& server) : UdpVpn() {
-    memset(&_server_addr, 0, sizeof(_server_addr));
-    set_server(server);
+    _peer = std::make_unique<VpnPeer>(this, server, in6addr_any);
 }
 
-void UdpVpnClient::set_server(const struct sockaddr_in6& server_addr) {
-    if(server_addr.sin6_family != AF_INET6)
-        throw UdpVpn::InitializationError("Server address must be IPv6");
-
-    memcpy(&_server_addr, &server_addr, sizeof(_server_addr));
-}
-
-void UdpVpnClient::receive_from_tun() {
-    VpnPacket packet(_vpn_mtu);
-    size_t nread = read_from_tun(packet);
-    if(nread == 0)
+void UdpVpnClient::acquire_peer(
+        TunnelledPacket& packet,
+        const sockaddr_in6&)
+{
+    if(!packet.parse_as_ipv6())
         return;
-
-    kdebugf("Transmitting %s -> %s, size %d\n",
-            format_address(packet.get_ipv6_header().source.s6_addr),
-            format_address(packet.get_ipv6_header().dest.s6_addr),
-            nread);
-
-    packet.prepare_for_sending();
-    write_to_server(packet);
-}
-
-void UdpVpnClient::receive_from_udp() {
-    VpnPacket packet(_vpn_mtu);
-    sockaddr_in6 peer_addr;
-    size_t nread = read_from_udp(packet, peer_addr);
-    if(nread == 0)
-        return;
-
-    // Reinject into tun
-    kdebugf("Receiving packet #%u of size %d from %s\n",
-            packet.get_seqno(),
-            nread,
-            format_address(packet.get_ipv6_header().source.s6_addr));
-    _tun_dev.write(packet.get_payload(), packet.get_payload_size());
-}
-
-size_t UdpVpnClient::write_to_server(const VpnPacket& packet) {
-    ssize_t nsent;
-
-    nsent = sendto(_socket, packet.get_data(), packet.get_data_size(),
-            MSG_CONFIRM,
-            (const struct sockaddr*) &_server_addr, sizeof(_server_addr));
-    if(nsent < 0)
-        throw NetError("Could not send UDP packet", errno, true);
-
-    return (size_t) nsent;
+    _peer->set_int_addr(packet.get_ipv6_header().source);
 }

UdpVpnClient.hpp

@@ -7,12 +7,7 @@ class UdpVpnClient: public UdpVpn {
         UdpVpnClient(const struct sockaddr_in6& server);
 
     protected:
-        void set_server(const struct sockaddr_in6& server_addr);
-
-        virtual void receive_from_tun();
-        virtual void receive_from_udp();
-
-        size_t write_to_server(const VpnPacket& packet);
-
-        struct sockaddr_in6 _server_addr;
+        virtual void acquire_peer(
+                TunnelledPacket& packet,
+                const sockaddr_in6& peer_ext_addr);
 };

UdpVpnServer.cpp

@@ -15,6 +15,27 @@ UdpVpnServer::UdpVpnServer(const struct in6_addr& bind_addr6, in_port_t port)
     bind(bind_addr6, port);
 }
 
+void UdpVpnServer::acquire_peer(
+        TunnelledPacket& packet,
+        const sockaddr_in6& peer_ext_addr)
+{
+    if(_peer)
+        return;  // Refusing a connection if we already have one
+    // TODO: reset state at some point/if connection broken
+
+    if(!packet.parse_as_ipv6())
+        return;
+    const in6_addr& peer_inner_addr = packet.get_ipv6_header().source;
+    _peer = std::make_unique<VpnPeer>(this, peer_ext_addr, peer_inner_addr);
+
+    packet.get_packet().set_peer(_peer.get());
+
+    debugf("Got new peer %s:%d -- %s\n",
+            format_address(peer_ext_addr.sin6_addr.s6_addr),
+            htons(peer_ext_addr.sin6_port),
+            format_address(peer_inner_addr.s6_addr));
+}
+
 void UdpVpnServer::bind(const struct in6_addr& bind_addr6, in_port_t port) {
     int rc;
 
@@ -30,66 +51,3 @@ void UdpVpnServer::bind(const struct in6_addr& bind_addr6, in_port_t port) {
 
     debugf("> Listening on port %d\n", port);
 }
-
-
-std::shared_ptr<VpnPeer> UdpVpnServer::get_peer_for_ip(
-        const in6_addr& peer_addr)
-{
-    auto peer_iter = _peers.find(peer_addr);
-    if(peer_iter == _peers.end()) // Unknown peer
-        return nullptr;
-    return peer_iter->second;
-}
-
-void UdpVpnServer::receive_from_tun() {
-    VpnPacket packet(_vpn_mtu);
-    size_t nread = read_from_tun(packet);
-    if(nread == 0)
-        return;
-
-    // Recover VpnPeer -- or drop if new
-    const in6_addr& peer_inner_addr = packet.get_ipv6_header().dest;
-    std::shared_ptr<VpnPeer> peer = get_peer_for_ip(peer_inner_addr);
-    if(!peer) {
-        debugf("Dropping packet for destination %s -- unknown peer.\n",
-                format_address(peer_inner_addr.s6_addr));
-        return;
-    }
-    packet.set_peer(peer);
-
-    kdebugf("Transmitting %s -> %s, size %d\n",
-            format_address(packet.get_ipv6_header().source.s6_addr),
-            format_address(packet.get_ipv6_header().dest.s6_addr),
-            nread);
-    packet.prepare_for_sending();
-    peer->write(packet);
-}
-
-void UdpVpnServer::receive_from_udp() {
-    VpnPacket packet(_vpn_mtu);
-    sockaddr_in6 peer_addr;
-    size_t nread = read_from_udp(packet, peer_addr);
-    if(nread == 0)
-        return;
-
-    // Recover VpnPeer -- or create if new
-    const in6_addr& peer_inner_addr = packet.get_ipv6_header().source;
-    std::shared_ptr<VpnPeer> peer = get_peer_for_ip(peer_inner_addr);
-    if(!peer) {
-        peer = std::make_shared<VpnPeer>(this, peer_addr, peer_inner_addr);
-        _peers.insert({peer_inner_addr, peer});
-
-        debugf("Got new peer %s:%d -- %s\n",
-                format_address(peer_addr.sin6_addr.s6_addr),
-                htons(peer_addr.sin6_port),
-                format_address(peer_inner_addr.s6_addr));
-    }
-    packet.set_peer(peer);
-
-    // Reinject into tun
-    kdebugf("Receiving packet #%u of size %d from %s\n",
-            packet.get_seqno(),
-            nread,
-            format_address(packet.get_ipv6_header().source.s6_addr));
-    _tun_dev.write(packet.get_payload(), packet.get_payload_size());
-}

UdpVpnServer.hpp

@@ -10,14 +10,11 @@ class UdpVpnServer: public UdpVpn {
         UdpVpnServer(in_port_t port);
         UdpVpnServer(const struct in6_addr& bind_addr6, in_port_t port);
     protected:
+        virtual void acquire_peer(
+                TunnelledPacket& packet,
+                const sockaddr_in6& peer_ext_addr);
+
         void bind(const struct in6_addr& bind_addr6, in_port_t port);
 
-        /** Get the peer associated to this (internal) IP. */
-        std::shared_ptr<VpnPeer> get_peer_for_ip(const in6_addr& peer_addr);
-
-        virtual void receive_from_tun();
-        virtual void receive_from_udp();
-
         struct sockaddr_in6 _bind_addr;
-        std::unordered_map<in6_addr, std::shared_ptr<VpnPeer>> _peers;
 };

VpnPacket.cpp

@@ -2,9 +2,10 @@
 #include "VpnPeer.hpp"
 
 #include <chrono>
+#include <cstring>
 
 const size_t VpnPacket::VPN_HEADER_BYTES = 8;
-uint32_t VpnPacket::_next_general_seqno = 0;
+const size_t VpnPacket::TLV_HEADER_BYTES = 3;
 
 static const size_t OUTER_HEADERS_BYTES =
     40 /* IPv6 header */ + 8 /* UDP header */;
@@ -15,8 +16,8 @@ static const int
     DATA_TIMESTAMP_POS = 4;
 
 VpnPacket::VpnPacket(size_t mtu)
-    : _peer(nullptr), _data_space(mtu-OUTER_HEADERS_BYTES), _data_size(0),
-      _reception_timestamp(0)
+    : _peer(nullptr), _data_space(mtu-OUTER_HEADERS_BYTES),
+      _data_size(VPN_HEADER_BYTES), _reception_timestamp(0)
 {
     _data = new char[mtu - OUTER_HEADERS_BYTES];
 }
@@ -25,8 +26,12 @@ VpnPacket::~VpnPacket() {
     delete[] _data;
 }
 
-bool VpnPacket::parse_as_ipv6() {
-    return parse_ipv6_header(get_payload(), get_payload_size(), _ipv6_header);
+VpnPacketTLV VpnPacket::first_tlv() {
+    return VpnPacketTLV(*this, 0);
+}
+
+size_t VpnPacket::get_tunnelled_mtu(size_t udp_mtu) {
+    return udp_mtu - OUTER_HEADERS_BYTES - VPN_HEADER_BYTES - TLV_HEADER_BYTES;
 }
 
 uint32_t VpnPacket::get_seqno() const {
@@ -51,7 +56,77 @@ void VpnPacket::upon_reception() {
 }
 
 uint32_t VpnPacket::next_seqno() {
-    if(_peer)
-        return _peer->next_seqno();
-    return _next_general_seqno++;
+    if(!_peer)
+        throw PeerNotSet();
+    return _peer->next_seqno();
+}
+
+
+VpnPacketTLV::VpnPacketTLV(VpnPacket& packet, size_t payload_offset)
+    : _packet(packet), _tlv_pos(payload_offset)
+{}
+
+VpnPacketTLV::VpnPacketTLV(const VpnPacketTLV& other) :
+    _packet(other._packet), _tlv_pos(other._tlv_pos)
+{}
+
+VpnPacketTLV VpnPacketTLV::create(
+        VpnPacket& packet, VpnPacket::PayloadType type)
+{
+    VpnPacketTLV tlv = VpnPacketTLV(packet, packet.get_payload_size());
+    packet.increase_payload_size(VpnPacket::TLV_HEADER_BYTES);
+
+    char* data = tlv.get_data();
+    data[0] = type;
+    *(uint16_t*)(data+1) = 0; // Set len to 0
+
+    return tlv;
+}
+
+VpnPacketTLV VpnPacketTLV::next_tlv() {
+    size_t next_offset = _tlv_pos + get_payload_size();
+    return VpnPacketTLV(_packet, next_offset);
+}
+
+void VpnPacketTLV::seek_next_tlv() {
+    _tlv_pos = _tlv_pos + VpnPacket::TLV_HEADER_BYTES + get_payload_size();
+}
+
+uint16_t VpnPacketTLV::get_payload_size() const {
+    return *(uint16_t*)(get_data() + 1);
+}
+void VpnPacketTLV::set_payload_size(uint16_t size) {
+    uint16_t* data_size_ptr = (uint16_t*)(get_data() + 1);
+    uint16_t old_size = *data_size_ptr;
+    *data_size_ptr = size;
+    _packet.increase_payload_size(size - old_size);
+}
+
+uint16_t VpnPacketTLV::get_payload_space() const {
+    return _packet.get_payload_space()
+        - _packet.get_payload_size()
+        + get_payload_size();
+}
+
+VpnPacket::PayloadType VpnPacketTLV::get_type() const {
+    return (VpnPacket::PayloadType)(*(uint8_t*)(get_data()));
+}
+void VpnPacketTLV::set_type(VpnPacket::PayloadType type) {
+    *(uint8_t*)(get_data()) = (uint8_t) type;
+}
+
+TunnelledPacket::TunnelledPacket(VpnPacket& packet, size_t payload_offset)
+    : VpnPacketTLV(packet, payload_offset)
+{}
+
+TunnelledPacket::TunnelledPacket(const VpnPacketTLV& copy)
+    : VpnPacketTLV(copy._packet, copy._tlv_pos)
+{}
+
+TunnelledPacket TunnelledPacket::create(VpnPacket& packet) {
+    return VpnPacketTLV::create(packet, VpnPacket::PAYLOAD_TYPE_TUNNELLED);
+}
+
+bool TunnelledPacket::parse_as_ipv6() {
+    return parse_ipv6_header(get_payload(), get_payload_size(), _ipv6_header);
 }

VpnPacket.hpp

@@ -9,49 +9,95 @@
 
 /** VPN packet layout:
  *
- * +-------+-------+-------+-------+-------+-------+-------+-------+
- * |  1B   |  1B   |  1B   |  1B   |  1B   |  1B   |  1B   |  1B   |
- * +-------+-------+-------+-------+-------+-------+-------+-------+
- * | Sequence number [4B]          | Sending timestamp (μs) [4B]   |
+ * 0                   1                   2                   3
+ * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ * +---------------+---------------+---------------+---------------+
+ * |                      Sequence number [4B]                     |
  * +---------------------------------------------------------------+
- * | Nested packet (payload)                                       |
+ * |                  Sending timestamp (μs) [4B]                  |
+ * +---------------+-----------------------------------------------+
+ * |   Type [1B]   |  First payload size (B) [2B]  |   Payload     |
+ * +---------------+-----------------------------------------------+
+ * |                          Payload (cont.)                      |
+ * +---------------+-----------------------------------------------+
+ * |   Type [1B]   | Second payload size (B) [2B]  |   Payload     |
+ * +---------------+-----------------------------------------------+
+ * |                          Payload (cont.)                      |
  * +---------------------------------------------------------------+
+ * |                              ...                              |
+ * +---------------------------------------------------------------+
+ *
+ * Where
+ * - Type is one of the values from PayloadType below;
+ * - Sender ID is an arbitrary value, recommended to be randomly chosen
+ * - Payload size is the size of the payload (excluding headers), in bytes;
+ * - Payload is an arbitrary value, defined by Type
  */
 
 class VpnPeer;
+class VpnPacketTLV;
 
 class VpnPacket {
     public:
         static const size_t VPN_HEADER_BYTES;
+        static const size_t TLV_HEADER_BYTES;
+
+        enum PayloadType {
+            PAYLOAD_TYPE_UNDEF,       ///< Undefined packet type
+            PAYLOAD_TYPE_TUNNELLED,   ///< A tunnelled packet
+            PAYLOAD_TYPE_RR,          ///< Receiver report
+            PAYLOAD_TYPE_REMB,        ///< Receiver Estimated Maximum Bitrate
+        };
+
+        class PeerNotSet: public std::exception {};
 
         VpnPacket(size_t mtu);
         ~VpnPacket();
 
+        VpnPacketTLV first_tlv();
+
+        /// Get the maximal payload space for a given tunnel MTU
+        static size_t get_tunnelled_mtu(size_t udp_mtu);
+
         /// Set packet peer -- used for sequence numbers
-        void set_peer(std::shared_ptr<VpnPeer> peer) { _peer = peer; }
-
-        /// Try to parse the packet as IPv6, return `false` upon failure.
-        bool parse_as_ipv6();
-        bool ipv6_parsed() const { return _ipv6_parsed; }
-        const IPv6Header& get_ipv6_header() const { return _ipv6_header; }
+        void set_peer(VpnPeer* peer) { _peer = peer; }
 
+        /// Get a pointer to the packet payload (const version)
         const char* get_payload() const { return _data + VPN_HEADER_BYTES; }
+        /// Get a pointer to the packet payload
         char* get_payload() { return _data + VPN_HEADER_BYTES; }
+        /// Get a pointer to the first free byte of the packet payload
+        char* get_next_payload() { return get_payload() + get_payload_size(); }
+        /// Get the space allocated for the packet payload
         size_t get_payload_space() const {
             return _data_space - VPN_HEADER_BYTES; }
+        /// Get the current size filled by the payload
         size_t get_payload_size() const {
             return _data_size - VPN_HEADER_BYTES; }
+        /// Set current size filled by the packet payload
         void set_payload_size(size_t payload_size) {
             _data_size = payload_size + VPN_HEADER_BYTES; }
+        /// Increase the current size filled by the packet payload (size may be
+        /// negative).
+        void increase_payload_size(ssize_t payload_size_increment) {
+            _data_size += payload_size_increment; }
 
+        /// Get a pointer to the full packet data (const version)
         const char* get_data() const { return _data; }
+        /// Get a pointer to the full packet data
         char* get_data() { return _data; }
+        /// Get the space allocated for the packet
         size_t get_data_space() const { return _data_space; }
+        /// Get the total current size of the packet
         size_t get_data_size() const { return _data_size; }
+        /// Set the total current size of the packet
         void set_data_size(size_t data_size) { _data_size = data_size; }
 
+        /// Get this packet's seqno
         uint32_t get_seqno() const;
+        /// Get this packet's sending timestamp
         uint32_t get_sending_timestamp() const;
+        /// Get this packet's reception timestamp
         uint32_t get_reception_timestamp() const { return _reception_timestamp; }
 
         /** Fill the headers of the packet. This method must be called as close
@@ -69,15 +115,96 @@ class VpnPacket {
         inline uint32_t next_seqno();
 
     private:
-        std::shared_ptr<VpnPeer> _peer;
+        VpnPeer* _peer; // raw pointer: we do not own the peer in any way
 
         char* _data;
         size_t _data_space, _data_size;
 
+        uint32_t _reception_timestamp;
+};
+
+/** Base class for a TLV contained in a VpnPacket */
+class VpnPacketTLV {
+    public:
+        VpnPacketTLV(VpnPacket& packet, size_t payload_offset);
+        VpnPacketTLV(const VpnPacketTLV& other);
+
+        static VpnPacketTLV create(
+                VpnPacket& packet,
+                VpnPacket::PayloadType type=VpnPacket::PAYLOAD_TYPE_UNDEF);
+
+        const VpnPacket& get_packet() const { return _packet; }
+        VpnPacket& get_packet() { return _packet; }
+
+        /// Get the next TLV in this packet.
+        VpnPacketTLV next_tlv();
+        /// Point this object to the next TLV (ie. `next_tlv` in place)
+        void seek_next_tlv();
+
+        /// Check whether the current TLV is past the packet's end
+        bool past_the_end() const {
+            return _tlv_pos >= _packet.get_payload_size(); }
+
+        /// Get the offset in the packet
+        size_t get_offset() const { return _tlv_pos; }
+
+        /// Get a pointer to the payload (const version)
+        const char* get_payload() const {
+            return _packet.get_payload() + _tlv_pos + VpnPacket::TLV_HEADER_BYTES; }
+        /// Get a pointer to the payload
+        char* get_payload() {
+            return _packet.get_payload() + _tlv_pos + VpnPacket::TLV_HEADER_BYTES; }
+        /// Get the current payload size
+        uint16_t get_payload_size() const;
+        /// Set the current payload size
+        void set_payload_size(uint16_t size);
+        /// Get the total available raw data space
+        uint16_t get_payload_space() const;
+
+        /// Get a pointer to the raw data (const version)
+        const char* get_data() const {
+            return _packet.get_payload() + _tlv_pos; }
+        /// Get a pointer to the raw data
+        char* get_data() { return _packet.get_payload() + _tlv_pos; }
+        /// Get the current raw data size
+        uint16_t get_data_size() const {
+            return get_payload_size() + VpnPacket::TLV_HEADER_BYTES; }
+        /// Set the current raw data size
+        void set_data_size(uint16_t size) {
+            set_payload_size(size + VpnPacket::TLV_HEADER_BYTES); }
+
+        /// Get this TLV's type
+        VpnPacket::PayloadType get_type() const;
+
+        bool operator==(const VpnPacketTLV& other) const {
+            return &_packet == &other._packet && _tlv_pos == other._tlv_pos; }
+
+    protected: // meth
+        /// Set this TLV's type
+        void set_type(VpnPacket::PayloadType type);
+
+    protected:
+        VpnPacket& _packet;
+        size_t _tlv_pos;
+
+        friend class TunnelledPacket;
+};
+
+/** A packet sent through the VPN tunnel.
+ *
+ * This must instantiated just before filling it with data. */
+class TunnelledPacket: public VpnPacketTLV {
+    public:
+        TunnelledPacket(VpnPacket& packet, size_t payload_offset);
+        TunnelledPacket(const VpnPacketTLV& copy);
+        static TunnelledPacket create(VpnPacket& packet);
+
+        /// Try to parse the packet as IPv6, return `false` upon failure.
+        bool parse_as_ipv6();
+        bool ipv6_parsed() const { return _ipv6_parsed; }
+        const IPv6Header& get_ipv6_header() const { return _ipv6_header; }
+
+    private:
         bool _ipv6_parsed;
         IPv6Header _ipv6_header;
-
-        uint32_t _reception_timestamp;
-
-        static uint32_t _next_general_seqno;
 };

VpnPeer.cpp

@@ -6,10 +6,13 @@
 #include <functional>
 
 VpnPeer::VpnPeer(UdpVpn* vpn, const sockaddr_in6& ext_addr,
-        const in6_addr int_addr)
-    : _vpn(vpn), _ext_addr(ext_addr), _int_addr(int_addr), _next_seqno(0)
+        const in6_addr& int_addr)
+    : _vpn(vpn), _ext_addr(ext_addr), _int_addr(int_addr), _next_send_seqno(0)
 {}
 
+void VpnPeer::set_int_addr(const in6_addr& int_addr) {
+    memcpy(&_int_addr, &int_addr, sizeof(_int_addr));
+}
 
 size_t VpnPeer::write(const char* data, size_t len) {
     ssize_t nsent;
@@ -25,3 +28,43 @@ size_t VpnPeer::write(const char* data, size_t len) {
 size_t VpnPeer::write(const VpnPacket& packet) {
     return write(packet.get_data(), packet.get_data_size());
 }
+
+PacketLossLogger::PacketLossLogger() : _cur_seqno(0) {}
+
+void PacketLossLogger::log_packet(uint32_t seqno) {
+    uint32_t m_seqno = seqno % PACKET_LOSS_HISTSIZE;
+    int64_t diff = (int64_t)seqno - _cur_seqno;
+
+    if(diff == 1) {
+        _cur_seqno++;
+        _packet_loss_hist.reset(m_seqno);
+        while(_received_ahead.test((_cur_seqno + 1) % PACKET_LOST_AFTER)) {
+            _cur_seqno++;
+            _packet_loss_hist.reset(_cur_seqno % PACKET_LOSS_HISTSIZE);
+            _received_ahead.reset(_cur_seqno % PACKET_LOST_AFTER);
+        }
+    } else if(LIKELY(diff > 1)) {
+        if(diff < PACKET_LOST_AFTER)
+            _received_ahead.set(seqno % PACKET_LOST_AFTER);
+        else if(diff < PACKET_LOSS_HISTSIZE) {
+            // Packet too much forwards -- consider _cur_seqno lost
+            for(int offs=1; offs < PACKET_LOST_AFTER; ++offs) {
+                _packet_loss_hist[(_cur_seqno + offs) % PACKET_LOSS_HISTSIZE] =
+                    !_received_ahead[(_cur_seqno + offs) % PACKET_LOST_AFTER];
+            }
+            _received_ahead.reset();
+            _cur_seqno = seqno;
+            _packet_loss_hist.reset(m_seqno);
+        } else
+            reboot(); // This is a huge gap -- reboot
+    } else {
+        if(diff < - 2*PACKET_LOSS_HISTSIZE)
+            reboot(); // this is too much backwards -- something's wrong, reboot
+        // else: ignore, we've moved forward and counted the packet as lost
+    }
+}
+
+void PacketLossLogger::reboot() {
+    _packet_loss_hist.reset();
+    _received_ahead.reset();
+}

VpnPeer.hpp

@@ -3,11 +3,40 @@
 /** A peer of a bound (server) instance of UdpVpn */
 
 #include <netinet/in.h>
+#include <bitset>
 #include "util.hpp"
 #include "VpnPacket.hpp"
 
 class UdpVpn;
 
+const int PACKET_LOSS_HISTSIZE = 128, PACKET_LOST_AFTER = 8;
+
+class PacketLossLogger {
+    public:
+        PacketLossLogger();
+        void log_packet(uint32_t seqno);
+        double get_loss_rate() const {
+            return (double)_packet_loss_hist.count() / PACKET_LOSS_HISTSIZE;
+        }
+
+        const std::bitset<PACKET_LOSS_HISTSIZE> get_loss_hist() const {
+            return _packet_loss_hist;
+        }
+
+        const std::bitset<PACKET_LOST_AFTER> get_received_ahead() const {
+            return _received_ahead;
+        }
+
+        uint32_t get_cur_seqno() const { return _cur_seqno; }
+
+    private:
+        void reboot(); ///< completely reset the internal state
+
+        std::bitset<PACKET_LOSS_HISTSIZE> _packet_loss_hist;
+        std::bitset<PACKET_LOST_AFTER> _received_ahead;
+        uint32_t _cur_seqno;
+};
+
 class VpnPeer {
     public:
         class NetError : public MsgException {
@@ -20,20 +49,22 @@ class VpnPeer {
         };
 
         VpnPeer(UdpVpn* vpn, const sockaddr_in6& ext_addr,
-                const in6_addr int_addr);
+                const in6_addr& int_addr);
 
         const sockaddr_in6& get_ext_addr() const { return _ext_addr; }
         const in6_addr& get_int_addr() const { return _int_addr; }
 
+        void set_int_addr(const in6_addr& int_addr);
+
         size_t write(const char* data, size_t len);
         size_t write(const VpnPacket& packet);
 
-        uint32_t peek_next_seqno() const { return _next_seqno; }
-        uint32_t next_seqno() { return _next_seqno++; }
+        uint32_t peek_next_seqno() const { return _next_send_seqno; }
+        uint32_t next_seqno() { return _next_send_seqno++; }
 
     private:
         UdpVpn* _vpn;
         sockaddr_in6 _ext_addr;
         in6_addr _int_addr;
-        uint32_t _next_seqno;
+        uint32_t _next_send_seqno;
 };

ip_header.cpp

@@ -15,5 +15,6 @@ bool parse_ipv6_header(const char* data, size_t len, IPv6Header& header) {
 
     memcpy(&(header.source), data + 8, 16);
     memcpy(&(header.dest), data + 24, 16);
+    header.payload_length = ntohs(*(uint16_t*)(data + 4));
     return true;
 }

ip_header.hpp

@@ -5,6 +5,9 @@
 struct IPv6Header {
     in6_addr source;
     in6_addr dest;
+    uint16_t payload_length;
+
+    uint16_t packet_length() const { return payload_length + 40; }
 };
 
 /** Parse an IPv6 header, filling `header`. Returns `true` on success. */

tests/test_packetloss.cpp

@@ -0,0 +1,48 @@
+#include <cstdio>
+#include <vector>
+#include "VpnPeer.hpp"
+
+template<int len>
+std::string bitset_to_string(
+        const std::bitset<len>& bs,
+        char c_f, char c_t, size_t mark_pos, char cm_f, char cm_t)
+{
+    std::string out;
+    for(size_t pos=0; pos < len; ++pos) {
+        if(pos == mark_pos)
+            out += bs[pos] ? cm_t : cm_f;
+        else
+            out += bs[pos] ? c_t : c_f;
+    }
+    return out;
+}
+
+void dump(PacketLossLogger& pllog, int received) {
+    printf("%03d, %.03lf ## %s ## %s\n",
+            received,
+            pllog.get_loss_rate(),
+            bitset_to_string<PACKET_LOSS_HISTSIZE>(
+                pllog.get_loss_hist(), '_', 'X',
+                pllog.get_cur_seqno() % PACKET_LOSS_HISTSIZE,
+                '|', '#').c_str(),
+            bitset_to_string<PACKET_LOST_AFTER>(
+                pllog.get_received_ahead(), '_', 'X',
+                pllog.get_cur_seqno() % PACKET_LOST_AFTER,
+                '|', '#').c_str());
+}
+
+int main(void) {
+    PacketLossLogger pllog;
+    std::vector<int> sequence({
+        1, 2, 3, 5, 6, 4, 8, 7, 8, 9,
+        12, 13, 14, 15, 16, 17, 18, 19, 20, 10, 21
+    });
+
+    dump(pllog, 0);
+    for(auto val=sequence.begin(); val != sequence.end(); ++val) {
+        pllog.log_packet(*val);
+        dump(pllog, *val);
+    }
+
+    return 0;
+}