Implement SSB search based on correlation

lib/include/srsran/phy/sync/ssb.h

@@ -80,6 +80,8 @@ typedef struct SRSRAN_API {
   float    scs_hz;                           ///< Subcarrier spacing in Hz
   uint32_t max_symbol_sz;                    ///< Maximum symbol size given the minimum supported SCS and sampling rate
   uint32_t symbol_sz;                        ///< Current SSB symbol size (for the given base-band sampling rate)
+  uint32_t corr_sz;                          ///< Correlation size
+  uint32_t corr_window;                      ///< Correlation window length
   int32_t  f_offset;                         ///< Current SSB integer frequency offset (multiple of SCS)
   uint32_t t_offset;                         ///< Current SSB integer time offset (number of samples)
   uint32_t cp_sz[SRSRAN_SSB_DURATION_NSYMB]; ///< CP length for each SSB symbol
@@ -87,10 +89,14 @@ typedef struct SRSRAN_API {
   /// Internal Objects
   srsran_dft_plan_t ifft;      ///< IFFT object for modulating the SSB
   srsran_dft_plan_t fft;       ///< FFT object for demodulate the SSB.
+  srsran_dft_plan_t fft_corr;  ///< FFT for correlation
+  srsran_dft_plan_t ifft_corr; ///< IFFT for correlation
 
   /// Frequency/Time domain temporal data
-  cf_t* tmp_freq;
+  cf_t* tmp_freq;                     ///< Temporal frequency domain buffer
-  cf_t* tmp_time;
+  cf_t* tmp_time;                     ///< Temporal time domain buffer
+  cf_t* tmp_corr;                     ///< Temporal correlation frequency domain buffer
+  cf_t* pss_seq[SRSRAN_NOF_NID_2_NR]; ///< Possible frequency domain PSS for find
 } srsran_ssb_t;
 
 /**
@@ -148,8 +154,11 @@ srsran_ssb_add(srsran_ssb_t* q, uint32_t N_id, const srsran_pbch_msg_nr_t* msg,
  * @param meas SSB-based CSI measurement of the most suitable cell identifier
  * @return SRSRAN_SUCCESS if the parameters are valid, SRSRAN_ERROR code otherwise
  */
-SRSRAN_API int
+SRSRAN_API int srsran_ssb_csi_search(srsran_ssb_t*                  q,
-srsran_ssb_csi_search(srsran_ssb_t* q, const cf_t* in, uint32_t* N_id, srsran_csi_trs_measurements_t* meas);
+                                     const cf_t*                    in,
+                                     uint32_t                       nof_samples,
+                                     uint32_t*                      N_id,
+                                     srsran_csi_trs_measurements_t* meas);
 
 /**
  * @brief Perform Channel State Information (CSI) measurement from the SSB

lib/src/phy/sync/ssb.c

@@ -27,6 +27,31 @@
  */
 #define SSB_FREQ_OFFSET_MAX_ERROR_HZ 0.01
 
+/**
+ * Correlation size in number of FFTs. It is desired to be power of 2
+ */
+#define SSB_CORR_SZ 4
+
+static int ssb_init_corr(srsran_ssb_t* q)
+{
+  // Initialise correlation only if it is enabled
+  if (!q->args.enable_correlate) {
+    return SRSRAN_SUCCESS;
+  }
+
+  // For each PSS sequence allocate
+  for (uint32_t N_id_2 = 0; N_id_2 < SRSRAN_NOF_NID_2_NR; N_id_2++) {
+    // Allocate sequences
+    q->pss_seq[N_id_2] = srsran_vec_cf_malloc(2 * q->max_symbol_sz);
+    if (q->pss_seq[N_id_2] == NULL) {
+      ERROR("Malloc");
+      return SRSRAN_ERROR;
+    }
+  }
+
+  return SRSRAN_SUCCESS;
+}
+
 int srsran_ssb_init(srsran_ssb_t* q, const srsran_ssb_args_t* args)
 {
   // Verify input parameters
@@ -46,13 +71,19 @@ int srsran_ssb_init(srsran_ssb_t* q, const srsran_ssb_args_t* args)
   q->max_symbol_sz = (uint32_t)round(q->args.max_srate_hz / q->scs_hz);
 
   // Allocate temporal data
-  q->tmp_time = srsran_vec_cf_malloc(q->max_symbol_sz);
+  q->tmp_time = srsran_vec_cf_malloc(SSB_CORR_SZ * q->max_symbol_sz);
-  q->tmp_freq = srsran_vec_cf_malloc(q->max_symbol_sz);
+  q->tmp_freq = srsran_vec_cf_malloc(SSB_CORR_SZ * q->max_symbol_sz);
-  if (q->tmp_time == NULL || q->tmp_time == NULL) {
+  q->tmp_corr = srsran_vec_cf_malloc(SSB_CORR_SZ * q->max_symbol_sz);
+  if (q->tmp_time == NULL || q->tmp_freq == NULL || q->tmp_corr == NULL) {
     ERROR("Malloc");
     return SRSRAN_ERROR;
   }
 
+  // Allocate correlation buffers
+  if (ssb_init_corr(q) < SRSRAN_SUCCESS) {
+    return SRSRAN_ERROR;
+  }
+
   return SRSRAN_SUCCESS;
 }
 
@@ -70,8 +101,21 @@ void srsran_ssb_free(srsran_ssb_t* q)
     free(q->tmp_freq);
   }
 
+  if (q->tmp_corr != NULL) {
+    free(q->tmp_corr);
+  }
+
+  // For each PSS sequence allocate
+  for (uint32_t N_id_2 = 0; N_id_2 < SRSRAN_NOF_NID_2_NR; N_id_2++) {
+    if (q->pss_seq[N_id_2] != NULL) {
+      free(q->pss_seq[N_id_2]);
+    }
+  }
+
   srsran_dft_plan_free(&q->ifft);
   srsran_dft_plan_free(&q->fft);
+  srsran_dft_plan_free(&q->fft_corr);
+  srsran_dft_plan_free(&q->ifft_corr);
 
   SRSRAN_MEM_ZERO(q, srsran_ssb_t, 1);
 }
