mirror of https://github.com/PentHertz/srsLTE.git
Added vector estimate frequency
This commit is contained in:
parent
4187781268
commit
06a9d8eb6f
|
@ -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
|
||||
|
|
|
@ -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
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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++;
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue