From 6cf2bd7bc3f5d5cc92dba3c99a05f170708aed32 Mon Sep 17 00:00:00 2001
From: Pedro Alvarez <pedro.alvarez@softwareradiosystems.com>
Date: Tue, 1 Oct 2019 18:40:23 +0100
Subject: [PATCH] Starting to add reordering queue in the PDCP.

---
 lib/include/srslte/upper/pdcp_entity_nr.h |  4 +++
 lib/src/upper/pdcp_entity_nr.cc           | 41 +++++++++++++++--------
 2 files changed, 31 insertions(+), 14 deletions(-)

diff --git a/lib/include/srslte/upper/pdcp_entity_nr.h b/lib/include/srslte/upper/pdcp_entity_nr.h
index 743c8da4b..509762a43 100644
--- a/lib/include/srslte/upper/pdcp_entity_nr.h
+++ b/lib/include/srslte/upper/pdcp_entity_nr.h
@@ -29,6 +29,7 @@
 #include "srslte/common/log.h"
 #include "srslte/common/security.h"
 #include "srslte/common/threads.h"
+#include <map>
 
 namespace srslte {
 
@@ -65,6 +66,9 @@ private:
   srsue::rrc_interface_pdcp* rrc = nullptr;
   srsue::gw_interface_pdcp*  gw  = nullptr;
 
+  // Reordering Queue
+  std::map<uint32_t, unique_byte_buffer_t> reorder_queue;
+
   // State variables: 3GPP TS 38.323 v15.2.0, section 7.1
   uint32_t tx_next  = 0; // COUNT value of next SDU to be transmitted.
   uint32_t rx_next  = 0; // COUNT value of next SDU expected to be received.
diff --git a/lib/src/upper/pdcp_entity_nr.cc b/lib/src/upper/pdcp_entity_nr.cc
index 41e14a2bd..865b32bf7 100644
--- a/lib/src/upper/pdcp_entity_nr.cc
+++ b/lib/src/upper/pdcp_entity_nr.cc
@@ -149,8 +149,13 @@ void pdcp_entity_nr::write_pdu(unique_byte_buffer_t pdu)
     return; // Invalid count, drop.
   }
 
-  // TODO check if PDU has been received
-  // TODO Store PDU in reception buffer
+  // Check if PDU has been received
+  if (reorder_queue.find(rcvd_count) != reorder_queue.end()) {
+    return; // PDU already present, drop. 
+  }
+
+  // Store PDU in reception buffer
+  reorder_queue[rcvd_count] = std::move(pdu);
 
   // Update RX_NEXT
   if (rcvd_count >= rx_next) {
@@ -160,20 +165,29 @@ void pdcp_entity_nr::write_pdu(unique_byte_buffer_t pdu)
   // TODO if out-of-order configured, submit to upper layer
 
   if (rcvd_count == rx_deliv) {
-    // Deliver to upper layers (TODO queueing needs to be implemented)
-    if (is_srb()) {
-      rrc->write_pdu(lcid, std::move(pdu));
-    } else {
-      gw->write_pdu(lcid, std::move(pdu));
+    // Deliver to upper layers in ascending order of associeted COUNT
+    for (std::map<uint32_t, unique_byte_buffer_t>::iterator it = reorder_queue.begin();
+         it != reorder_queue.end();) {
+      if (it->first == rx_deliv) {
+        // Pass to upper layers
+        if (is_srb()) {
+          rrc->write_pdu(lcid, std::move(it->second));
+        } else {
+          gw->write_pdu(lcid, std::move(it->second));
+        }
+
+        // Remove from queue
+        reorder_queue.erase(it++);
+
+        // Update RX_DELIV
+        rx_deliv = rx_deliv + 1; // TODO needs to be corrected when queueing is implemented
+        printf("New RX_deliv %d, rcvd_count %d\n", rx_deliv, rcvd_count);
+      } else {
+        break;
+      }
     }
-
-    // Update RX_DELIV
-    rx_deliv = rcvd_count + 1; // TODO needs to be corrected when queueing is implemented
-    printf("New RX_deliv %d, rcvd_count %d\n", rx_deliv, rcvd_count);
   }
 
-  // Not clear how to update RX_DELIV without reception buffer (TODO)
-
   // TODO handle reordering timers
 }
 
@@ -197,7 +211,6 @@ uint32_t pdcp_entity_nr::read_data_header(const unique_byte_buffer_t& pdu)
       srslte::uint8_to_uint24(pdu->msg, &rcvd_sn_32);
       rcvd_sn_32 = SN(rcvd_sn_32);
       break;
-      break;
     default:
       log->error("Cannot extract RCVD_SN, invalid SN length configured: %d\n", cfg.sn_len);
   }