feature: Estimate CFO from CSI-RS tracking signal

TRS resources if configured by gnb, is present in two consequtive slots
and two symbols per slot. LS estimates from the first symbol is saved
and in second symbol frequency offset and time offset is estimated. The
estimated FO is saved in UE global structure which is used in the last
TRS slot. If the FO is above a threshold, the radio's center frequency
is adjusted.

Time offset is estimated but not used because time is already tracked
using PBCH DMRS.

Signed-off-by: Sakthivel Velumani <s.velumani@northeastern.edu>

executables/nr-ue.c

@@ -215,9 +215,8 @@ static void UE_synch(void *arg) {
         ((ret.rx_offset << 1) / fp->samples_per_subframe * fp->slots_per_subframe)
         + round((float)((ret.rx_offset << 1) % fp->samples_per_subframe) / fp->samples_per_slot0);
 
-    if (get_nrUE_params()->cont_fo_comp) {
     UE->freq_offset = freq_offset - UE->dl_Doppler_shift;
-    } else {
+    if (!get_nrUE_params()->cont_fo_comp) {
       // rerun with new cell parameters and frequency-offset
       nrue_ru_set_freq(UE, ul_carrier, dl_carrier, freq_offset);
     }
@@ -711,6 +710,18 @@ static inline int get_readBlockSize(uint16_t slot, const NR_DL_FRAME_PARMS *fp)
   return rem_samples + next_slot_first_symbol;
 }
 
+void trs_freq_correction(PHY_VARS_NR_UE *ue, int cfo)
+{
+  if (abs(cfo) > TRS_CFO_THRESH) {
+    LOG_A(PHY, "CFO estimated (%d) from TRS exceeded threshold (%d). Adjusting radio CF\n", cfo, TRS_CFO_THRESH);
+    ue->freq_offset += cfo;
+    uint64_t dl_carrier;
+    uint64_t ul_carrier;
+    nr_get_carrier_frequencies(ue, &dl_carrier, &ul_carrier);
+    nrue_ru_set_freq(ue, ul_carrier, dl_carrier, ue->freq_offset);
+  }
+}
+
 void *UE_thread(void *arg)
 {
   //this thread should be over the processing thread to keep in real time

nfapi/open-nFAPI/nfapi/public_inc/fapi_nr_ue_interface.h

@@ -463,6 +463,7 @@ typedef struct {
   uint8_t power_control_offset;     // Ratio of PDSCH EPRE to NZP CSI-RSEPRE [3GPP TS 38.214, sec 5.2.2.3.1], Value: 0->23 representing -8 to 15 dB in 1dB steps; 255: L1 is configured with ProfileSSS
   uint8_t power_control_offset_ss;  // Ratio of NZP CSI-RS EPRE to SSB/PBCH block EPRE [3GPP TS 38.214, sec 5.2.2.3.1], Values: 0: -3dB; 1: 0dB; 2: 3dB; 3: 6dB; 255: L1 is configured with ProfileSSS
   uint8_t measurement_bitmap;       // bit 0 RSRP, bit 1 RI, bit 2 LI, bit 3 PMI, bit 4 CQI, bit 5 i1
+  uint8_t last_trs_slot;            // indicates to PHY if the slot is end of TRS burst
 } fapi_nr_dl_config_csirs_pdu_rel15_t;
 
 typedef enum{vrb_to_prb_mapping_non_interleaved = 0, vrb_to_prb_mapping_interleaved = 1} vrb_to_prb_mapping_t;

openair1/PHY/NR_UE_TRANSPORT/csi_rx.c

@@ -262,6 +262,8 @@ static int nr_csi_rs_channel_estimation(
     int16_t *log2_maxh,
     uint32_t *noise_power)
 {
+  const int meas_bitmap = csirs_config_pdu->measurement_bitmap;
+  AssertFatal(meas_bitmap != 1, "No need to do CSI-RS channel estimation for only RSRP measurment\n");
   *noise_power = 0;
   int maxh = 0;
   int count = 0;
@@ -301,10 +303,16 @@ static int nr_csi_rs_channel_estimation(
               const c16_t *rx_csi_rs_signal = &csi_rs_received_signal[ant_rx][symbol_offset];
               c16_t tmp =
                   c16MulConjShift(tx_csi_rs_signal[k_tx], rx_csi_rs_signal[k_rx], nr_csi_info->csi_rs_generated_signal_bits);
+              if (csirs_config_pdu->csi_type != 0) {
                 // This is not just the LS estimation for each (k,l), but also the sum of the different contributions
                 // for the sake of optimizing the memory used.
                 csi_rs_ls_estimated_channel[ant_rx][port_tx][kinit_tx].r += tmp.r;
                 csi_rs_ls_estimated_channel[ant_rx][port_tx][kinit_tx].i += tmp.i;
+              } else {
+                // for tracking we want estimates of all sub carriers having CSI-RS
+                csi_rs_ls_estimated_channel[ant_rx][port_tx][k_tx].r = tmp.r;
+                csi_rs_ls_estimated_channel[ant_rx][port_tx][k_tx].i = tmp.i;
+              }
             }
           }
         }
@@ -362,6 +370,10 @@ static int nr_csi_rs_channel_estimation(
       }
     }
 
