From cf696138d3347dea8731ee389239ffe8d059d2ed Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Sat, 10 Apr 2021 12:28:34 +0200
Subject: [PATCH] mac_nr: fix packing order of SDU and CEs

SDUs need to go first in the MAC PDU, followed by CEs
---
 srsue/hdr/stack/mac_nr/mux_nr.h            |  4 +-
 srsue/src/stack/mac_nr/mux_nr.cc           | 53 +++++++++++++++-------
 srsue/src/stack/mac_nr/test/mac_nr_test.cc |  6 +--
 3 files changed, 41 insertions(+), 22 deletions(-)

diff --git a/srsue/hdr/stack/mac_nr/mux_nr.h b/srsue/hdr/stack/mac_nr/mux_nr.h
index d3cd2bd33..18e5b3a6c 100644
--- a/srsue/hdr/stack/mac_nr/mux_nr.h
+++ b/srsue/hdr/stack/mac_nr/mux_nr.h
@@ -62,12 +62,12 @@ private:
   typedef enum { none, pending, transmitted } msg3_state_t;
   msg3_state_t msg3_state = none;
 
-  static constexpr uint32_t MIN_RLC_PDU_LEN =
+  static constexpr int32_t MIN_RLC_PDU_LEN =
       5; ///< minimum bytes that need to be available in a MAC PDU for attempting to add another RLC SDU
 
   srsran::unique_byte_buffer_t rlc_buff = nullptr;
 
-  srsran::mac_sch_pdu_nr tx_pdu;
+  srsran::mac_sch_pdu_nr tx_pdu; /// single MAC PDU for packing
 
   enum { no_bsr, sbsr_ce, lbsr_ce } add_bsr_ce = no_bsr; /// BSR procedure requests MUX to add a BSR CE
 
diff --git a/srsue/src/stack/mac_nr/mux_nr.cc b/srsue/src/stack/mac_nr/mux_nr.cc
index f949d960b..d0eff00ae 100644
--- a/srsue/src/stack/mac_nr/mux_nr.cc
+++ b/srsue/src/stack/mac_nr/mux_nr.cc
@@ -70,40 +70,59 @@ srsran::unique_byte_buffer_t mux_nr::get_pdu(uint32_t max_pdu_len)
     msg3_transmitted();
   } else {
     // Pack normal UL data PDU
+    int32_t remaining_len = tx_pdu.get_remaing_len(); // local variable to reserv space for CEs
+
     if (add_bsr_ce == sbsr_ce) {
-      tx_pdu.add_sbsr_ce(mac.generate_sbsr());
-      add_bsr_ce = no_bsr;
+      // reserve space for SBSR
+      remaining_len -= 2;
     }
 
-    // TODO: Add proper priority handling
+    // First add MAC SDUs
     for (const auto& lc : logical_channels) {
-      while (tx_pdu.get_remaing_len() >= MIN_RLC_PDU_LEN) {
+      // TODO: Add proper priority handling
+      logger.debug("Adding SDUs for LCID=%d (max %d B)", lc.lcid, remaining_len);
+      while (remaining_len >= MIN_RLC_PDU_LEN) {
         // read RLC PDU
         rlc_buff->clear();
         uint8_t* rd = rlc_buff->msg;
 
         // Determine space for RLC
-        uint32_t rlc_opportunity = tx_pdu.get_remaing_len();
-        rlc_opportunity -= tx_pdu.get_remaing_len() >= srsran::mac_sch_subpdu_nr::MAC_SUBHEADER_LEN_THRESHOLD ? 3 : 2;
+        remaining_len -= remaining_len >= srsran::mac_sch_subpdu_nr::MAC_SUBHEADER_LEN_THRESHOLD ? 3 : 2;
 
         // Read PDU from RLC
-        int pdu_len = rlc->read_pdu(lc.lcid, rd, rlc_opportunity);
-
-        // Add SDU if RLC has something to tx
-        if (pdu_len > 0) {
-          rlc_buff->N_bytes = pdu_len;
-          logger.debug(rlc_buff->msg, rlc_buff->N_bytes, "Read %d B from RLC", rlc_buff->N_bytes);
+        int pdu_len = rlc->read_pdu(lc.lcid, rd, remaining_len);
 
-          // add to MAC PDU and pack
-          if (tx_pdu.add_sdu(lc.lcid, rlc_buff->msg, rlc_buff->N_bytes) != SRSRAN_SUCCESS) {
-            logger.error("Error packing MAC PDU");
+        if (pdu_len > remaining_len) {
+          logger.error("Can't add SDU of %d B. Available space %d B", pdu_len, remaining_len);
+          break;
+        } else {
+          // Add SDU if RLC has something to tx
+          if (pdu_len > 0) {
+            rlc_buff->N_bytes = pdu_len;
+            logger.debug(rlc_buff->msg, rlc_buff->N_bytes, "Read %d B from RLC", rlc_buff->N_bytes);
+
+            // add to MAC PDU and pack
+            if (tx_pdu.add_sdu(lc.lcid, rlc_buff->msg, rlc_buff->N_bytes) != SRSRAN_SUCCESS) {
+              logger.error("Error packing MAC PDU");
+              break;
+            }
+          } else {
             break;
           }
-        } else {
-          break;
+
+          remaining_len -= pdu_len;
+          logger.debug("%d B remaining PDU", remaining_len);
         }
       }
     }
+
+    // Second add fixed-sized MAC CEs (e.g. SBSR)
+    if (add_bsr_ce == sbsr_ce) {
+      tx_pdu.add_sbsr_ce(mac.generate_sbsr());
+      add_bsr_ce = no_bsr;
+    }
+
+    // Lastly, add variable-sized MAC CEs
   }
 
   // Pack PDU
diff --git a/srsue/src/stack/mac_nr/test/mac_nr_test.cc b/srsue/src/stack/mac_nr/test/mac_nr_test.cc
index ea1da823f..eeb1b8860 100644
--- a/srsue/src/stack/mac_nr/test/mac_nr_test.cc
+++ b/srsue/src/stack/mac_nr/test/mac_nr_test.cc
@@ -325,9 +325,9 @@ int mac_nr_ul_logical_channel_prioritization_test2()
 int mac_nr_ul_periodic_bsr_test()
 {
   // PDU layout (10 B in total)
-  // - SBSR
-  // - 6B LCID=4
-  const uint8_t tv1[] = {0x3d, 0xd1, 0x04, 0x06, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04};
+  // - 6B LCID=4 (+2B header, 8 B total)
+  // - 2B SBSR
+  const uint8_t tv1[] = {0x04, 0x06, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x3d, 0xd1};
 
   // PDU layout (10 B in total)
   // - 8B LCID=4