From f07f2a853d24647fd755ec1d3501df5a92d5f573 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Th=C3=A9ophile=20Bastian?= <contact@tobast.fr>
Date: Tue, 23 Jun 2020 15:36:49 +0200
Subject: [PATCH] Add PacketLossManager

---
 VpnPeer.cpp               | 42 +++++++++++++++++++++++++++++++++-
 VpnPeer.hpp               | 35 +++++++++++++++++++++++++---
 tests/test_packetloss.cpp | 48 +++++++++++++++++++++++++++++++++++++++
 3 files changed, 121 insertions(+), 4 deletions(-)
 create mode 100644 tests/test_packetloss.cpp

diff --git a/VpnPeer.cpp b/VpnPeer.cpp
index b4c01e5..68e73a7 100644
--- a/VpnPeer.cpp
+++ b/VpnPeer.cpp
@@ -7,7 +7,7 @@
 
 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)
+    : _vpn(vpn), _ext_addr(ext_addr), _int_addr(int_addr), _next_send_seqno(0)
 {}
 
 void VpnPeer::set_int_addr(const in6_addr& int_addr) {
@@ -28,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();
+}
diff --git a/VpnPeer.hpp b/VpnPeer.hpp
index 66fafd1..7f836f3 100644
--- a/VpnPeer.hpp
+++ b/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 {
@@ -30,12 +59,12 @@ class VpnPeer {
         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;
 };
diff --git a/tests/test_packetloss.cpp b/tests/test_packetloss.cpp
new file mode 100644
index 0000000..dc11290
--- /dev/null
+++ b/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;
+}