phy: add Cedron freq estimation algorithm

srsran · Nov 22, 2023 · 4eb990c · 4eb990c
1 parent cb581e3
commit 4eb990c
Show file tree

Hide file tree

Showing 13 changed files with 387 additions and 5 deletions.
diff --git a/lib/include/srsran/phy/ch_estimation/cedron_freq_estimator.h b/lib/include/srsran/phy/ch_estimation/cedron_freq_estimator.h
@@ -0,0 +1,39 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2023 Software Radio Systems Limited
+ *
+ * By using this file, you agree to the terms and conditions set
+ * forth in the LICENSE file which can be found at the top level of
+ * the distribution.
+ *
+ */
+
+#ifndef SRSRAN_CEDRON_FREQ_ESTIMATOR_H
+#define SRSRAN_CEDRON_FREQ_ESTIMATOR_H
+
+#include "srsran/config.h"
+#include "srsran/phy/common/phy_common.h"
+#include <stdio.h>
+
+typedef struct {
+  // DFT
+  void* in;  // Input buffer
+  void* out; // Output buffer
+  void* p;   // DFT plan
+  cf_t* X;   // Output buffer as cf_t*
+  int   init_size;
+  int   fft_size; // Currently used FFT size
+
+} srsran_cedron_freq_est_t;
+
+SRSRAN_API int srsran_cedron_freq_est_init(srsran_cedron_freq_est_t* q, uint32_t nof_prbs);
+
+SRSRAN_API void srsran_cedron_freq_est_free(srsran_cedron_freq_est_t* q);
+
+SRSRAN_API int srsran_cedron_freq_est_replan_c(srsran_cedron_freq_est_t* q, int new_dft_points);
+
+SRSRAN_API float srsran_cedron_freq_estimate(srsran_cedron_freq_est_t* q, const cf_t* x, int len);
+
+#endif // SRSRAN_CEDRON_FREQ_ESTIMATOR_H
diff --git a/lib/include/srsran/phy/ch_estimation/chest_ul.h b/lib/include/srsran/phy/ch_estimation/chest_ul.h
@@ -29,6 +29,7 @@
 
 #include "srsran/config.h"
 
+#include "srsran/phy/ch_estimation/cedron_freq_estimator.h"
 #include "srsran/phy/ch_estimation/chest_common.h"
 #include "srsran/phy/ch_estimation/refsignal_ul.h"
 #include "srsran/phy/common/phy_common.h"
@@ -76,6 +77,7 @@ typedef struct {
 
   srsran_interp_linsrsran_vec_t srsran_interp_linvec;
 
+  srsran_cedron_freq_est_t srsran_cedron_freq_est;
 } srsran_chest_ul_t;
 
 SRSRAN_API int srsran_chest_ul_init(srsran_chest_ul_t* q, uint32_t max_prb);

