From a044ec262732060cde5624f28b011f9e4d07e2fd Mon Sep 17 00:00:00 2001
From: Guy De Souza <desouza@eurecom.fr>
Date: Fri, 30 Nov 2018 11:33:43 +0100
Subject: [PATCH] scrambling fixes gNB/UE

---
 openair1/PHY/NR_TRANSPORT/nr_pbch.c    | 61 +++++++++++++++++++++++++-
 openair1/PHY/NR_UE_TRANSPORT/nr_pbch.c |  4 +-
 2 files changed, 63 insertions(+), 2 deletions(-)

diff --git a/openair1/PHY/NR_TRANSPORT/nr_pbch.c b/openair1/PHY/NR_TRANSPORT/nr_pbch.c
index 474bb0d505..7aaecdc882 100644
--- a/openair1/PHY/NR_TRANSPORT/nr_pbch.c
+++ b/openair1/PHY/NR_TRANSPORT/nr_pbch.c
@@ -149,7 +149,7 @@ int nr_generate_pbch_dmrs(uint32_t *gold_pbch_dmrs,
   return 0;
 }
 
-void nr_pbch_scrambling(NR_gNB_PBCH *pbch,
+/*void nr_pbch_scrambling(NR_gNB_PBCH *pbch,
                         uint32_t Nid,
                         uint8_t nushift,
                         uint16_t M,
@@ -189,8 +189,67 @@ void nr_pbch_scrambling(NR_gNB_PBCH *pbch,
     else
       pbch_e[i>>5] ^= (((s>>((i+offset)&0x1f))&1)<<(i&0x1f));
   }
+}*/
+
+void nr_pbch_scrambling(NR_gNB_PBCH *pbch,
+                        uint32_t Nid,
+                        uint8_t nushift,
+                        uint16_t M,
+                        uint16_t length,
+                        uint8_t encoded,
+                        uint32_t unscrambling_mask) {
+  uint8_t reset, offset;
+  uint32_t x1, x2, s=0;
+  uint32_t *pbch_e = pbch->pbch_e;
+  reset = 1;
+  // x1 is set in lte_gold_generic
+  x2 = Nid;
+
+  // The Gold sequence is shifted by nushift* M, so we skip (nushift*M /32) double words
+  for (int i=0; i<(uint16_t)ceil(((float)nushift*M)/32); i++) {
+    s = lte_gold_generic(&x1, &x2, reset);
+    reset = 0;
+  }
+
+  // Scrambling is now done with offset (nushift*M)%32
+  offset = (nushift*M)&0x1f;
+#ifdef DEBUG_PBCH_ENCODING
+  printf("Scrambling params: nushift %d M %d length %d encoded %d offset %d\n", nushift, M, length, encoded, offset);
+#endif
+#ifdef DEBUG_PBCH_ENCODING
+  printf("s: %04x\t", s);
+#endif
+  int k = 0;
+
+  if (!encoded) {
+    /// 1st Scrambling
+    for (int i = 0; i < length; ++i) {
+      if ((unscrambling_mask>>i)&1)
+        pbch->pbch_a_prime ^= ((pbch->pbch_a_interleaved>>i)&1)<<i;
+      else {
+        if (((k+offset)&0x1f)==0) {
+          s = lte_gold_generic(&x1, &x2, reset);
+          reset = 0;
+        }
+
+        pbch->pbch_a_prime ^= (((pbch->pbch_a_interleaved>>i)&1) ^ ((s>>((k+offset)&0x1f))&1))<<i;
+        k++;                  /// k increase only when payload bit is not special bit
+      }
+    }
+  } else {
+    /// 2nd Scrambling
+    for (int i = 0; i < length; ++i) {
+      if (((i+offset)&0x1f)==0) {
+        s = lte_gold_generic(&x1, &x2, reset);
+        reset = 0;
+      }
+
+      pbch_e[i>>5] ^= (((s>>((i+offset)&0x1f))&1)<<(i&0x1f));
+    }
+  }
 }
 
+
 uint8_t nr_init_pbch_interleaver(uint8_t *interleaver) {
   uint8_t j_sfn=0, j_hrf=10, j_ssb=11, j_other=14;
   memset((void*)interleaver,0, NR_POLAR_PBCH_PAYLOAD_BITS);
diff --git a/openair1/PHY/NR_UE_TRANSPORT/nr_pbch.c b/openair1/PHY/NR_UE_TRANSPORT/nr_pbch.c
index 454c597f9f..35a081a6f2 100644
--- a/openair1/PHY/NR_UE_TRANSPORT/nr_pbch.c
+++ b/openair1/PHY/NR_UE_TRANSPORT/nr_pbch.c
@@ -441,6 +441,7 @@ void nr_pbch_unscrambling(NR_UE_PBCH *pbch,
 {
   uint8_t reset, offset;
   uint32_t x1, x2, s=0;
+  uint8_t k=0;
   int16_t *demod_pbch_e = pbch->llr;
 
 
@@ -468,7 +469,8 @@ void nr_pbch_unscrambling(NR_UE_PBCH *pbch,
 #endif
       if (bitwise) {
 	
-        (pbch->pbch_a_interleaved) ^= ((unscrambling_mask>>i)&1)? ((pbch->pbch_a_prime>>i)&1)<<i : (((pbch->pbch_a_prime>>i)&1) ^ ((s>>((i+offset)&0x1f))&1))<<i;
+        (pbch->pbch_a_interleaved) ^= ((unscrambling_mask>>i)&1)? ((pbch->pbch_a_prime>>i)&1)<<i : (((pbch->pbch_a_prime>>i)&1) ^ ((s>>((k+offset)&0x1f))&1))<<i;
+        k+=!((unscrambling_mask>>i)&1);
       }
 
       else {