From 3f6ed8d14a9ef74281034181adbdd6ebe1e3ac3d Mon Sep 17 00:00:00 2001
From: Sagar Parsawar <sagar.parsawar@eurecom.fr>
Date: Fri, 4 Nov 2022 14:59:55 +0100
Subject: [PATCH] Fix for multiple gNB SSB decode in UE phy-test mode

---
 executables/nr-softmodem-common.h             |  2 +
 executables/nr-ue.c                           |  6 ++-
 executables/nr-uesoftmodem.c                  |  4 +-
 executables/nr-uesoftmodem.h                  |  6 ++-
 .../nr_dl_channel_estimation.c                | 38 ++++++++++++++-----
 openair1/PHY/NR_UE_ESTIMATION/nr_estimation.h |  5 ++-
 .../PHY/NR_UE_TRANSPORT/nr_initial_sync.c     |  2 +-
 openair1/SCHED_NR_UE/phy_procedures_nr_ue.c   |  3 +-
 openair2/RRC/NR_UE/main_ue.c                  |  4 +-
 openair2/RRC/NR_UE/rrc_UE.c                   | 14 ++++---
 openair2/RRC/NR_UE/rrc_proto.h                |  4 +-
 11 files changed, 61 insertions(+), 27 deletions(-)

diff --git a/executables/nr-softmodem-common.h b/executables/nr-softmodem-common.h
index fe56f14ee4..8a25395abe 100644
--- a/executables/nr-softmodem-common.h
+++ b/executables/nr-softmodem-common.h
@@ -88,6 +88,8 @@
 #define CONFIG_HLP_DISABLNBIOT   "disable nb-iot, even if defined in config\n"
 #define CONFIG_HLP_DISABLETIMECORR "disable UE timing correction\n"
 #define CONFIG_HLP_RRC_CFG_PATH  "path for RRC configuration\n"
+#define CONFIG_HLP_RE_CFG_FILE   "filename for reconfig.raw in phy-test mode\n"
+#define CONFIG_HLP_RB_CFG_FILE   "filename for rbconfig.raw in phy-test mode\n"
 #define CONFIG_HLP_UECAP_FILE    "path for UE Capabilities file\n"
 
 #define CONFIG_HLP_NUMEROLOGY    "adding numerology for 5G\n"
