1+ /**
2+ *
3+ * \section COPYRIGHT
4+ *
5+ * Copyright 2013-2023 Software Radio Systems Limited
6+ *
7+ * By using this file, you agree to the terms and conditions set
8+ * forth in the LICENSE file which can be found at the top level of
9+ * the distribution.
10+ *
11+ */
12+
13+ #include <complex.h>
14+ #include <math.h>
15+ #include <stdio.h>
16+
17+ #include "srsran/config.h"
18+ #include "srsran/phy/ch_estimation/cedron_freq_estimator.h"
19+ #include "srsran/phy/utils/vector_simd.h"
20+ #include "srsran/srsran.h"
21+ #include <fftw3.h>
22+
23+ static pthread_mutex_t freq_est_fft_mutex = PTHREAD_MUTEX_INITIALIZER ;
24+
25+ int srsran_cedron_freq_est_init (srsran_cedron_freq_est_t * q , uint32_t nof_prbs )
26+ {
27+ int ret = SRSRAN_ERROR_INVALID_INPUTS ;
28+ int N = SRSRAN_MAX_PRB * SRSRAN_NRE ;
29+ if (q != NULL ) {
30+ bzero (q , sizeof (srsran_cedron_freq_est_t ));
31+
32+ q -> init_size = N ;
33+ q -> fft_size = nof_prbs * SRSRAN_NRE ;
34+ q -> in = fftwf_malloc (sizeof (fftwf_complex ) * N );
35+ if (!q -> in ) {
36+ perror ("fftwf_malloc" );
37+ goto clean_exit ;
38+ }
39+ q -> out = fftwf_malloc (sizeof (fftwf_complex ) * N );
40+ if (!q -> out ) {
41+ perror ("fftwf_malloc" );
42+ goto clean_exit ;
43+ }
44+
45+ pthread_mutex_lock (& freq_est_fft_mutex );
46+ q -> p = fftwf_plan_dft_1d (q -> fft_size , q -> in , q -> out , FFTW_FORWARD , 0U );
47+ pthread_mutex_unlock (& freq_est_fft_mutex );
48+
49+ if (!q -> p ) {
50+ perror ("fftwf_plan_dft_1d" );
51+ goto clean_exit ;
52+ }
53+ q -> X = q -> out ;
54+ }
55+
56+ ret = SRSRAN_SUCCESS ;
57+
58+ clean_exit :
59+ if (ret != SRSRAN_SUCCESS ) {
60+ srsran_cedron_freq_est_free (q );
61+ }
62+ return ret ;
63+ }
64+
65+ void srsran_cedron_freq_est_free (srsran_cedron_freq_est_t * q )
66+ {
67+ if (!q ) {
68+ return ;
69+ }
70+ pthread_mutex_lock (& freq_est_fft_mutex );
71+ if (q -> in ) {
72+ fftwf_free (q -> in );
73+ }
74+ if (q -> out ) {
75+ fftwf_free (q -> out );
76+ }
77+ if (q -> p ) {
78+ fftwf_destroy_plan (q -> p );
79+ q -> p = NULL ;
80+ }
81+ q -> X = NULL ;
82+ pthread_mutex_unlock (& freq_est_fft_mutex );
83+ bzero (q , sizeof (srsran_cedron_freq_est_t ));
84+ }
85+
86+ int srsran_cedron_freq_est_replan_c (srsran_cedron_freq_est_t * q , int new_dft_points )
87+ {
88+ // No change in size, skip re-planning
89+ if (q -> fft_size == new_dft_points ) {
90+ return 0 ;
91+ }
92+
93+ pthread_mutex_lock (& freq_est_fft_mutex );
94+ if (q -> p ) {
95+ fftwf_destroy_plan (q -> p );
96+ q -> p = NULL ;
97+ }
98+ q -> p = fftwf_plan_dft_1d (new_dft_points , q -> in , q -> out , FFTW_FORWARD , FFTW_MEASURE );
99+ pthread_mutex_unlock (& freq_est_fft_mutex );
100+
101+ if (!q -> p ) {
102+ return -1 ;
103+ }
104+ q -> fft_size = new_dft_points ;
105+ return 0 ;
106+ }
107+
108+ float srsran_cedron_freq_estimate (srsran_cedron_freq_est_t * q , const cf_t * x , int N )
109+ {
110+ /*
111+ * Three Bin Exact Frequency Formulas for a Pure Complex Tone in a DFT
112+ * Cedron Dawg
113+ * https://www.dsprelated.com/showarticle/1043.php
114+ */
115+ const float TWOPI = 2.0f * (float )M_PI ;
116+ cf_t Z [3 ], R1 , num , den , ratio ;
117+ float alpha , f_est ;
118+ int32_t k_max ;
119+
120+ if (N != q -> fft_size ) {
121+ srsran_cedron_freq_est_replan_c (q , N );
122+ }
123+
124+ memcpy (q -> in , x , sizeof (cf_t ) * N );
125+ fftwf_execute (q -> p );
126+
127+ k_max = srsran_vec_max_ci_simd (q -> X , N );
128+ if (k_max == 0 ) {
129+ Z [0 ] = q -> X [N - 1 ];
130+ Z [1 ] = q -> X [0 ];
131+ Z [2 ] = q -> X [1 ];
132+ } else if (k_max == N - 1 ) {
133+ Z [0 ] = q -> X [N - 2 ];
134+ Z [1 ] = q -> X [N - 1 ];
135+ Z [2 ] = q -> X [0 ];
136+ } else {
137+ Z [0 ] = q -> X [k_max - 1 ];
138+ Z [1 ] = q -> X [k_max ];
139+ Z [2 ] = q -> X [k_max + 1 ];
140+ }
141+
142+ R1 = cexpf (-1.0 * _Complex_I * TWOPI / N );
143+ num = - R1 * Z [0 ] + (1 + R1 ) * Z [1 ] - Z [2 ];
144+ den = - Z [0 ] + (1 + R1 ) * Z [1 ] - R1 * Z [2 ];
145+ srsran_vec_div_ccc_simd (& num , & den , & ratio , 1 );
146+ alpha = atan2f (__imag__ (ratio ), __real__ (ratio ));
147+
148+ if (k_max > floor (N / 2 )) {
149+ k_max = - (N - k_max );
150+ }
151+ f_est = 1.0 * k_max / N + alpha * M_1_PI * 0.5f ;
152+
153+ return - f_est ;
154+ }
0 commit comments