@@ -225,6 +269,94 @@ static int ssb_first_symbol(const srsran_ssb_cfg_t* cfg, uint32_t ssb_i)
   return SRSRAN_ERROR;
 }
 
+// Modulates a given symbol l and stores the time domain signal in q->tmp_time
+static void ssb_modulate_symbol(srsran_ssb_t* q, cf_t ssb_grid[SRSRAN_SSB_NOF_RE], uint32_t l)
+{
+  // Select symbol in grid
+  cf_t* ptr = &ssb_grid[l * SRSRAN_SSB_BW_SUBC];
+
+  // Map grid into frequency domain symbol
+  if (q->f_offset >= SRSRAN_SSB_BW_SUBC / 2) {
+    srsran_vec_cf_copy(&q->tmp_freq[q->f_offset - SRSRAN_SSB_BW_SUBC / 2], ptr, SRSRAN_SSB_BW_SUBC);
+  } else if (q->f_offset <= -SRSRAN_SSB_BW_SUBC / 2) {
+    srsran_vec_cf_copy(&q->tmp_freq[q->symbol_sz + q->f_offset - SRSRAN_SSB_BW_SUBC / 2], ptr, SRSRAN_SSB_BW_SUBC);
+  } else {
+    srsran_vec_cf_copy(
+        &q->tmp_freq[0], &ptr[SRSRAN_SSB_BW_SUBC / 2 - q->f_offset], SRSRAN_SSB_BW_SUBC / 2 + q->f_offset);
+    srsran_vec_cf_copy(&q->tmp_freq[q->symbol_sz - SRSRAN_SSB_BW_SUBC / 2 + q->f_offset],
+                       &ptr[0],
+                       SRSRAN_SSB_BW_SUBC / 2 - q->f_offset);
+  }
+
+  // Convert to time domain
+  srsran_dft_run_guru_c(&q->ifft);
+
+  // Normalise output
+  float norm = sqrtf((float)q->symbol_sz);
+  if (isnormal(norm)) {
+    srsran_vec_sc_prod_cfc(q->tmp_time, 1.0f / norm, q->tmp_time, q->symbol_sz);
+  }
+}
+
+static int ssb_setup_corr(srsran_ssb_t* q)
+{
+  // Skip if disabled
+  if (!q->args.enable_correlate) {
+    return SRSRAN_SUCCESS;
+  }
+
+  // Skip if the symbol size is unchanged
+  uint32_t corr_sz = q->symbol_sz * SSB_CORR_SZ;
+  if (q->corr_sz == corr_sz) {
+    return SRSRAN_SUCCESS;
+  }
+  q->corr_sz     = corr_sz;
+  q->corr_window = corr_sz - q->symbol_sz;
+
+  // Free correlation
+  srsran_dft_plan_free(&q->fft_corr);
+  srsran_dft_plan_free(&q->ifft_corr);
+
+  // Prepare correlation FFT
+  if (srsran_dft_plan_guru_c(&q->fft_corr, (int)corr_sz, SRSRAN_DFT_FORWARD, q->tmp_time, q->tmp_freq, 1, 1, 1, 1, 1) <
+      SRSRAN_SUCCESS) {
+    ERROR("Error planning correlation DFT");
+    return SRSRAN_ERROR;
+  }
+  if (srsran_dft_plan_guru_c(
+          &q->ifft_corr, (int)corr_sz, SRSRAN_DFT_BACKWARD, q->tmp_corr, q->tmp_time, 1, 1, 1, 1, 1) < SRSRAN_SUCCESS) {
+    ERROR("Error planning correlation DFT");
+    return SRSRAN_ERROR;
+  }
+
+  // Initialise frequency domain
+  srsran_vec_cf_zero(q->tmp_freq, q->symbol_sz);
+
+  // Zero the time domain signal last samples
+  srsran_vec_cf_zero(&q->tmp_time[q->symbol_sz], q->corr_window);
+
+  // Initialise correlation sequence
+  for (uint32_t N_id_2 = 0; N_id_2 < SRSRAN_NOF_NID_2_NR; N_id_2++) {
+    // Put the PSS in SSB grid
+    cf_t ssb_grid[SRSRAN_SSB_NOF_RE] = {};
+    if (srsran_pss_nr_put(ssb_grid, N_id_2, 1.0f) < SRSRAN_SUCCESS) {
+      ERROR("Error putting PDD N_id_2=%d", N_id_2);
+      return SRSRAN_ERROR;
+    }
+
+    // Modulate symbol with PSS
+    ssb_modulate_symbol(q, ssb_grid, SRSRAN_PSS_NR_SYMBOL_IDX);
+
+    // Convert to frequency domain
+    srsran_dft_run_guru_c(&q->fft_corr);
+
+    // Copy frequency domain sequence
+    srsran_vec_cf_copy(q->pss_seq[N_id_2], q->tmp_freq, q->corr_sz);
+  }
+
+  return SRSRAN_SUCCESS;
+}
+
 int srsran_ssb_set_cfg(srsran_ssb_t* q, const srsran_ssb_cfg_t* cfg)
 {
   // Verify input parameters
@@ -238,10 +370,9 @@ int srsran_ssb_set_cfg(srsran_ssb_t* q, const srsran_ssb_cfg_t* cfg)
   // Get first symbol
   int l_begin = ssb_first_symbol(cfg, 0);
   if (l_begin < SRSRAN_SUCCESS) {
-    ERROR("Calculating first SSB symbol");
+    // set it to 2 in case it is not selected
-    return SRSRAN_ERROR;
-  }
     l_begin = 2;
+  }
 
   float t_offset_s = srsran_symbol_offset_s((uint32_t)l_begin, cfg->scs);
   if (isnan(t_offset_s) || isinf(t_offset_s) || t_offset_s < 0.0f) {
@@ -286,7 +417,7 @@ int srsran_ssb_set_cfg(srsran_ssb_t* q, const srsran_ssb_cfg_t* cfg)
   }
 
   // Replan iFFT
