From ea2e692836238e98fb75408dfc9efdc88e517b58 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Wed, 29 May 2019 21:41:18 +0200
Subject: [PATCH] pdu: fix MAC RAR PDU packing and unpacking with backoff
 indicator

---
 lib/include/srslte/common/pdu.h |  8 ++++--
 lib/src/common/pdu.cc           | 45 ++++++++++++++++++++-------------
 2 files changed, 33 insertions(+), 20 deletions(-)

diff --git a/lib/include/srslte/common/pdu.h b/lib/include/srslte/common/pdu.h
index 6ea7cdad6..1dc514c35 100644
--- a/lib/include/srslte/common/pdu.h
+++ b/lib/include/srslte/common/pdu.h
@@ -338,12 +338,15 @@ public:
 class rar_subh : public subh<rar_subh>
 {
 public:
+  typedef enum { BACKOFF = 0, RAPID } rar_subh_type_t;
+
   rar_subh() {
     bzero(&grant, sizeof(grant));
     ta        = 0;
     temp_rnti = 0;
     preamble  = 0;
     parent    = NULL;
+    type      = BACKOFF;
   }
 
   static const uint32_t RAR_GRANT_LEN = 20; 
@@ -351,6 +354,7 @@ public:
   // Reading functions
   bool     read_subheader(uint8_t** ptr);
   void     read_payload(uint8_t** ptr);
+  bool     has_rapid();
   uint32_t get_rapid();
   uint32_t get_ta_cmd();
   uint16_t get_temp_crnti();
@@ -372,6 +376,7 @@ private:
   uint32_t ta; 
   uint16_t temp_rnti; 
   uint32_t preamble;
+  rar_subh_type_t type;
 };
 
 class rar_pdu : public pdu<rar_subh>
@@ -389,8 +394,7 @@ public:
 
 private:
   bool       has_backoff_indicator;
-  uint8_t    backoff_indicator; 
-
+  uint8_t    backoff_indicator;
 };
 
 class mch_subh : public sch_subh
diff --git a/lib/src/common/pdu.cc b/lib/src/common/pdu.cc
index b0fde7dc3..b3a4d1cf7 100644
--- a/lib/src/common/pdu.cc
+++ b/lib/src/common/pdu.cc
@@ -826,15 +826,12 @@ uint8_t sch_subh::phr_report_table(float phr_value)
 void rar_pdu::fprint(FILE* stream)
 {
   fprintf(stream, "MAC PDU for RAR. ");
-  if (has_backoff_indicator) {
-    fprintf(stream, "Backoff Indicator %d. ", backoff_indicator);
-  }
   pdu::fprint(stream);  
 } 
 
 rar_pdu::rar_pdu(uint32_t max_rars_) : pdu(max_rars_)
 {
-  backoff_indicator = 0; 
+  backoff_indicator     = 0;
   has_backoff_indicator = false; 
 }
 
@@ -861,7 +858,7 @@ bool rar_pdu::write_packet(uint8_t* ptr)
   if (has_backoff_indicator) {
     *(ptr) = backoff_indicator&0xf;
     if (nof_subheaders > 0) {
-      *(ptr) = 1<<7;
+      *(ptr) |= 1 << 7;
     }
     ptr++;
   }
@@ -882,7 +879,12 @@ bool rar_pdu::write_packet(uint8_t* ptr)
 
 void rar_subh::fprint(FILE* stream)
 {
-  fprintf(stream, "RAPID: %d, Temp C-RNTI: %d, TA: %d, UL Grant: ", preamble, temp_rnti, ta);
+  if (type == RAPID) {
+    fprintf(stream, "RAPID: %d, Temp C-RNTI: %d, TA: %d, UL Grant: ", preamble, temp_rnti, ta);
+  } else {
+    fprintf(stream, "Backoff Indicator %d. ", ((rar_pdu*)parent)->get_backoff());
+  }
+
   srslte_vec_fprint_hex(stream, grant, 20);
 }
 
@@ -898,6 +900,11 @@ uint32_t rar_subh::get_rapid()
   return preamble; 
 }
 
+bool rar_subh::has_rapid()
+{
+  return (type == RAPID);
+}
+
 void rar_subh::get_sched_grant(uint8_t grant_[RAR_GRANT_LEN])
 {
   memcpy(grant_, grant, sizeof(uint8_t)*RAR_GRANT_LEN);
@@ -954,23 +961,25 @@ void rar_subh::write_payload(uint8_t** ptr)
 
 void rar_subh::read_payload(uint8_t** ptr)
 {
-  ta = ((uint32_t) *(*ptr + 0)&0x7f)<<4 | (*(*ptr + 1)&0xf0)>>4;
-  grant[0] = *(*ptr + 1)&0x8?1:0;
-  grant[1] = *(*ptr + 1)&0x4?1:0;
-  grant[2] = *(*ptr + 1)&0x2?1:0;
-  grant[3] = *(*ptr + 1)&0x1?1:0;
-  uint8_t *x = &grant[4];
-  srslte_bit_unpack(*(*ptr+2), &x, 8);
-  srslte_bit_unpack(*(*ptr+3), &x, 8);
-  temp_rnti = ((uint16_t) *(*ptr + 4))<<8 | *(*ptr + 5);    
-  *ptr += 6;
+  if (type == RAPID) {
+    ta         = ((uint32_t) * (*ptr + 0) & 0x7f) << 4 | (*(*ptr + 1) & 0xf0) >> 4;
+    grant[0]   = *(*ptr + 1) & 0x8 ? 1 : 0;
+    grant[1]   = *(*ptr + 1) & 0x4 ? 1 : 0;
+    grant[2]   = *(*ptr + 1) & 0x2 ? 1 : 0;
+    grant[3]   = *(*ptr + 1) & 0x1 ? 1 : 0;
+    uint8_t* x = &grant[4];
+    srslte_bit_unpack(*(*ptr + 2), &x, 8);
+    srslte_bit_unpack(*(*ptr + 3), &x, 8);
+    temp_rnti = ((uint16_t) * (*ptr + 4)) << 8 | *(*ptr + 5);
+    *ptr += 6;
+  }
 }
 
 bool rar_subh::read_subheader(uint8_t** ptr)
 {
   bool e_bit = *(*ptr) & 0x80?true:false;
-  bool type  = *(*ptr) & 0x40?true:false;
-  if (type) {
+  type       = *(*ptr) & 0x40 ? RAPID : BACKOFF;
+  if (type == RAPID) {
     preamble = *(*ptr) & 0x3f;
   } else {
     ((rar_pdu*)parent)->set_backoff(*(*ptr) & 0xf);