+    // we need only ls estimates for tracking CSI. So stop here.
+    if (meas_bitmap == 0)
+      continue;
+
     /// Power noise estimation
     AssertFatal(csirs_config_pdu->nr_of_rbs > 0, " nr_of_rbs needs to be greater than 0\n");
     uint16_t noise_real[fp->nb_antennas_rx][csi_mapping->ports][csirs_config_pdu->nr_of_rbs];
@@ -406,9 +418,11 @@ static int nr_csi_rs_channel_estimation(
 
   }
 
+  if (meas_bitmap > 1) {
     *noise_power /= (fp->nb_antennas_rx * csi_mapping->ports);
     *log2_maxh = log2_approx(maxh - 1);
     *log2_re = log2_approx(count - 1);
+  }
 
 #ifdef NR_CSIRS_DEBUG
   LOG_I(NR_PHY, "Noise power estimation based on CSI-RS: %i\n", *noise_power);
@@ -774,6 +788,61 @@ static void nr_csi_im_power_estimation(const PHY_VARS_NR_UE *ue,
 #endif
 }
 
+static double get_cfo(const double phase_diff, const int sym0, const int sym1, const NR_DL_FRAME_PARMS *fp)
+{
+  const double one_fs = 1.0 / (fp->samples_per_frame * 100);
+  int sample_count = 0;
+  for (int s = sym0 + 1; s <= sym1; s++) {
+    const int prefix = (s % (7 * (1 << fp->numerology_index))) ? fp->nb_prefix_samples : fp->nb_prefix_samples0;
+    sample_count += (fp->ofdm_symbol_size + prefix);
+  }
+  const double delta_time = sample_count * one_fs;
+  const double cfo = phase_diff / (2 * M_PI * delta_time);
+  return cfo;
+}
+
+static void nr_ue_trs_processing(PHY_VARS_NR_UE *ue,
+                                 const c16_t res0_est[][1][ue->frame_parms.ofdm_symbol_size],
+                                 const c16_t res1_est[][1][ue->frame_parms.ofdm_symbol_size],
+                                 const c16_t freq_interp_est[][1][ue->frame_parms.ofdm_symbol_size],
+                                 const fapi_nr_dl_config_csirs_pdu_rel15_t *csirs_config_pdu,
+                                 const csi_mapping_parms_t *csi_mapping,
+                                 const int sym0,
+                                 const int sym1,
+                                 int *cfo,
+                                 int *time_offset)
+{
+  AssertFatal((sym0 > -1) && (sym1 > -1) && (sym1 > sym0), "Invalid symbol index for TRS estimation\n");
+  const NR_DL_FRAME_PARMS *fp = &ue->frame_parms;
+  // CFO estimation
+  const int ant_rx = 0; // Estimate only on first antenna port
+  cd_t phase_diff = {0.0};
+  AssertFatal(csirs_config_pdu->freq_density == 3,
+              "CSI-RS for tracking must have freq density of 3 but has %d\n",
+              csirs_config_pdu->freq_density);
+  AssertFatal(csi_mapping->kprime == 0 && csi_mapping->lprime == 0, "Invalid kprime, lprime for CSI-RS for tracking (row 1)\n");
+  for (int rb = csirs_config_pdu->start_rb; rb < (csirs_config_pdu->start_rb + csirs_config_pdu->nr_of_rbs); rb++) {
+    for (int cdm_id = 0; cdm_id < csi_mapping->size; cdm_id++) {
+      uint16_t kinit = rb * NR_NB_SC_PER_RB;
+      uint16_t k = kinit + csi_mapping->koverline[cdm_id];
+
+      const c16_t *res0 = res0_est[ant_rx][0];
+      const c16_t *res1 = res1_est[ant_rx][0];
+      phase_diff.r += res1[k].r * res0[k].r + res1[k].i * res0[k].i;
+      phase_diff.i += res1[k].i * res0[k].r - res1[k].r * res0[k].i;
+    }
+  }
+  *cfo = (int)get_cfo(atan2(phase_diff.i, phase_diff.r), sym0, sym1, fp);
+
+  if (time_offset) {
+    // Time offset estimation
+    __attribute__((aligned(32))) c16_t time_est[fp->ofdm_symbol_size];
+    delay_t delay = {0};
+    nr_est_delay(fp->ofdm_symbol_size, freq_interp_est[ant_rx][0], time_est, &delay);
+    *time_offset = delay.delay_max_pos;
+  }
+}
+
 void nr_ue_csi_im_procedures(PHY_VARS_NR_UE *ue,
                              const c16_t rxdataF[][ue->frame_parms.samples_per_slot_wCP],
                              const fapi_nr_dl_config_csiim_pdu_rel15_t *csiim_config_pdu)