diff --git a/lib/include/srsran/phy/phch/pucch_cfg.h b/lib/include/srsran/phy/phch/pucch_cfg.h
@@ -69,6 +69,7 @@ typedef struct SRSRAN_API {
   float threshold_data_valid_format3;
   float threshold_dmrs_detection;
   bool  meas_ta_en;
+  bool  use_cedron_alg;
 
   // PUCCH configuration generated during a call to encode/decode
   srsran_pucch_format_t format;

diff --git a/lib/include/srsran/phy/phch/pusch_cfg.h b/lib/include/srsran/phy/phch/pusch_cfg.h
@@ -72,6 +72,7 @@ typedef struct SRSRAN_API {
 
   bool meas_epre_en;
   bool meas_ta_en;
+  bool use_cedron_alg;
   bool meas_evm_en;
 
 } srsran_pusch_cfg_t;

diff --git a/lib/include/srsran/srsran.h b/lib/include/srsran/srsran.h
@@ -37,6 +37,7 @@ extern "C" {
 #include "srsran/phy/common/timestamp.h"
 #include "srsran/phy/utils/phy_logger.h"
 
+#include "srsran/phy/ch_estimation/cedron_freq_estimator.h"
 #include "srsran/phy/ch_estimation/chest_dl.h"
 #include "srsran/phy/ch_estimation/chest_ul.h"
 #include "srsran/phy/ch_estimation/csi_rs.h"

diff --git a/lib/src/phy/ch_estimation/cedron_freq_estimator.c b/lib/src/phy/ch_estimation/cedron_freq_estimator.c
@@ -0,0 +1,154 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2023 Software Radio Systems Limited
+ *
+ * By using this file, you agree to the terms and conditions set
+ * forth in the LICENSE file which can be found at the top level of
+ * the distribution.
+ *
+ */
+
+#include <complex.h>
+#include <math.h>
+#include <stdio.h>
+
+#include "srsran/config.h"
+#include "srsran/phy/ch_estimation/cedron_freq_estimator.h"
+#include "srsran/phy/utils/vector_simd.h"
+#include "srsran/srsran.h"
+#include <fftw3.h>
+
+static pthread_mutex_t freq_est_fft_mutex = PTHREAD_MUTEX_INITIALIZER;
+
+int srsran_cedron_freq_est_init(srsran_cedron_freq_est_t* q, uint32_t nof_prbs)
+{
+  int ret = SRSRAN_ERROR_INVALID_INPUTS;
+  int N   = SRSRAN_MAX_PRB * SRSRAN_NRE;
+  if (q != NULL) {
+    bzero(q, sizeof(srsran_cedron_freq_est_t));
+
+    q->init_size = N;
+    q->fft_size  = nof_prbs * SRSRAN_NRE;
+    q->in        = fftwf_malloc(sizeof(fftwf_complex) * N);
+    if (!q->in) {
+      perror("fftwf_malloc");
+      goto clean_exit;
+    }
+    q->out = fftwf_malloc(sizeof(fftwf_complex) * N);
+    if (!q->out) {
+      perror("fftwf_malloc");
+      goto clean_exit;
+    }
+
+    pthread_mutex_lock(&freq_est_fft_mutex);
+    q->p = fftwf_plan_dft_1d(q->fft_size, q->in, q->out, FFTW_FORWARD, 0U);
+    pthread_mutex_unlock(&freq_est_fft_mutex);
+
+    if (!q->p) {
+      perror("fftwf_plan_dft_1d");
+      goto clean_exit;
+    }
+    q->X = q->out;
+  }
+
+  ret = SRSRAN_SUCCESS;
+
+clean_exit:
+  if (ret != SRSRAN_SUCCESS) {
+    srsran_cedron_freq_est_free(q);
+  }
+  return ret;
+}
+
+void srsran_cedron_freq_est_free(srsran_cedron_freq_est_t* q)
+{
+  if (!q) {
+    return;
+  }
+  pthread_mutex_lock(&freq_est_fft_mutex);
+  if (q->in) {
+    fftwf_free(q->in);
+  }
+  if (q->out) {
+    fftwf_free(q->out);
+  }
+  if (q->p) {
+    fftwf_destroy_plan(q->p);
+    q->p = NULL;
+  }
+  q->X = NULL;
+  pthread_mutex_unlock(&freq_est_fft_mutex);
+  bzero(q, sizeof(srsran_cedron_freq_est_t));
+}
+
+int srsran_cedron_freq_est_replan_c(srsran_cedron_freq_est_t* q, int new_dft_points)
+{
+  // No change in size, skip re-planning
+  if (q->fft_size == new_dft_points) {
+    return 0;
+  }
+
+  pthread_mutex_lock(&freq_est_fft_mutex);
+  if (q->p) {
+    fftwf_destroy_plan(q->p);
+    q->p = NULL;
+  }
+  q->p = fftwf_plan_dft_1d(new_dft_points, q->in, q->out, FFTW_FORWARD, FFTW_MEASURE);
+  pthread_mutex_unlock(&freq_est_fft_mutex);
+
+  if (!q->p) {
+    return -1;
+  }
+  q->fft_size = new_dft_points;
+  return 0;
+}
+
+float srsran_cedron_freq_estimate(srsran_cedron_freq_est_t* q, const cf_t* x, int N)
+{
+  /*
+   * Three Bin Exact Frequency Formulas for a Pure Complex Tone in a DFT
+   * Cedron Dawg
+   * https://www.dsprelated.com/showarticle/1043.php
+   */
+  const float TWOPI = 2.0f * (float)M_PI;
+  cf_t        Z[3], R1, num, den, ratio;
+  float       alpha, f_est;
+  int32_t     k_max;
+
+  if (N != q->fft_size) {
+    srsran_cedron_freq_est_replan_c(q, N);
+  }
+
+  memcpy(q->in, x, sizeof(cf_t) * N);
+  fftwf_execute(q->p);
+
+  k_max = srsran_vec_max_ci_simd(q->X, N);
+  if (k_max == 0) {
+    Z[0] = q->X[N - 1];
+    Z[1] = q->X[0];
+    Z[2] = q->X[1];
+  } else if (k_max == N - 1) {
+    Z[0] = q->X[N - 2];
+    Z[1] = q->X[N - 1];
+    Z[2] = q->X[0];
+  } else {
+    Z[0] = q->X[k_max - 1];
+    Z[1] = q->X[k_max];
+    Z[2] = q->X[k_max + 1];
+  }
+
+  R1  = cexpf(-1.0 * _Complex_I * TWOPI / N);
+  num = -R1 * Z[0] + (1 + R1) * Z[1] - Z[2];
+  den = -Z[0] + (1 + R1) * Z[1] - R1 * Z[2];
+  srsran_vec_div_ccc_simd(&num, &den, &ratio, 1);
+  alpha = atan2f(__imag__(ratio), __real__(ratio));
+
+  if (k_max > floor(N / 2)) {
+    k_max = -(N - k_max);
+  }
+  f_est = 1.0 * k_max / N + alpha * M_1_PI * 0.5f;
+
+  return -f_est;
+}
diff --git a/lib/src/phy/ch_estimation/chest_ul.c b/lib/src/phy/ch_estimation/chest_ul.c
@@ -19,6 +19,7 @@
 #include <strings.h>
 
 #include "srsran/config.h"
+#include "srsran/phy/ch_estimation/cedron_freq_estimator.h"
 #include "srsran/phy/ch_estimation/chest_ul.h"
 #include "srsran/phy/dft/dft_precoding.h"
 #include "srsran/phy/utils/convolution.h"
@@ -89,6 +90,11 @@ int srsran_chest_ul_init(srsran_chest_ul_t* q, uint32_t max_prb)
       ERROR("Error allocating memory for pregenerated signals");
       goto clean_exit;
     }
+
+    if (srsran_cedron_freq_est_init(&q->srsran_cedron_freq_est, max_prb)) {
+      ERROR("Error initializing cedron freq estimation algorithm.");
+      goto clean_exit;
+    }
   }
 
   ret = SRSRAN_SUCCESS;