diff --git a/executables/nr-ue.c b/executables/nr-ue.c
index 8b1f6b969f..f0f9927f3c 100644
--- a/executables/nr-ue.c
+++ b/executables/nr-ue.c
@@ -946,13 +946,15 @@ void *UE_thread(void *arg)
 
 void init_NR_UE(int nb_inst,
                 char* uecap_file,
-                char* rrc_config_path) {
+                char* rrc_config_path, 
+                char* reconfig_file, 
+                char* rbconfig_file) {
   int inst;
   NR_UE_MAC_INST_t *mac_inst;
   NR_UE_RRC_INST_t* rrc_inst;
   
   for (inst=0; inst < nb_inst; inst++) {
-    AssertFatal((rrc_inst = nr_l3_init_ue(uecap_file,rrc_config_path)) != NULL, "can not initialize RRC module\n");
+    AssertFatal((rrc_inst = nr_l3_init_ue(uecap_file,rrc_config_path, reconfig_file, rbconfig_file)) != NULL, "can not initialize RRC module\n");
     AssertFatal((mac_inst = nr_l2_init_ue(rrc_inst)) != NULL, "can not initialize L2 module\n");
     AssertFatal((mac_inst->if_module = nr_ue_if_module_init(inst)) != NULL, "can not initialize IF module\n");
   }
diff --git a/executables/nr-uesoftmodem.c b/executables/nr-uesoftmodem.c
index 1c147047c1..92f957ab8f 100644
--- a/executables/nr-uesoftmodem.c
+++ b/executables/nr-uesoftmodem.c
@@ -123,6 +123,8 @@ char             *usrp_args = NULL;
 char             *tx_subdev = NULL;
 char             *rx_subdev = NULL;
 char       *rrc_config_path = NULL;
+char         *reconfig_file = NULL;
+char         *rbconfig_file = NULL;
 char            *uecap_file = NULL;
 int               dumpframe = 0;
 
@@ -472,7 +474,7 @@ int main( int argc, char **argv ) {
 #endif
   LOG_I(HW, "Version: %s\n", PACKAGE_VERSION);
 
-  init_NR_UE(1,uecap_file,rrc_config_path);
+  init_NR_UE(1,uecap_file,rrc_config_path,reconfig_file,rbconfig_file);
 
   int mode_offset = get_softmodem_params()->nsa ? NUMBER_OF_UE_MAX : 1;
   uint16_t node_number = get_softmodem_params()->node_number;
diff --git a/executables/nr-uesoftmodem.h b/executables/nr-uesoftmodem.h
index 418373ccb4..a9e18098d8 100644
--- a/executables/nr-uesoftmodem.h
+++ b/executables/nr-uesoftmodem.h
@@ -36,11 +36,13 @@
   {"single-thread-disable",    CONFIG_HLP_NOSNGLT,             PARAMFLAG_BOOL,  .iptr=&single_thread_flag,                .defintval=1,                    TYPE_INT,      0}, \
   {"dlsch-parallel",           CONFIG_HLP_DLSCH_PARA,          0,               .u8ptr=NULL,                              .defintval=0,                    TYPE_UINT8,    0}, \
   {"offset-divisor",           CONFIG_HLP_OFFSET_DIV,          0,               .uptr=&nrUE_params.ofdm_offset_divisor,   .defuintval=8,                   TYPE_UINT32,   0}, \
-  {"max-ldpc-iterations",      CONFIG_HLP_MAX_LDPC_ITERATIONS, 0,               .iptr=&nrUE_params.max_ldpc_iterations,  .defuintval=5,                   TYPE_UINT8,    0}, \
+  {"max-ldpc-iterations",      CONFIG_HLP_MAX_LDPC_ITERATIONS, 0,               .iptr=&nrUE_params.max_ldpc_iterations,   .defuintval=5,                   TYPE_UINT8,    0}, \
   {"nr-dlsch-demod-shift",     CONFIG_HLP_DLSHIFT,             0,               .iptr=(int32_t *)&nr_dlsch_demod_shift,   .defintval=0,                    TYPE_INT,      0}, \
   {"V" ,                       CONFIG_HLP_VCD,                 PARAMFLAG_BOOL,  .iptr=&vcdflag,                           .defintval=0,                    TYPE_INT,      0}, \
   {"uecap_file",               CONFIG_HLP_UECAP_FILE,          0,               .strptr=&uecap_file,                      .defstrval="./uecap_ports1.xml", TYPE_STRING,   0}, \
   {"rrc_config_path",          CONFIG_HLP_RRC_CFG_PATH,        0,               .strptr=&rrc_config_path,                 .defstrval="./",                 TYPE_STRING,   0}, \
+  {"reconfig_file",            CONFIG_HLP_RE_CFG_FILE,         0,               .strptr=&reconfig_file,                   .defstrval="reconfig.raw",       TYPE_STRING,   0}, \
+  {"rbconfig_file",            CONFIG_HLP_RB_CFG_FILE,         0,               .strptr=&rbconfig_file,                   .defstrval="rbconfig.raw",       TYPE_STRING,   0}, \
   {"ue-idx-standalone",        NULL,                           0,               .u16ptr=&ue_idx_standalone,               .defuintval=0xFFFF,              TYPE_UINT16,   0}, \
   {"ue-rxgain",                    CONFIG_HLP_UERXG,           0,               .dblptr=&(rx_gain[0][0]),            .defdblval=110,    TYPE_DOUBLE,   0}, \
   {"ue-rxgain-off",                CONFIG_HLP_UERXGOFF,        0,               .dblptr=&rx_gain_off,                .defdblval=0,      TYPE_DOUBLE,   0}, \
@@ -91,7 +93,7 @@ extern nrUE_params_t *get_nrUE_params(void);
 // In nr-ue.c
 extern int setup_nr_ue_buffers(PHY_VARS_NR_UE **phy_vars_ue, openair0_config_t *openair0_cfg);
 extern void fill_ue_band_info(void);
-extern void init_NR_UE(int, char*, char*);
+extern void init_NR_UE(int, char*, char*, char*, char*);
 extern void init_NR_UE_threads(int);
 extern void reset_opp_meas(void);
 extern void print_opp_meas(void);
diff --git a/openair1/PHY/NR_UE_ESTIMATION/nr_dl_channel_estimation.c b/openair1/PHY/NR_UE_ESTIMATION/nr_dl_channel_estimation.c
index 60d2368a48..6684fdf222 100644
--- a/openair1/PHY/NR_UE_ESTIMATION/nr_dl_channel_estimation.c
+++ b/openair1/PHY/NR_UE_ESTIMATION/nr_dl_channel_estimation.c
@@ -34,6 +34,7 @@
 #include "filt16a_32.h"
 #include "T.h"
 #include <openair1/PHY/TOOLS/phy_scope_interface.h>
+extern openair0_config_t openair0_cfg[];
 
 //#define DEBUG_PDSCH
 //#define DEBUG_PDCCH
@@ -45,7 +46,7 @@
 #define NO_INTERP 1
 
 /* Generic function to find the peak of channel estimation buffer */
-void peak_estimator(int32_t *buffer, int32_t buf_len, int32_t *peak_idx, int32_t *peak_val)
+void peak_estimator(int32_t *buffer, int32_t buf_len, int32_t *peak_idx, int32_t *peak_val, int32_t mean_val)
 {
   int32_t max_val = 0, max_idx = 0, abs_val = 0;
   for(int k = 0; k < buf_len; k++)
@@ -57,18 +58,29 @@ void peak_estimator(int32_t *buffer, int32_t buf_len, int32_t *peak_idx, int32_t
       max_idx = k;
     }
   }
-  *peak_val = max_val;
-  *peak_idx = max_idx;
+  
+  // Check for detection threshold
+  LOG_D(PHY,"PRS ToA estimator: max_val %d, mean_val %d, max_idx %d\n", max_val, mean_val, max_idx);
+  if((mean_val !=0) && (max_val/mean_val > 10))
+  {
+    *peak_val = max_val;
+    *peak_idx = max_idx;
+  }
+  else
+  {
+    *peak_val = 0;
+    *peak_idx = 0;
+  }
 }
 
-int nr_prs_channel_estimation(uint8_t rsc_id,
+int nr_prs_channel_estimation(uint8_t gNB_id,
+                              uint8_t rsc_id,
                               uint8_t rep_num,
                               PHY_VARS_NR_UE *ue,
                               UE_nr_rxtx_proc_t *proc,
                               NR_DL_FRAME_PARMS *frame_params,
                               c16_t rxdataF[][ue->frame_parms.samples_per_slot_wCP])
 {
-  int gNB_id = proc->gNB_id;
   uint8_t rxAnt = 0, idx = 0;
   prs_config_t *prs_cfg  = &ue->prs_vars[gNB_id]->prs_resource[rsc_id].prs_cfg;
   prs_meas_t **prs_meas  = ue->prs_vars[gNB_id]->prs_resource[rsc_id].prs_meas;
@@ -84,9 +96,10 @@ int nr_prs_channel_estimation(uint8_t rsc_id,
 
   int16_t *rxF, *pil, mod_prs[NR_MAX_PRS_LENGTH << 1];
   const int16_t *fl, *fm, *fmm, *fml, *fmr, *fr;
-  int16_t ch[2] = {0}, noiseFig[2] = {0};
+  int16_t ch[2] = {0};
   int16_t k_prime = 0, k = 0, re_offset = 0, first_half = 0, second_half = 0;
-  int32_t ch_pwr = 0, snr = 0;
+  int32_t ch_pwr = 0, snr = 0, noiseFig[2] = {0}, mean_val = 0;
+  double ch_pwr_dbm = 0.0f;
 #ifdef DEBUG_PRS_CHEST
   char filename[64] = {0}, varname[64] = {0};
 #endif
@@ -527,10 +540,17 @@ int nr_prs_channel_estimation(uint8_t rsc_id,
          (int16_t *)&chT_interpol[rxAnt][0],1);
 
     // peak estimator
+    mean_val = abs32((int32_t)ch_tmp[(prs_cfg->NumRB*12)]);
     peak_estimator(&chT_interpol[rxAnt][start_offset],
                    frame_params->ofdm_symbol_size,
                    &prs_meas[rxAnt]->dl_toa,
-                   &ch_pwr);
+                   &ch_pwr,
+                   mean_val);
+
+    // adjusting the rx_gains for channel peak power
+    ch_pwr_dbm =  10*log10(ch_pwr)+30
+                  - 10*log10(pow(2,30)) - ((int)openair0_cfg[0].rx_gain[0]
+                  - (int)openair0_cfg[0].rx_gain_offset[0]) - dB_fixed(frame_params->ofdm_symbol_size);
 
     //prs measurements
     prs_meas[rxAnt]->gNB_id     = gNB_id;
@@ -538,7 +558,7 @@ int nr_prs_channel_estimation(uint8_t rsc_id,
     prs_meas[rxAnt]->slot       = proc->nr_slot_rx;
     prs_meas[rxAnt]->rxAnt_idx  = rxAnt;
     prs_meas[rxAnt]->dl_aoa     = rsc_id;
-    LOG_I(PHY, "[gNB %d][rsc %d][Rx %d][sfn %d][slot %d] DL PRS ToA ==> %d / %d samples, peak channel power %.1f dBm, SNR %+2d dB\n", gNB_id, rsc_id, rxAnt, proc->frame_rx, proc->nr_slot_rx, prs_meas[rxAnt]->dl_toa, frame_params->ofdm_symbol_size, 10*log10(ch_pwr/frame_params->ofdm_symbol_size)-30, prs_meas[rxAnt]->snr);
+    LOG_I(PHY, "[gNB %d][rsc %d][Rx %d][sfn %d][slot %d] DL PRS ToA ==> %d / %d samples, peak channel power %.1f dBm, SNR %+2d dB\n", gNB_id, rsc_id, rxAnt, proc->frame_rx, proc->nr_slot_rx, prs_meas[rxAnt]->dl_toa, frame_params->ofdm_symbol_size, ch_pwr_dbm, prs_meas[rxAnt]->snr);
 
 #ifdef DEBUG_PRS_CHEST
     sprintf(filename, "%s%i%s", "PRSpilot_", rxAnt, ".m");
diff --git a/openair1/PHY/NR_UE_ESTIMATION/nr_estimation.h b/openair1/PHY/NR_UE_ESTIMATION/nr_estimation.h
index 19bc2f410c..8ac877a29e 100644
--- a/openair1/PHY/NR_UE_ESTIMATION/nr_estimation.h
+++ b/openair1/PHY/NR_UE_ESTIMATION/nr_estimation.h
@@ -33,7 +33,8 @@
 #define SYNCH_HYST 2
 
 /* A function to perform the channel estimation of DL PRS signal */
-int nr_prs_channel_estimation(uint8_t rsc_id,
+int nr_prs_channel_estimation(uint8_t gNB_id,
+                              uint8_t rsc_id,
                               uint8_t rep_num,
                               PHY_VARS_NR_UE *ue,
                               UE_nr_rxtx_proc_t *proc,
@@ -41,7 +42,7 @@ int nr_prs_channel_estimation(uint8_t rsc_id,
                               c16_t rxdataF[][ue->frame_parms.samples_per_slot_wCP]);
 
 /* Generic function to find the peak of channel estimation buffer */
-void peak_estimator(int32_t *buffer, int32_t buf_len, int32_t *peak_idx, int32_t *peak_val);
+void peak_estimator(int32_t *buffer, int32_t buf_len, int32_t *peak_idx, int32_t *peak_val, int32_t mean_val);
 
 /*!
 \brief This function performs channel estimation including frequency and temporal interpolation
diff --git a/openair1/PHY/NR_UE_TRANSPORT/nr_initial_sync.c b/openair1/PHY/NR_UE_TRANSPORT/nr_initial_sync.c
index a800d3b8d7..8141ece1c9 100644
--- a/openair1/PHY/NR_UE_TRANSPORT/nr_initial_sync.c
+++ b/openair1/PHY/NR_UE_TRANSPORT/nr_initial_sync.c
@@ -187,7 +187,7 @@ int nr_pbch_detection(UE_nr_rxtx_proc_t * proc, PHY_VARS_NR_UE *ue, int pbch_ini
 
 
 #ifdef DEBUG_INITIAL_SYNCH
-    LOG_I(PHY,"[UE%d] Initial sync: pbch decoded sucessfully\n",ue->Mod_id);
+    LOG_I(PHY,"[UE%d] Initial sync: pbch decoded sucessfully, ssb index %d\n",ue->Mod_id, frame_parms->ssb_index);
 #endif
     return(0);
   } else {
diff --git a/openair1/SCHED_NR_UE/phy_procedures_nr_ue.c b/openair1/SCHED_NR_UE/phy_procedures_nr_ue.c
index 21b344d901..0950cbdd34 100644
--- a/openair1/SCHED_NR_UE/phy_procedures_nr_ue.c
+++ b/openair1/SCHED_NR_UE/phy_procedures_nr_ue.c
@@ -943,7 +943,8 @@ void pbch_pdcch_processing(PHY_VARS_NR_UE *ue,
                         (j%fp->symbols_per_slot),
                         rxdataF);
           }
-          nr_prs_channel_estimation(rsc_id,
+          nr_prs_channel_estimation(gNB_id,
+                                    rsc_id,
                                     i,
                                     ue,
                                     proc,
diff --git a/openair2/RRC/NR_UE/main_ue.c b/openair2/RRC/NR_UE/main_ue.c
index 5e2bc429eb..bf7e313879 100644
--- a/openair2/RRC/NR_UE/main_ue.c
+++ b/openair2/RRC/NR_UE/main_ue.c
@@ -34,8 +34,8 @@
 #include "rrc_proto.h"
 #include "common/utils/LOG/log.h"
 
-NR_UE_RRC_INST_t* nr_l3_init_ue(char* uecap, char* rrc_config_path){
+NR_UE_RRC_INST_t* nr_l3_init_ue(char* uecap, char* rrc_config_path, char* reconfig_file, char* rbconfig_file){
     //LOG_I(RRC, "[MAIN] NR UE MAC initialization...\n");
 
-    return openair_rrc_top_init_ue_nr(uecap,rrc_config_path);
+    return openair_rrc_top_init_ue_nr(uecap,rrc_config_path,reconfig_file,rbconfig_file);
 }
diff --git a/openair2/RRC/NR_UE/rrc_UE.c b/openair2/RRC/NR_UE/rrc_UE.c
index bf9861ecfb..9881be47dd 100644
--- a/openair2/RRC/NR_UE/rrc_UE.c
+++ b/openair2/RRC/NR_UE/rrc_UE.c
@@ -345,7 +345,7 @@ void process_nsa_message(NR_UE_RRC_INST_t *rrc, nsa_message_t nsa_message_type,
 }
 
 
-NR_UE_RRC_INST_t* openair_rrc_top_init_ue_nr(char* uecap_file, char* rrc_config_path)
+NR_UE_RRC_INST_t* openair_rrc_top_init_ue_nr(char* uecap_file, char* rrc_config_path, char* reconfig_file, char* rbconfig_file)
 {
   if(NB_NR_UE_INST > 0) {
     NR_UE_rrc_inst = (NR_UE_RRC_INST_t *)calloc(NB_NR_UE_INST , sizeof(NR_UE_RRC_INST_t));
@@ -370,10 +370,12 @@ NR_UE_RRC_INST_t* openair_rrc_top_init_ue_nr(char* uecap_file, char* rrc_config_
       // read in files for RRCReconfiguration and RBconfig
       FILE *fd;
       char filename[1024];
-      if (rrc_config_path)
-        sprintf(filename,"%s/reconfig.raw",rrc_config_path);
+      if (rrc_config_path && reconfig_file)
+        sprintf(filename,"%s/%s",rrc_config_path,reconfig_file);
       else
         sprintf(filename,"reconfig.raw");
+      
+      LOG_I(NR_RRC, "using %s for rrc init[1/2]\n",filename);
       fd = fopen(filename,"r");
       char buffer[1024];
       AssertFatal(fd,
@@ -384,10 +386,12 @@ NR_UE_RRC_INST_t* openair_rrc_top_init_ue_nr(char* uecap_file, char* rrc_config_
       int msg_len=fread(buffer,1,1024,fd);
       fclose(fd);
       process_nsa_message(NR_UE_rrc_inst, nr_SecondaryCellGroupConfig_r15, buffer,msg_len);
-      if (rrc_config_path)
-        sprintf(filename,"%s/rbconfig.raw",rrc_config_path);
+      if (rrc_config_path && rbconfig_file)
+        sprintf(filename,"%s/%s",rrc_config_path,rbconfig_file);
       else
         sprintf(filename,"rbconfig.raw");
+      
+      LOG_I(NR_RRC, "using %s for rrc init[2/2]\n",filename);
       fd = fopen(filename,"r");
       AssertFatal(fd,
                   "cannot read file %s: errno %d, %s\n",
diff --git a/openair2/RRC/NR_UE/rrc_proto.h b/openair2/RRC/NR_UE/rrc_proto.h
index 55e025e0f1..a10b3570ae 100644
--- a/openair2/RRC/NR_UE/rrc_proto.h
+++ b/openair2/RRC/NR_UE/rrc_proto.h
@@ -56,14 +56,14 @@ extern queue_t nr_ul_tti_req_queue;
 //  main_rrc.c
 //
 /**\brief Layer 3 initialization*/
-NR_UE_RRC_INST_t* nr_l3_init_ue(char*,char*);
+NR_UE_RRC_INST_t* nr_l3_init_ue(char*,char*,char*,char*);
 
 //
 //  UE_rrc.c
 //
 
 /**\brief Initial the top level RRC structure instance*/
-NR_UE_RRC_INST_t* openair_rrc_top_init_ue_nr(char*,char*);
+NR_UE_RRC_INST_t* openair_rrc_top_init_ue_nr(char*,char*,char*,char*);
 
 
 
-- 
2.26.2