From 236b03c9b1f3630603455cc5e3c77026d6a488f6 Mon Sep 17 00:00:00 2001
From: Stephen Seo <seo.disparate@gmail.com>
Date: Tue, 27 Aug 2019 13:03:38 +0900
Subject: [PATCH] Impl storing received packet

---
 cpp_impl/src/TSQueue.hpp       |  8 ++++
 cpp_impl/src/UDPC_Defines.hpp  |  1 +
 cpp_impl/src/UDPConnection.cpp | 69 +++++++++++++++++++++++++++++++++-
 3 files changed, 77 insertions(+), 1 deletion(-)

diff --git a/cpp_impl/src/TSQueue.hpp b/cpp_impl/src/TSQueue.hpp
index 8b7d65c..947f76c 100644
--- a/cpp_impl/src/TSQueue.hpp
+++ b/cpp_impl/src/TSQueue.hpp
@@ -28,6 +28,7 @@ class TSQueue {
     void clear();
     void changeCapacity(unsigned int newCapacity);
     unsigned int size();
+    unsigned int capacity();
     bool empty();
 
   private:
@@ -114,6 +115,13 @@ unsigned int TSQueue<T>::size() {
     return size;
 }
 
+template <typename T>
+unsigned int TSQueue<T>::capacity() {
+    std::lock_guard<std::mutex> lock(mutex);
+    unsigned int capacity = rb.getCapacity();
+    return capacity;
+}
+
 template <typename T>
 bool TSQueue<T>::empty() {
     // No lock required, since this is calling size() that uses a lock
diff --git a/cpp_impl/src/UDPC_Defines.hpp b/cpp_impl/src/UDPC_Defines.hpp
index a59617d..52ffb44 100644
--- a/cpp_impl/src/UDPC_Defines.hpp
+++ b/cpp_impl/src/UDPC_Defines.hpp
@@ -82,6 +82,7 @@ struct ConnectionData {
     std::deque<UDPC_PacketInfo> sentPkts;
     TSQueue<UDPC_PacketInfo> sendPkts;
     TSQueue<UDPC_PacketInfo> priorityPkts;
+    TSQueue<UDPC_PacketInfo> receivedPkts;
     // pkt id to pkt shared_ptr
     std::unordered_map<uint32_t, SentPktInfo::Ptr> sentInfoMap;
     std::chrono::steady_clock::time_point received;
diff --git a/cpp_impl/src/UDPConnection.cpp b/cpp_impl/src/UDPConnection.cpp
index 64961cf..dddd708 100644
--- a/cpp_impl/src/UDPConnection.cpp
+++ b/cpp_impl/src/UDPConnection.cpp
@@ -627,7 +627,74 @@ void UDPC_update(void *ctx) {
         --rseq;
     }
 
-    // TODO impl
+    // calculate sequence and ack
+    bool isOutOfOrder = false;
+    uint32_t diff = 0;
+    if(seqID > iter->second.rseq) {
+        diff = seqID - iter->second.rseq;
+        if(diff <= 0x7FFFFFFF) {
+            // sequence is more recent
+            iter->second.rseq = seqID;
+            iter->second.ack = (iter->second.ack >> diff) | 0x80000000;
+        } else {
+            // sequence is older, recalc diff
+            diff = 0xFFFFFFFF - seqID + 1 + iter->second.rseq;
+            if((iter->second.ack & (0x80000000 >> (diff - 1))) != 0) {
+                // already received packet
+                // TODO log that already received packet is ignored
+                return;
+            }
+            iter->second.ack |= 0x80000000 >> (diff - 1);
+            isOutOfOrder = true;
+        }
+    } else if(seqID < iter->second.rseq) {
+        diff = iter->second.rseq - seqID;
+        if(diff <= 0x7FFFFFFF) {
+            // sequence is older
+            if((iter->second.ack & (0x80000000 >> (diff - 1))) != 0) {
+                // already received packet
+                // TODO log that already received packet is ignored
+                return;
+            }
+            iter->second.ack |= 0x80000000 >> (diff - 1);
+            isOutOfOrder = true;
+        } else {
+            // sequence is more recent, recalc diff
+            diff = 0xFFFFFFFF - iter->second.rseq + 1 + seqID;
+            iter->second.rseq = seqID;
+            iter->second.ack = (iter->second.ack >> diff) | 0x80000000;
+        }
+    } else {
+        // already received packet
+        // TODO log that already received packet is ignored
+        return;
+    }
+
+    // TODO log that received packet is out of order
+
+    if(bytes > 20) {
+        UDPC_PacketInfo recPktInfo;
+        std::memcpy(recPktInfo.data, c->recvBuf, bytes);
+        recPktInfo.dataSize = bytes;
+        recPktInfo.flags =
+            (isConnect ? 0x1 : 0)
+            | (isPing ? 0x2 : 0)
+            | (isNotRecChecked ? 0x4 : 0)
+            | (isResending ? 0x8 : 0);
+        recPktInfo.sender = receivedData.sin_addr.s_addr;
+        recPktInfo.receiver = UDPC::LOCAL_ADDR;
+        recPktInfo.senderPort = receivedData.sin_port;
+        recPktInfo.receiverPort = c->socketInfo.sin_port;
+
+        if(iter->second.receivedPkts.size() == iter->second.receivedPkts.capacity()) {
+            // TODO log that receivedPkts is full, so removed an entry for a new one
+            iter->second.receivedPkts.pop();
+        }
+
+        iter->second.receivedPkts.push(recPktInfo);
+    } else if(bytes == 20) {
+        // TODO log that packet had no payload
+    }
 }
 
 int UDPC_get_queue_send_available(void *ctx, uint32_t addr) {