@@ -108,6 +114,7 @@ void srsran_chest_ul_free(srsran_chest_ul_t* q)
     free(q->tmp_noise);
   }
   srsran_interp_linear_vector_free(&q->srsran_interp_linvec);
+  srsran_cedron_freq_est_free(&q->srsran_cedron_freq_est);
 
   if (q->pilot_estimates) {
     free(q->pilot_estimates);
@@ -284,6 +291,7 @@ static void chest_ul_estimate(srsran_chest_ul_t*     q,
                               uint32_t               nrefs_sym,
                               uint32_t               stride,
                               bool                   meas_ta_en,
+                              bool                   use_cedron_alg,
                               bool                   write_estimates,
                               uint32_t               n_prb[SRSRAN_NOF_SLOTS_PER_SF],
                               srsran_chest_ul_res_t* res)
@@ -301,7 +309,13 @@ static void chest_ul_estimate(srsran_chest_ul_t*     q,
   float ta_err = 0.0f;
   if (meas_ta_en) {
     for (int i = 0; i < nslots; i++) {
-      ta_err += srsran_vec_estimate_frequency(&q->pilot_estimates[i * nrefs_sym], nrefs_sym) / nslots;
+      if (use_cedron_alg) {
+        ta_err +=
+            srsran_cedron_freq_estimate(&q->srsran_cedron_freq_est, &q->pilot_estimates[i * nrefs_sym], nrefs_sym) /
+            nslots;
+      } else {
+        ta_err += srsran_vec_estimate_frequency(&q->pilot_estimates[i * nrefs_sym], nrefs_sym) / nslots;
+      }
     }
   }
 
@@ -403,7 +417,8 @@ int srsran_chest_ul_estimate_pusch(srsran_chest_ul_t*     q,
                            nrefs_sf);
 
   // Estimate
-  chest_ul_estimate(q, SRSRAN_NOF_SLOTS_PER_SF, nrefs_sym, 1, cfg->meas_ta_en, true, cfg->grant.n_prb, res);
+  chest_ul_estimate(
+      q, SRSRAN_NOF_SLOTS_PER_SF, nrefs_sym, 1, cfg->meas_ta_en, cfg->use_cedron_alg, true, cfg->grant.n_prb, res);
 
   return 0;
 }
@@ -506,8 +521,14 @@ int srsran_chest_ul_estimate_pucch(srsran_chest_ul_t*     q,
     float ta_err = 0.0f;
     for (int ns = 0; ns < SRSRAN_NOF_SLOTS_PER_SF; ns++) {
       for (int i = 0; i < n_rs; i++) {
-        ta_err += srsran_vec_estimate_frequency(&q->pilot_estimates[(i + ns * n_rs) * SRSRAN_NRE], SRSRAN_NRE) /
-                  (float)(SRSRAN_NOF_SLOTS_PER_SF * n_rs);
+        if (cfg->use_cedron_alg) {
+          ta_err += srsran_cedron_freq_estimate(
+                        &q->srsran_cedron_freq_est, &q->pilot_estimates[(i + ns * n_rs) * SRSRAN_NRE], SRSRAN_NRE) /
+                    (float)(SRSRAN_NOF_SLOTS_PER_SF * n_rs);
+        } else {
+          ta_err += srsran_vec_estimate_frequency(&q->pilot_estimates[(i + ns * n_rs) * SRSRAN_NRE], SRSRAN_NRE) /
+                    (float)(SRSRAN_NOF_SLOTS_PER_SF * n_rs);
+        }
       }
     }
 
@@ -611,7 +632,7 @@ int srsran_chest_ul_estimate_srs(srsran_chest_ul_t*                 q,
 
   // Estimate
   uint32_t n_prb[2] = {};
-  chest_ul_estimate(q, 1, n_srs_re, 1, true, false, n_prb, res);
+  chest_ul_estimate(q, 1, n_srs_re, 1, true, false, false, n_prb, res);
 
   return SRSRAN_SUCCESS;
 }
diff --git a/lib/src/phy/utils/test/CMakeLists.txt b/lib/src/phy/utils/test/CMakeLists.txt
@@ -21,6 +21,9 @@ add_test(dft_dc dft_test -b -d)   # Backwards first & handle dc internally
 add_test(dft_odd dft_test -N 255) # Odd-length
 add_test(dft_odd_dc dft_test -N 255 -b -d) # Odd-length, backwards first, handle dc
 
+add_executable(freq_est_test freq_est_test.c)
+target_link_libraries(freq_est_test srsran_phy)
+
 ########################################################################
 # Algebra TEST
 ########################################################################