@@ -796,7 +865,10 @@ void nr_ue_csi_im_procedures(PHY_VARS_NR_UE *ue,
 void nr_ue_csi_rs_procedures(PHY_VARS_NR_UE *ue,
                              const UE_nr_rxtx_proc_t *proc,
                              const c16_t rxdataF[][ue->frame_parms.samples_per_slot_wCP],
-                             fapi_nr_dl_config_csirs_pdu_rel15_t *csirs_config_pdu)
+                             fapi_nr_dl_config_csirs_pdu_rel15_t *csirs_config_pdu,
+                             c16_t trs_estimates[][1][ue->frame_parms.ofdm_symbol_size],
+                             const int res_idx,
+                             const int trs_sym0)
 {
 
 #ifdef NR_CSIRS_DEBUG
@@ -816,11 +888,6 @@ void nr_ue_csi_rs_procedures(PHY_VARS_NR_UE *ue,
   LOG_I(NR_PHY, "csirs_config_pdu->power_control_offset_ss = %i\n", csirs_config_pdu->power_control_offset_ss);
 #endif
 
-  if(csirs_config_pdu->csi_type == 0) {
-    LOG_E(NR_PHY, "Handling of CSI-RS for tracking not handled yet at PHY\n");
-    return;
-  }
-
   if(csirs_config_pdu->csi_type == 2) {
     LOG_E(NR_PHY, "Handling of ZP CSI-RS not handled yet at PHY\n");
     return;
@@ -880,7 +947,8 @@ void nr_ue_csi_rs_procedures(PHY_VARS_NR_UE *ue,
   int16_t log2_re = 0;
   int16_t log2_maxh = 0;
   // if we need to measure only RSRP no need to do channel estimation
-  if (csirs_config_pdu->measurement_bitmap > 1)
+  if (csirs_config_pdu->measurement_bitmap != 1) {
+    const bool use_trs_buff = (csirs_config_pdu->csi_type == 0 && res_idx == 0);
     nr_csi_rs_channel_estimation(frame_parms,
                                 csirs_config_pdu,
                                 csi_info,
@@ -888,11 +956,12 @@ void nr_ue_csi_rs_procedures(PHY_VARS_NR_UE *ue,
                                 csi_rs_received_signal,
                                 &mapping_parms,
                                 CDM_group_size,
-                                 csi_rs_ls_estimated_channel,
+                                (use_trs_buff) ? trs_estimates : csi_rs_ls_estimated_channel,
                                 csi_rs_estimated_channel_freq,
                                 &log2_re,
                                 &log2_maxh,
                                 &noise_power);
+  }
 
   uint8_t rank_indicator = 0;
   // bit 1 in bitmap to indicate RI measurment
@@ -926,9 +995,32 @@ void nr_ue_csi_rs_procedures(PHY_VARS_NR_UE *ue,
       nr_csi_rs_cqi_estimation(precoded_sinr_dB, &cqi);
   }
 
+  int trs_cfo = 0;
+  const bool do_trs_est = (csirs_config_pdu->csi_type == 0) && (res_idx == 1);
+  if (do_trs_est) {
+    nr_ue_trs_processing(ue,
+                         trs_estimates,
+                         csi_rs_ls_estimated_channel,
+                         csi_rs_estimated_channel_freq,
+                         csirs_config_pdu,
+                         &mapping_parms,
+                         trs_sym0,
+                         csirs_config_pdu->symb_l0,
+                         &trs_cfo,
+                         NULL); // Time offset not estimated because it is corrected using PBCH DMRS
+  }
+
   switch (csirs_config_pdu->measurement_bitmap) {
-    case 1 :
+    case 0:
-      LOG_I(NR_PHY, "[UE %d] RSRP = %i dBm\n", ue->Mod_id, rsrp_dBm);
+      if (do_trs_est)
+        LOG_I(NR_PHY,
+              "%d.%d TRS estimated CFO: %d Hz\n",
+              proc->frame_rx,
+              proc->nr_slot_rx,
+              trs_cfo);
+      break;
+    case 1:
+      LOG_I(NR_PHY, "%d.%d [UE %d] RSRP = %i dBm\n", proc->frame_rx, proc->nr_slot_rx, ue->Mod_id, rsrp_dBm);
       break;
     case 26 :
       LOG_I(NR_PHY, "RI = %i i1 = %i.%i.%i, i2 = %i, SINR = %i dB, CQI = %i\n",
@@ -942,6 +1034,10 @@ void nr_ue_csi_rs_procedures(PHY_VARS_NR_UE *ue,
       AssertFatal(false, "Not supported measurement configuration\n");
   }
 
+  if (!ue->cont_fo_comp && do_trs_est && csirs_config_pdu->last_trs_slot) {
+    trs_freq_correction(ue, trs_cfo);
+  }
+
   // Send CSI measurements to MAC
   if (!ue->if_inst || !ue->if_inst->dl_indication)
     return;

openair1/PHY/defs_nr_UE.h

@@ -56,6 +56,11 @@
 //       (0  + 0 * 20) % 512 = 0
 #define NUM_PROCESS_SLOT_TX_BARRIERS 512
 
+// CSI for tracking can have up to 2 resources per slot
+#define MAX_CSI_RES_SLOT 2
+// Threshold to change radio frequency
+#define TRS_CFO_THRESH 500
+
 #include "impl_defs_nr.h"
 #include "time_meas.h"
 #include "PHY/CODING/coding_defs.h"
@@ -553,7 +558,8 @@ typedef struct nr_phy_data_s {
   int n_dlsch_codewords;
   // Sidelink Rx action decided by MAC
   sl_nr_rx_config_type_enum_t sl_rx_action;
-  NR_UE_CSI_RS csirs_vars;
+  int num_csirs;
+  NR_UE_CSI_RS csirs_vars[MAX_CSI_RES_SLOT];
   NR_UE_CSI_IM csiim_vars;
 } nr_phy_data_t;
 

openair1/SCHED_NR_UE/defs.h

@@ -134,7 +134,12 @@ void nr_ue_csi_im_procedures(PHY_VARS_NR_UE *ue,
 void nr_ue_csi_rs_procedures(PHY_VARS_NR_UE *ue,
                              const UE_nr_rxtx_proc_t *proc,
                              const c16_t rxdataF[][ue->frame_parms.samples_per_slot_wCP],
-                             fapi_nr_dl_config_csirs_pdu_rel15_t *csirs_config_pdu);
+                             fapi_nr_dl_config_csirs_pdu_rel15_t *csirs_config_pdu,
+                             c16_t trs_estimates[][1][ue->frame_parms.ofdm_symbol_size],
+                             const int res_idx,
+                             const int trs_sym0);
+
+void trs_freq_correction(PHY_VARS_NR_UE *ue, int cfo);
 
 int psbch_pscch_processing(PHY_VARS_NR_UE *ue, const UE_nr_rxtx_proc_t *proc, nr_phy_data_t *phy_data);
 void phy_procedures_nrUE_SL_TX(PHY_VARS_NR_UE *ue, const UE_nr_rxtx_proc_t *proc, nr_phy_data_tx_t *phy_data, c16_t **txp);

openair1/SCHED_NR_UE/fapi_nr_ue_l1.c

@@ -150,8 +150,14 @@ static void nr_ue_scheduled_response_dl(NR_UE_MAC_INST_t *mac,
         phy_data->csiim_vars.active = true;
         break;
       case FAPI_NR_DL_CONFIG_TYPE_CSI_RS:
-        phy_data->csirs_vars.csirs_config_pdu = pdu->csirs_config_pdu.csirs_config_rel15;
+        AssertFatal(phy_data->num_csirs < MAX_CSI_RES_SLOT, "CSI resources per slot exceeded limit\n");
-        phy_data->csirs_vars.active = true;
+        const int c = phy_data->num_csirs;
+        if (phy_data->csirs_vars[c].active) {
+          AssertFatal(false, "Resource should not be active before its configured\n");
+        }
+        phy_data->csirs_vars[c].csirs_config_pdu = pdu->csirs_config_pdu.csirs_config_rel15;
+        phy_data->csirs_vars[c].active = true;
+        phy_data->num_csirs++;
         break;
       case FAPI_NR_DL_CONFIG_TYPE_RA_DLSCH:
       case FAPI_NR_DL_CONFIG_TYPE_SI_DLSCH:

openair1/SCHED_NR_UE/phy_procedures_nr_ue.c

@@ -1184,16 +1184,35 @@ void pdsch_processing(PHY_VARS_NR_UE *ue, const UE_nr_rxtx_proc_t *proc, nr_phy_
   }
 
   // do procedures for CSI-RS
-  if (phy_data->csirs_vars.active == 1) {
+  {
-    for(int symb = 0; symb < ue->frame_parms.symbols_per_slot; symb++) {
+    /*
-      if(is_csi_rs_in_symbol(phy_data->csirs_vars.csirs_config_pdu, symb)) {
+    CSI-RS for tracking use only one port.
+    Number of CSI-RS resources for tracking is always 2 per slot.
+    Computed estimates from first resource is saved and used while estimating second resource.
+    */
+    c16_t trs_estimates[ue->frame_parms.nb_antennas_rx][1][ue->frame_parms.ofdm_symbol_size];
+    for (int res = 0; res < MAX_CSI_RES_SLOT; res++) {
+      if (phy_data->csirs_vars[res].active == 1) {
+        for (int symb = 0; symb < ue->frame_parms.symbols_per_slot; symb++) {
+          if (is_csi_rs_in_symbol(phy_data->csirs_vars[res].csirs_config_pdu, symb)) {
             if (!slot_fep_map[symb]) {
               nr_slot_fep(ue, &ue->frame_parms, proc->nr_slot_rx, symb, rxdataF, link_type_dl, 0, ue->common_vars.rxdata);
               slot_fep_map[symb] = true;
             }
           }
         }
-    nr_ue_csi_rs_procedures(ue, proc, rxdataF, &phy_data->csirs_vars.csirs_config_pdu);
+        if (res > 0 && phy_data->csirs_vars[res].csirs_config_pdu.csi_type == 0) // tracking CSI
+          AssertFatal(phy_data->csirs_vars[res - 1].active && (phy_data->csirs_vars[res - 1].csirs_config_pdu.csi_type == 0),
+                      "CSI-RS for tracking must have two consecutive active resources\n");
+        nr_ue_csi_rs_procedures(ue,
+                                proc,
+                                rxdataF,
+                                &phy_data->csirs_vars[res].csirs_config_pdu,
+                                trs_estimates,
+                                res,
+                                (res == 1) ? phy_data->csirs_vars[0].csirs_config_pdu.symb_l0 : -1);
+      }
+    }
   }
 
   int16_t *llr[2];

openair1/SIMULATION/NR_PHY/nr_unitary_common.c

@@ -68,3 +68,7 @@ void configure_nr_nfapi_vnf(eth_params_t params)
 {
   UNUSED(params);
 }
+
+void trs_freq_correction(PHY_VARS_NR_UE *ue, int cfo)
+{
+}

openair2/LAYER2/NR_MAC_UE/nr_ue_scheduler.c

@@ -1114,7 +1114,8 @@ static void schedule_ta_command(fapi_nr_dl_config_request_t *dl_config, NR_UE_MA
 
 static NR_CSI_ResourceConfigId_t find_CSI_resourceconfig(NR_CSI_MeasConfig_t *csi_measconfig,
                                                          NR_BWP_Id_t dl_bwp_id,
-                                                         NR_NZP_CSI_RS_ResourceId_t csi_id)
+                                                         NR_NZP_CSI_RS_ResourceId_t csi_id,
+                                                         bool *is_last_res)
 {
   bool found = false;
   for (int csi_list = 0; csi_list < csi_measconfig->csi_ResourceConfigToAddModList->list.count; csi_list++) {
@@ -1139,14 +1140,14 @@ static NR_CSI_ResourceConfigId_t find_CSI_resourceconfig(NR_CSI_MeasConfig_t *cs
           AssertFatal(csi_res->nzp_CSI_RS_Resources.list.array[k], "NZP_CSI_RS_ResourceId shoulan't be NULL\n");
           if (csi_id == *csi_res->nzp_CSI_RS_Resources.list.array[k]) {
             found = true;
+            *is_last_res = (k == (csi_res->nzp_CSI_RS_Resources.list.count - 1));
             break;
           }
         }
         if (found && csi_res->trs_Info)
-          // CRI-RS for Tracking (not implemented yet)
+          /* CRI-RS for Tracking. In this case there is no associated CSI report
-          // in this case we there is no associated CSI report
+           * therefore to signal this we return a value higher than
-          // therefore to signal this we return a value higher than
+           * maxNrofCSI-ResourceConfigurations. */
-          // maxNrofCSI-ResourceConfigurations
           return NR_maxNrofCSI_ResourceConfigurations + 1;
         else if (found)
           return csires->csi_ResourceConfigId;
@@ -1216,7 +1217,8 @@ static void nr_schedule_csirs_reception(NR_UE_MAC_INST_t *mac, int frame, int sl
     csi_period_offset(NULL, nzpcsi->periodicityAndOffset, &period, &offset);
     if((frame * mac->frame_structure.numb_slots_frame + slot-offset) % period != 0)
       continue;
-    NR_CSI_ResourceConfigId_t csi_res_id = find_CSI_resourceconfig(csi_measconfig, dl_bwp_id, nzpcsi->nzp_CSI_RS_ResourceId);
+    bool is_last_res = false;
+    NR_CSI_ResourceConfigId_t csi_res_id = find_CSI_resourceconfig(csi_measconfig, dl_bwp_id, nzpcsi->nzp_CSI_RS_ResourceId, &is_last_res);
     // do not schedule reseption of this CSI-RS if not associated with current BWP
     if(csi_res_id < 0)
       continue;
@@ -1226,9 +1228,12 @@ static void nr_schedule_csirs_reception(NR_UE_MAC_INST_t *mac, int frame, int sl
     csirs_config_pdu->subcarrier_spacing = mu;
     csirs_config_pdu->cyclic_prefix = current_DL_BWP->cyclicprefix ? *current_DL_BWP->cyclicprefix : 0;
 
-    if (csi_res_id > NR_maxNrofCSI_ResourceConfigurations)
+    if (csi_res_id > NR_maxNrofCSI_ResourceConfigurations) {
+      /* According to 38.214 5.1.6.1.1, the number of resources indicate if one
+       * or two consequtive slots for TRS is used. We indicate to phy the last slot. */
+      csirs_config_pdu->last_trs_slot = is_last_res;
       csirs_config_pdu->csi_type = 0; // TRS
-    else
+    } else
       csirs_config_pdu->csi_type = 1; // NZP-CSI-RS
 
     csirs_config_pdu->scramb_id = nzpcsi->scramblingID;