From 06a9d8eb6f649316a2372a70ab4493bdf7658363 Mon Sep 17 00:00:00 2001 From: Xavier Arteaga Date: Fri, 12 Apr 2019 17:21:24 +0200 Subject: [PATCH] Added vector estimate frequency --- lib/include/srslte/phy/utils/simd.h | 17 ++++++++ lib/include/srslte/phy/utils/vector.h | 1 + lib/include/srslte/phy/utils/vector_simd.h | 1 + lib/src/phy/utils/test/vector_test.c | 14 ++++++ lib/src/phy/utils/vector.c | 5 +++ lib/src/phy/utils/vector_simd.c | 51 ++++++++++++++++++++++ 6 files changed, 89 insertions(+) diff --git a/lib/include/srslte/phy/utils/simd.h b/lib/include/srslte/phy/utils/simd.h index 28a233da5..a1cfe9c08 100644 --- a/lib/include/srslte/phy/utils/simd.h +++ b/lib/include/srslte/phy/utils/simd.h @@ -773,6 +773,23 @@ static inline simd_f_t srslte_simd_cf_re(simd_cf_t in) { return out; } +static inline simd_f_t srslte_simd_cf_im(simd_cf_t in) +{ +#ifdef HAVE_NEON + simd_f_t out = in.val[1]; +#else + simd_f_t out = in.im; +#endif /*HAVE_NEON*/ +#ifndef LV_HAVE_AVX512 +#ifdef LV_HAVE_AVX2 + /* Permute for AVX registers (mis SSE registers) */ + const __m256i idx = _mm256_setr_epi32(0, 2, 4, 6, 1, 3, 5, 7); + out = _mm256_permutevar8x32_ps(out, idx); +#endif /* LV_HAVE_AVX2 */ +#endif /* LV_HAVE_AVX512 */ + return out; +} + static inline simd_cf_t srslte_simd_cf_set1 (cf_t x) { simd_cf_t ret; #ifdef LV_HAVE_AVX512 diff --git a/lib/include/srslte/phy/utils/vector.h b/lib/include/srslte/phy/utils/vector.h index 7e7ae546c..ebd60e06a 100644 --- a/lib/include/srslte/phy/utils/vector.h +++ b/lib/include/srslte/phy/utils/vector.h @@ -162,6 +162,7 @@ SRSLTE_API void srslte_vec_interleave_add(const cf_t *x, const cf_t *y, cf_t *z, SRSLTE_API void srslte_vec_apply_cfo(const cf_t *x, float cfo, cf_t *z, int len); +SRSLTE_API float srslte_vec_estimate_frequency(const cf_t* x, int len); #ifdef __cplusplus } diff --git a/lib/include/srslte/phy/utils/vector_simd.h b/lib/include/srslte/phy/utils/vector_simd.h index 60cc9967b..22646b729 100644 --- a/lib/include/srslte/phy/utils/vector_simd.h +++ b/lib/include/srslte/phy/utils/vector_simd.h @@ -137,6 +137,7 @@ SRSLTE_API void srslte_vec_interleave_add_simd(const cf_t *x, const cf_t *y, cf_ SRSLTE_API void srslte_vec_apply_cfo_simd(const cf_t *x, float cfo, cf_t *z, int len); +SRSLTE_API float srslte_vec_estimate_frequency_simd(const cf_t* x, int len); /* SIMD Find Max functions */ SRSLTE_API uint32_t srslte_vec_max_fi_simd(const float *x, const int len); diff --git a/lib/src/phy/utils/test/vector_test.c b/lib/src/phy/utils/test/vector_test.c index c42eb56e5..1bec5b503 100644 --- a/lib/src/phy/utils/test/vector_test.c +++ b/lib/src/phy/utils/test/vector_test.c @@ -800,6 +800,16 @@ TEST(srslte_vec_apply_cfo, free(z); ) +TEST(srslte_vec_estimate_frequency, MALLOC(cf_t, x); float freq_gold = 0.1f; float freq = 0.1f; + + for (int i = 0; i < block_size; i++) { x[i] = cexpf(-I * 2.0f * M_PI * (float)i * freq_gold); } + + if (block_size > 6) { + TEST_CALL(freq = srslte_vec_estimate_frequency(x, block_size);) + } mse = cabsf(freq - freq_gold); + + free(x);) + TEST(srslte_cfo_correct, srslte_cfo_t srslte_cfo; bzero(&srslte_cfo, sizeof(srslte_cfo)); @@ -961,6 +971,10 @@ int main(int argc, char **argv) { passed[func_count][size_count] = test_srslte_vec_apply_cfo(func_names[func_count], &timmings[func_count][size_count], block_size); func_count++; + passed[func_count][size_count] = + test_srslte_vec_estimate_frequency(func_names[func_count], &timmings[func_count][size_count], block_size); + func_count++; + passed[func_count][size_count] = test_srslte_cfo_correct(func_names[func_count], &timmings[func_count][size_count], block_size); func_count++; diff --git a/lib/src/phy/utils/vector.c b/lib/src/phy/utils/vector.c index 4305b109b..e75ee6e91 100644 --- a/lib/src/phy/utils/vector.c +++ b/lib/src/phy/utils/vector.c @@ -467,3 +467,8 @@ void srslte_vec_interleave_add(const cf_t *x, const cf_t *y, cf_t *z, const int void srslte_vec_apply_cfo(const cf_t *x, float cfo, cf_t *z, int len) { srslte_vec_apply_cfo_simd(x, cfo, z, len); } + +float srslte_vec_estimate_frequency(const cf_t* x, int len) +{ + return srslte_vec_estimate_frequency_simd(x, len); +} diff --git a/lib/src/phy/utils/vector_simd.c b/lib/src/phy/utils/vector_simd.c index a92861c4b..a80cf96cb 100644 --- a/lib/src/phy/utils/vector_simd.c +++ b/lib/src/phy/utils/vector_simd.c @@ -1557,3 +1557,54 @@ void srslte_vec_apply_cfo_simd(const cf_t *x, float cfo, cf_t *z, int len) { } } +float srslte_vec_estimate_frequency_simd(const cf_t* x, int len) +{ + float sum_sin = 0.0f; + + /* Asssumes x[n] = cexp(j·2·pi·n·O) = cos(j·2·pi·n·O) + j · sin(j·2·pi·n·O) + * where O = f / f_s */ + + int i = 1; + +#if SRSLTE_SIMD_CF_SIZE + simd_f_t _sum_sin = srslte_simd_f_zero(); + + for (; i < len - SRSLTE_SIMD_CF_SIZE + 1; i += SRSLTE_SIMD_CF_SIZE) { + simd_cf_t a1 = srslte_simd_cfi_loadu(&x[i]); + simd_f_t re1 = srslte_simd_cf_re(a1); + simd_f_t im1 = srslte_simd_cf_im(a1); + + simd_cf_t a2 = srslte_simd_cfi_loadu(&x[i - 1]); + simd_f_t re2 = srslte_simd_cf_re(a2); + simd_f_t im2 = srslte_simd_cf_im(a2); + + simd_f_t _pow = srslte_simd_f_sqrt( + srslte_simd_f_mul(srslte_simd_f_add(srslte_simd_f_mul(re1, re1), srslte_simd_f_mul(im1, im1)), + srslte_simd_f_add(srslte_simd_f_mul(re2, re2), srslte_simd_f_mul(im2, im2)))); + + simd_f_t _sin = srslte_simd_f_mul(srslte_simd_f_sub(srslte_simd_f_mul(re1, im2), srslte_simd_f_mul(re2, im1)), + srslte_simd_f_rcp(_pow)); + _sum_sin = srslte_simd_f_add(_sum_sin, _sin); + } + + float _sum_sin_v[SRSLTE_SIMD_CF_SIZE]; + srslte_simd_f_storeu(_sum_sin_v, _sum_sin); + for (int k = 0; k < SRSLTE_SIMD_CF_SIZE; k++) { + sum_sin += _sum_sin_v[k]; + } +#endif /* SRSLTE_SIMD_CF_SIZE */ + + for (; i < len; i++) { + /* Load current Sample */ + float re1 = crealf(x[i]); + float im1 = cimagf(x[i]); + + /* Load previous sample */ + float re2 = crealf(x[i - 1]); + float im2 = cimagf(x[i - 1]); + + float pow = sqrtf((re1 * re1 + im1 * im1) * (re2 * re2 + im2 * im2)); + sum_sin += (re1 * im2 - re2 * im1) / pow; + } + return asinf(sum_sin / (float)(len - 1)) / (2.0f * (float)M_PI); +}