-  if ((q->args.enable_encode) && q->symbol_sz != symbol_sz) {
+  if ((q->args.enable_encode || q->args.enable_correlate) && q->symbol_sz != symbol_sz) {
     // free the current IFFT, it internally checks if the plan was created
     srsran_dft_plan_free(&q->ifft);
 
@@ -315,6 +446,12 @@ int srsran_ssb_set_cfg(srsran_ssb_t* q, const srsran_ssb_cfg_t* cfg)
   q->cfg       = *cfg;
   q->symbol_sz = symbol_sz;
 
+  // Initialise correlation
+  if (ssb_setup_corr(q) < SRSRAN_SUCCESS) {
+    ERROR("Error initialising correlation");
+    return SRSRAN_ERROR;
+  }
+
   if (!isnormal(q->cfg.beta_pss)) {
     q->cfg.beta_pss = SRSRAN_SSB_DEFAULT_BETA;
   }
@@ -394,30 +531,7 @@ int srsran_ssb_add(srsran_ssb_t* q, uint32_t N_id, const srsran_pbch_msg_nr_t* m
     // Get CP length
     uint32_t cp_len = q->cp_sz[l];
 
-    // Select symbol in grid
+    ssb_modulate_symbol(q, ssb_grid, l);
-    cf_t* ptr = &ssb_grid[l * SRSRAN_SSB_BW_SUBC];
-
-    // Map grid into frequency domain symbol
-    if (q->f_offset >= SRSRAN_SSB_BW_SUBC / 2) {
-      srsran_vec_cf_copy(&q->tmp_freq[q->f_offset - SRSRAN_SSB_BW_SUBC / 2], ptr, SRSRAN_SSB_BW_SUBC);
-    } else if (q->f_offset <= -SRSRAN_SSB_BW_SUBC / 2) {
-      srsran_vec_cf_copy(&q->tmp_freq[q->symbol_sz + q->f_offset - SRSRAN_SSB_BW_SUBC / 2], ptr, SRSRAN_SSB_BW_SUBC);
-    } else {
-      srsran_vec_cf_copy(
-          &q->tmp_freq[0], &ptr[SRSRAN_SSB_BW_SUBC / 2 - q->f_offset], SRSRAN_SSB_BW_SUBC / 2 + q->f_offset);
-      srsran_vec_cf_copy(&q->tmp_freq[q->symbol_sz - SRSRAN_SSB_BW_SUBC / 2 + q->f_offset],
-                         &ptr[0],
-                         SRSRAN_SSB_BW_SUBC / 2 - q->f_offset);
-    }
-
-    // Convert to time domain
-    srsran_dft_run_guru_c(&q->ifft);
-
-    // Normalise output
-    float norm = sqrtf((float)q->symbol_sz);
-    if (isnormal(norm)) {
-      srsran_vec_sc_prod_cfc(q->tmp_time, 1.0f / norm, q->tmp_time, q->symbol_sz);
-    }
 
     // Add cyclic prefix to input;
     srsran_vec_sum_ccc(in_ptr, &q->tmp_time[q->symbol_sz - cp_len], out_ptr, cp_len);
@@ -433,9 +547,9 @@ int srsran_ssb_add(srsran_ssb_t* q, uint32_t N_id, const srsran_pbch_msg_nr_t* m
   return SRSRAN_SUCCESS;
 }
 
-static int ssb_demodulate(srsran_ssb_t* q, const cf_t* in, cf_t ssb_grid[SRSRAN_SSB_NOF_RE])
+static int ssb_demodulate(srsran_ssb_t* q, const cf_t* in, uint32_t t_offset, cf_t ssb_grid[SRSRAN_SSB_NOF_RE])
 {
-  const cf_t* in_ptr = &in[q->t_offset];
+  const cf_t* in_ptr = &in[t_offset];
   for (uint32_t l = 0; l < SRSRAN_SSB_DURATION_NSYMB; l++) {
     // Get CP length
     uint32_t cp_len = q->cp_sz[l];
@@ -561,31 +675,95 @@ ssb_measure(srsran_ssb_t* q, const cf_t ssb_grid[SRSRAN_SSB_NOF_RE], uint32_t N_
 
   return SRSRAN_SUCCESS;
 }
-int srsran_ssb_csi_search(srsran_ssb_t* q, const cf_t* in, uint32_t* N_id, srsran_csi_trs_measurements_t* meas)
+
+static int
+ssb_pss_search(srsran_ssb_t* q, const cf_t* in, uint32_t nof_samples, uint32_t* found_N_id_2, uint32_t* found_delay)
+{
+  // verify it is initialised
+  if (q->corr_sz == 0) {
+    return SRSRAN_ERROR;
+  }
+
+  // Correlation best sequence
+  float    best_corr   = 0;
+  uint32_t best_delay  = 0;
+  uint32_t best_N_id_2 = 0;
+
+  // Delay in correlation window
+  uint32_t t_offset = 0;
+  while ((t_offset + q->corr_sz) < nof_samples) {
+    // Prepare time domain signal
+    srsran_vec_cf_copy(q->tmp_time, &in[t_offset], q->corr_sz);
+
+    // Convert to frequency domain
+    srsran_dft_run_guru_c(&q->fft_corr);
+
+    // Try each N_id_2 sequence
+    for (uint32_t N_id_2 = 0; N_id_2 < SRSRAN_NOF_NID_2_NR; N_id_2++) {
+      // Actual correlation in frequency domain
+      srsran_vec_prod_conj_ccc(q->tmp_freq, q->pss_seq[N_id_2], q->tmp_corr, q->corr_sz);
+
+      // Convert to time domain
+      srsran_dft_run_guru_c(&q->ifft_corr);
+
+      // Find maximum
+      uint32_t this_max_idx = srsran_vec_max_abs_ci(q->tmp_time, q->corr_window);
+      float    corr         = SRSRAN_CSQABS(q->tmp_time[this_max_idx]);
+
+      // Update if the correlation is better than the current best
+      if (best_corr < corr) {
+        best_corr   = corr;
+        best_delay  = this_max_idx + t_offset;
+        best_N_id_2 = N_id_2;
+      }
+    }
+
+    // Advance time
+    t_offset += q->corr_window;
+  }
+
+  // Save findings
+  *found_delay  = best_delay;
+  *found_N_id_2 = best_N_id_2;
+
+  return SRSRAN_SUCCESS;
+}
+
+int srsran_ssb_csi_search(srsran_ssb_t*                  q,
+                          const cf_t*                    in,
+                          uint32_t                       nof_samples,
+                          uint32_t*                      N_id,
+                          srsran_csi_trs_measurements_t* meas)
 {
   // Verify inputs
   if (q == NULL || in == NULL || N_id == NULL || meas == NULL || !isnormal(q->scs_hz)) {
     return SRSRAN_ERROR_INVALID_INPUTS;
   }
 
-  if (!q->args.enable_measure) {
+  if (!q->args.enable_correlate) {
     ERROR("SSB is not configured for search");
     return SRSRAN_ERROR;
   }
 
-  cf_t ssb_grid[SRSRAN_SSB_NOF_RE] = {};
+  // Search for PSS in time domain
-
+  uint32_t N_id_2   = 0;
-  // Demodulate
+  uint32_t t_offset = 0;
-  if (ssb_demodulate(q, in, ssb_grid) < SRSRAN_SUCCESS) {
+  if (ssb_pss_search(q, in, nof_samples, &N_id_2, &t_offset) < SRSRAN_SUCCESS) {
-    ERROR("Error demodulating");
+    ERROR("Error searching for N_id_2");
     return SRSRAN_ERROR;
   }
 
-  // Find best N_id_2
+  // Remove CP offset prior demodulation
-  uint32_t N_id_2   = 0;
+  if (t_offset >= q->cp_sz[0]) {
-  float    pss_corr = 0.0f;
+    t_offset -= q->cp_sz[0];
-  if (srsran_pss_nr_find(ssb_grid, &pss_corr, &N_id_2) < SRSRAN_SUCCESS) {
+  } else {
-    ERROR("Error searching for N_id_2");
+    t_offset = 0;
+  }
+
+  // Demodulate
+  cf_t ssb_grid[SRSRAN_SSB_NOF_RE] = {};
+  if (ssb_demodulate(q, in, t_offset, ssb_grid) < SRSRAN_SUCCESS) {
+    ERROR("Error demodulating");
     return SRSRAN_ERROR;
   }
 
@@ -606,6 +784,9 @@ int srsran_ssb_csi_search(srsran_ssb_t* q, const cf_t* in, uint32_t* N_id, srsra
     return SRSRAN_ERROR;
   }
 
+  // Add delay to measure
+  meas->delay_us += (float)(1e6 * t_offset / q->cfg.srate_hz);
+
   return SRSRAN_SUCCESS;
 }
 
@@ -624,7 +805,7 @@ int srsran_ssb_csi_measure(srsran_ssb_t* q, uint32_t N_id, const cf_t* in, srsra
   cf_t ssb_grid[SRSRAN_SSB_NOF_RE] = {};
 
   // Demodulate
-  if (ssb_demodulate(q, in, ssb_grid) < SRSRAN_SUCCESS) {
+  if (ssb_demodulate(q, in, q->t_offset, ssb_grid) < SRSRAN_SUCCESS) {
     ERROR("Error demodulating");
     return SRSRAN_ERROR;
   }

lib/src/phy/sync/test/ssb_measure_test.c

@@ -124,19 +124,18 @@ static int test_case_1(srsran_ssb_t* ssb)
     gettimeofday(&t[1], NULL);
     uint32_t                      N_id_found  = 0;
     srsran_csi_trs_measurements_t meas_search = {};
-    TESTASSERT(srsran_ssb_csi_search(ssb, buffer, &N_id_found, &meas_search) == SRSRAN_SUCCESS);
+    TESTASSERT(srsran_ssb_csi_search(ssb, buffer, sf_len, &N_id_found, &meas_search) == SRSRAN_SUCCESS);
     gettimeofday(&t[2], NULL);
     get_time_interval(t);
     t_find_usec += t[0].tv_usec + t[0].tv_sec * 1000000UL;
 
     // Print measurement
-    char str[512];
+    char str[512] = {};
     srsran_csi_meas_info(&meas_search, str, sizeof(str));
-    INFO("test_case_1 - pci=%d %s", pci, str);
+    INFO("test_case_1 - search pci=%d %s", pci, str);
 
-    // Assert find and measurements
+    // Assert find
     TESTASSERT(N_id_found == pci);
-    TESTASSERT(assert_measure(&meas_search) == SRSRAN_SUCCESS);
 
     // Measure
     gettimeofday(&t[1], NULL);
@@ -146,12 +145,16 @@ static int test_case_1(srsran_ssb_t* ssb)
     get_time_interval(t);
     t_meas_usec += t[0].tv_usec + t[0].tv_sec * 1000000UL;
 
+    srsran_csi_meas_info(&meas, str, sizeof(str));
+    INFO("test_case_1 - measure pci=%d %s", pci, str);
+
     // Assert measurements
     TESTASSERT(assert_measure(&meas) == SRSRAN_SUCCESS);
   }
 
-  INFO("test_case_1 - %.1f usec/search; %.1f usec/measurement",
+  INFO("test_case_1 - %.1f usec/search; Max srate %.1f MSps; %.1f usec/measurement",
        (double)t_find_usec / (double)SRSRAN_NOF_NID_NR,
+       (double)sf_len * (double)SRSRAN_NOF_NID_NR / (double)t_find_usec,
        (double)t_meas_usec / (double)SRSRAN_NOF_NID_NR);
 
   return SRSRAN_SUCCESS;
@@ -171,6 +174,7 @@ int main(int argc, char** argv)
   srsran_ssb_args_t ssb_args = {};
   ssb_args.enable_encode     = true;
   ssb_args.enable_measure    = true;
+  ssb_args.enable_correlate  = true;
 
   if (buffer == NULL) {
     ERROR("Malloc");