From 66bfbffe89f739d6104ed11801e8c9867f5053e8 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 9 Aug 2018 22:00:22 +0200 Subject: [PATCH] Add AVX implementation --- ...k_gnsssdr_32f_fast_resamplerxnpuppet_32f.h | 120 ++++--- ...olk_gnsssdr_32f_xn_fast_resampler_32f_xn.h | 332 ++++++++++-------- 2 files changed, 238 insertions(+), 214 deletions(-) diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_fast_resamplerxnpuppet_32f.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_fast_resamplerxnpuppet_32f.h index c63c643da..bb64dfd6c 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_fast_resamplerxnpuppet_32f.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_fast_resamplerxnpuppet_32f.h @@ -71,9 +71,9 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_generic(float* re volk_gnsssdr_free(result_aux); } - #endif /* LV_HAVE_GENERIC */ + #ifdef LV_HAVE_SSE3 static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_sse3(float* result, const float* local_code, unsigned int num_points) { @@ -104,6 +104,7 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_sse3(float* res #endif + #ifdef LV_HAVE_SSE3 static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_sse3(float* result, const float* local_code, unsigned int num_points) { @@ -195,63 +196,66 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_sse4_1(float* r } #endif -// -//#ifdef LV_HAVE_AVX -//static inline void volk_gnsssdr_32f_resamplerxnpuppet_32f_a_avx(float* result, const float* local_code, unsigned int num_points) -//{ -// int code_length_chips = 2046; -// float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points); -// int num_out_vectors = 3; -// float rem_code_phase_chips = -0.234; -// unsigned int n; -// float shifts_chips[3] = {-0.1, 0.0, 0.1}; -// -// float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment()); -// for (n = 0; n < num_out_vectors; n++) -// { -// result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment()); -// } -// -// volk_gnsssdr_32f_xn_resampler_32f_xn_a_avx(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points); -// -// memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points); -// -// for (n = 0; n < num_out_vectors; n++) -// { -// volk_gnsssdr_free(result_aux[n]); -// } -// volk_gnsssdr_free(result_aux); -//} -//#endif -// -// -//#ifdef LV_HAVE_AVX -//static inline void volk_gnsssdr_32f_resamplerxnpuppet_32f_u_avx(float* result, const float* local_code, unsigned int num_points) -//{ -// int code_length_chips = 2046; -// float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points); -// int num_out_vectors = 3; -// float rem_code_phase_chips = -0.234; -// unsigned int n; -// float shifts_chips[3] = {-0.1, 0.0, 0.1}; -// -// float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment()); -// for (n = 0; n < num_out_vectors; n++) -// { -// result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment()); -// } -// -// volk_gnsssdr_32f_xn_resampler_32f_xn_u_avx(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points); -// -// memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points); -// -// for (n = 0; n < num_out_vectors; n++) -// { -// volk_gnsssdr_free(result_aux[n]); -// } -// volk_gnsssdr_free(result_aux); -//} -//#endif + + +#ifdef LV_HAVE_AVX +static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_avx(float* result, const float* local_code, unsigned int num_points) +{ + int code_length_chips = 2046; + float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points); + int num_out_vectors = 3; + float rem_code_phase_chips = -0.8234; + float code_phase_rate_step_chips = 1.0 / powf(2.0, 33.0); + unsigned int n; + float shifts_chips[3] = {-0.1, 0.0, 0.1}; + + float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment()); + for (n = 0; n < num_out_vectors; n++) + { + result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment()); + } + + volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_avx(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points); + + memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points); + + for (n = 0; n < num_out_vectors; n++) + { + volk_gnsssdr_free(result_aux[n]); + } + volk_gnsssdr_free(result_aux); +} +#endif + + +#ifdef LV_HAVE_AVX +static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_avx(float* result, const float* local_code, unsigned int num_points) +{ + int code_length_chips = 2046; + float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points); + int num_out_vectors = 3; + float rem_code_phase_chips = -0.8234; + float code_phase_rate_step_chips = 1.0 / powf(2.0, 33.0); + unsigned int n; + float shifts_chips[3] = {-0.1, 0.0, 0.1}; + + float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment()); + for (n = 0; n < num_out_vectors; n++) + { + result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment()); + } + + volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_avx(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points); + + memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points); + + for (n = 0; n < num_out_vectors; n++) + { + volk_gnsssdr_free(result_aux[n]); + } + volk_gnsssdr_free(result_aux); +} +#endif // //#ifdef LV_HAVE_NEONV7 //static inline void volk_gnsssdr_32f_resamplerxnpuppet_32f_neon(float* result, const float* local_code, unsigned int num_points) diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_xn_fast_resampler_32f_xn.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_xn_fast_resampler_32f_xn.h index a3728ea16..6f39a680b 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_xn_fast_resampler_32f_xn.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_xn_fast_resampler_32f_xn.h @@ -440,162 +440,182 @@ static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_sse4_1(float** re } #endif -// -// -//#ifdef LV_HAVE_AVX -//#include -//static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_avx(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points) -//{ -// float** _result = result; -// const unsigned int avx_iters = num_points / 8; -// int current_correlator_tap; -// unsigned int n; -// unsigned int k; -// const __m256 eights = _mm256_set1_ps(8.0f); -// const __m256 rem_code_phase_chips_reg = _mm256_set1_ps(rem_code_phase_chips); -// const __m256 code_phase_step_chips_reg = _mm256_set1_ps(code_phase_step_chips); -// -// __VOLK_ATTR_ALIGNED(32) -// int local_code_chip_index[8]; -// int local_code_chip_index_; -// -// const __m256 zeros = _mm256_setzero_ps(); -// const __m256 code_length_chips_reg_f = _mm256_set1_ps((float)code_length_chips); -// const __m256 n0 = _mm256_set_ps(7.0f, 6.0f, 5.0f, 4.0f, 3.0f, 2.0f, 1.0f, 0.0f); -// -// __m256i local_code_chip_index_reg, i; -// __m256 aux, aux2, aux3, shifts_chips_reg, c, cTrunc, base, negatives, indexn; -// -// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++) -// { -// shifts_chips_reg = _mm256_set1_ps((float)shifts_chips[current_correlator_tap]); -// aux2 = _mm256_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg); -// indexn = n0; -// for (n = 0; n < avx_iters; n++) -// { -// __VOLK_GNSSSDR_PREFETCH_LOCALITY(&_result[current_correlator_tap][8 * n + 7], 1, 0); -// __VOLK_GNSSSDR_PREFETCH_LOCALITY(&local_code_chip_index[8], 1, 3); -// aux = _mm256_mul_ps(code_phase_step_chips_reg, indexn); -// aux = _mm256_add_ps(aux, aux2); -// // floor -// aux = _mm256_floor_ps(aux); -// -// // fmod -// c = _mm256_div_ps(aux, code_length_chips_reg_f); -// i = _mm256_cvttps_epi32(c); -// cTrunc = _mm256_cvtepi32_ps(i); -// base = _mm256_mul_ps(cTrunc, code_length_chips_reg_f); -// local_code_chip_index_reg = _mm256_cvttps_epi32(_mm256_sub_ps(aux, base)); -// -// // no negatives -// c = _mm256_cvtepi32_ps(local_code_chip_index_reg); -// negatives = _mm256_cmp_ps(c, zeros, 0x01); -// aux3 = _mm256_and_ps(code_length_chips_reg_f, negatives); -// aux = _mm256_add_ps(c, aux3); -// local_code_chip_index_reg = _mm256_cvttps_epi32(aux); -// -// _mm256_store_si256((__m256i*)local_code_chip_index, local_code_chip_index_reg); -// for (k = 0; k < 8; ++k) -// { -// _result[current_correlator_tap][n * 8 + k] = local_code[local_code_chip_index[k]]; -// } -// indexn = _mm256_add_ps(indexn, eights); -// } -// } -// _mm256_zeroupper(); -// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++) -// { -// for (n = avx_iters * 8; n < num_points; n++) -// { -// // resample code for current tap -// local_code_chip_index_ = (int)floor(code_phase_step_chips * (float)n + shifts_chips[current_correlator_tap] - rem_code_phase_chips); -// //Take into account that in multitap correlators, the shifts can be negative! -// if (local_code_chip_index_ < 0) local_code_chip_index_ += (int)code_length_chips * (abs(local_code_chip_index_) / code_length_chips + 1); -// local_code_chip_index_ = local_code_chip_index_ % code_length_chips; -// _result[current_correlator_tap][n] = local_code[local_code_chip_index_]; -// } -// } -//} -// -//#endif -// -// -//#ifdef LV_HAVE_AVX -//#include -//static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_avx(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points) -//{ -// float** _result = result; -// const unsigned int avx_iters = num_points / 8; -// int current_correlator_tap; -// unsigned int n; -// unsigned int k; -// const __m256 eights = _mm256_set1_ps(8.0f); -// const __m256 rem_code_phase_chips_reg = _mm256_set1_ps(rem_code_phase_chips); -// const __m256 code_phase_step_chips_reg = _mm256_set1_ps(code_phase_step_chips); -// -// __VOLK_ATTR_ALIGNED(32) -// int local_code_chip_index[8]; -// int local_code_chip_index_; -// -// const __m256 zeros = _mm256_setzero_ps(); -// const __m256 code_length_chips_reg_f = _mm256_set1_ps((float)code_length_chips); -// const __m256 n0 = _mm256_set_ps(7.0f, 6.0f, 5.0f, 4.0f, 3.0f, 2.0f, 1.0f, 0.0f); -// -// __m256i local_code_chip_index_reg, i; -// __m256 aux, aux2, aux3, shifts_chips_reg, c, cTrunc, base, negatives, indexn; -// -// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++) -// { -// shifts_chips_reg = _mm256_set1_ps((float)shifts_chips[current_correlator_tap]); -// aux2 = _mm256_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg); -// indexn = n0; -// for (n = 0; n < avx_iters; n++) -// { -// __VOLK_GNSSSDR_PREFETCH_LOCALITY(&_result[current_correlator_tap][8 * n + 7], 1, 0); -// __VOLK_GNSSSDR_PREFETCH_LOCALITY(&local_code_chip_index[8], 1, 3); -// aux = _mm256_mul_ps(code_phase_step_chips_reg, indexn); -// aux = _mm256_add_ps(aux, aux2); -// // floor -// aux = _mm256_floor_ps(aux); -// -// // fmod -// c = _mm256_div_ps(aux, code_length_chips_reg_f); -// i = _mm256_cvttps_epi32(c); -// cTrunc = _mm256_cvtepi32_ps(i); -// base = _mm256_mul_ps(cTrunc, code_length_chips_reg_f); -// local_code_chip_index_reg = _mm256_cvttps_epi32(_mm256_sub_ps(aux, base)); -// -// // no negatives -// c = _mm256_cvtepi32_ps(local_code_chip_index_reg); -// negatives = _mm256_cmp_ps(c, zeros, 0x01); -// aux3 = _mm256_and_ps(code_length_chips_reg_f, negatives); -// aux = _mm256_add_ps(c, aux3); -// local_code_chip_index_reg = _mm256_cvttps_epi32(aux); -// -// _mm256_store_si256((__m256i*)local_code_chip_index, local_code_chip_index_reg); -// for (k = 0; k < 8; ++k) -// { -// _result[current_correlator_tap][n * 8 + k] = local_code[local_code_chip_index[k]]; -// } -// indexn = _mm256_add_ps(indexn, eights); -// } -// } -// _mm256_zeroupper(); -// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++) -// { -// for (n = avx_iters * 8; n < num_points; n++) -// { -// // resample code for current tap -// local_code_chip_index_ = (int)floor(code_phase_step_chips * (float)n + shifts_chips[current_correlator_tap] - rem_code_phase_chips); -// //Take into account that in multitap correlators, the shifts can be negative! -// if (local_code_chip_index_ < 0) local_code_chip_index_ += (int)code_length_chips * (abs(local_code_chip_index_) / code_length_chips + 1); -// local_code_chip_index_ = local_code_chip_index_ % code_length_chips; -// _result[current_correlator_tap][n] = local_code[local_code_chip_index_]; -// } -// } -//} -// -//#endif + + +#ifdef LV_HAVE_AVX +#include +static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_avx(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points) +{ + float** _result = result; + const unsigned int avx_iters = num_points / 8; + int current_correlator_tap; + unsigned int n; + unsigned int k; + const __m256 eights = _mm256_set1_ps(8.0f); + const __m256 ones = _mm256_set1_ps(1.0f); + const __m256 rem_code_phase_chips_reg = _mm256_set1_ps(rem_code_phase_chips); + const __m256 code_phase_step_chips_reg = _mm256_set1_ps(code_phase_step_chips); + const __m256 code_phase_rate_step_chips_reg = _mm256_set1_ps(code_phase_rate_step_chips); + + __VOLK_ATTR_ALIGNED(32) + int local_code_chip_index[8]; + int local_code_chip_index_; + + const __m256 zeros = _mm256_setzero_ps(); + const __m256 code_length_chips_reg_f = _mm256_set1_ps((float)code_length_chips); + const __m256 n0 = _mm256_set_ps(7.0f, 6.0f, 5.0f, 4.0f, 3.0f, 2.0f, 1.0f, 0.0f); + + __m256i local_code_chip_index_reg, i; + __m256 aux, aux2, aux3, shifts_chips_reg, c, cTrunc, base, negatives, indexn, indexnn; + + shifts_chips_reg = _mm256_set1_ps((float)shifts_chips[0]); + aux2 = _mm256_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg); + indexn = n0; + for (n = 0; n < avx_iters; n++) + { + __VOLK_GNSSSDR_PREFETCH_LOCALITY(&_result[0][8 * n + 7], 1, 0); + __VOLK_GNSSSDR_PREFETCH_LOCALITY(&local_code_chip_index[8], 1, 3); + aux = _mm256_mul_ps(code_phase_step_chips_reg, indexn); + indexnn = _mm256_mul_ps(indexn, indexn); + aux3 = _mm256_mul_ps(code_phase_rate_step_chips_reg, indexnn); + aux = _mm256_add_ps(aux, aux3); + aux = _mm256_add_ps(aux, aux2); + // floor + aux = _mm256_floor_ps(aux); + + // Correct negative shift + c = _mm256_div_ps(aux, code_length_chips_reg_f); + aux3 = _mm256_add_ps(c, ones); + i = _mm256_cvttps_epi32(aux3); + cTrunc = _mm256_cvtepi32_ps(i); + base = _mm256_mul_ps(cTrunc, code_length_chips_reg_f); + local_code_chip_index_reg = _mm256_cvttps_epi32(_mm256_sub_ps(aux, base)); + + c = _mm256_cvtepi32_ps(local_code_chip_index_reg); + negatives = _mm256_cmp_ps(c, zeros, 0x01); + aux3 = _mm256_and_ps(code_length_chips_reg_f, negatives); + aux = _mm256_add_ps(c, aux3); + local_code_chip_index_reg = _mm256_cvttps_epi32(aux); + + _mm256_store_si256((__m256i*)local_code_chip_index, local_code_chip_index_reg); + for (k = 0; k < 8; ++k) + { + _result[0][n * 8 + k] = local_code[local_code_chip_index[k]]; + } + indexn = _mm256_add_ps(indexn, eights); + } + + _mm256_zeroupper(); + + for (n = avx_iters * 8; n < num_points; n++) + { + // resample code for first tap + local_code_chip_index_ = (int)floor(code_phase_step_chips * (float)n + code_phase_rate_step_chips * (float)(n * n) + shifts_chips[0] - rem_code_phase_chips); + // Take into account that in multitap correlators, the shifts can be negative! + if (local_code_chip_index_ < 0) local_code_chip_index_ += (int)code_length_chips * (abs(local_code_chip_index_) / code_length_chips + 1); + local_code_chip_index_ = local_code_chip_index_ % code_length_chips; + _result[0][n] = local_code[local_code_chip_index_]; + } + + // adjacent correlators + unsigned int shift_samples = 0; + for (current_correlator_tap = 1; current_correlator_tap < num_out_vectors; current_correlator_tap++) + { + shift_samples += (int)round((shifts_chips[current_correlator_tap] - shifts_chips[current_correlator_tap - 1]) / code_phase_step_chips); + memcpy(&_result[current_correlator_tap][0], &_result[0][shift_samples], (num_points - shift_samples) * sizeof(float)); + memcpy(&_result[current_correlator_tap][num_points - shift_samples], &_result[0][0], shift_samples * sizeof(float)); + } +} + +#endif + + +#ifdef LV_HAVE_AVX +#include +static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_avx(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points) +{ + float** _result = result; + const unsigned int avx_iters = num_points / 8; + int current_correlator_tap; + unsigned int n; + unsigned int k; + const __m256 eights = _mm256_set1_ps(8.0f); + const __m256 ones = _mm256_set1_ps(1.0f); + const __m256 rem_code_phase_chips_reg = _mm256_set1_ps(rem_code_phase_chips); + const __m256 code_phase_step_chips_reg = _mm256_set1_ps(code_phase_step_chips); + const __m256 code_phase_rate_step_chips_reg = _mm256_set1_ps(code_phase_rate_step_chips); + + __VOLK_ATTR_ALIGNED(32) + int local_code_chip_index[8]; + int local_code_chip_index_; + + const __m256 zeros = _mm256_setzero_ps(); + const __m256 code_length_chips_reg_f = _mm256_set1_ps((float)code_length_chips); + const __m256 n0 = _mm256_set_ps(7.0f, 6.0f, 5.0f, 4.0f, 3.0f, 2.0f, 1.0f, 0.0f); + + __m256i local_code_chip_index_reg, i; + __m256 aux, aux2, aux3, shifts_chips_reg, c, cTrunc, base, negatives, indexn, indexnn; + + shifts_chips_reg = _mm256_set1_ps((float)shifts_chips[0]); + aux2 = _mm256_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg); + indexn = n0; + for (n = 0; n < avx_iters; n++) + { + __VOLK_GNSSSDR_PREFETCH_LOCALITY(&_result[0][8 * n + 7], 1, 0); + __VOLK_GNSSSDR_PREFETCH_LOCALITY(&local_code_chip_index[8], 1, 3); + aux = _mm256_mul_ps(code_phase_step_chips_reg, indexn); + indexnn = _mm256_mul_ps(indexn, indexn); + aux3 = _mm256_mul_ps(code_phase_rate_step_chips_reg, indexnn); + aux = _mm256_add_ps(aux, aux3); + aux = _mm256_add_ps(aux, aux2); + // floor + aux = _mm256_floor_ps(aux); + + // Correct negative shift + c = _mm256_div_ps(aux, code_length_chips_reg_f); + aux3 = _mm256_add_ps(c, ones); + i = _mm256_cvttps_epi32(aux3); + cTrunc = _mm256_cvtepi32_ps(i); + base = _mm256_mul_ps(cTrunc, code_length_chips_reg_f); + local_code_chip_index_reg = _mm256_cvttps_epi32(_mm256_sub_ps(aux, base)); + + c = _mm256_cvtepi32_ps(local_code_chip_index_reg); + negatives = _mm256_cmp_ps(c, zeros, 0x01); + aux3 = _mm256_and_ps(code_length_chips_reg_f, negatives); + aux = _mm256_add_ps(c, aux3); + local_code_chip_index_reg = _mm256_cvttps_epi32(aux); + + _mm256_store_si256((__m256i*)local_code_chip_index, local_code_chip_index_reg); + for (k = 0; k < 8; ++k) + { + _result[0][n * 8 + k] = local_code[local_code_chip_index[k]]; + } + indexn = _mm256_add_ps(indexn, eights); + } + + _mm256_zeroupper(); + + for (n = avx_iters * 8; n < num_points; n++) + { + // resample code for first tap + local_code_chip_index_ = (int)floor(code_phase_step_chips * (float)n + code_phase_rate_step_chips * (float)(n * n) + shifts_chips[0] - rem_code_phase_chips); + // Take into account that in multitap correlators, the shifts can be negative! + if (local_code_chip_index_ < 0) local_code_chip_index_ += (int)code_length_chips * (abs(local_code_chip_index_) / code_length_chips + 1); + local_code_chip_index_ = local_code_chip_index_ % code_length_chips; + _result[0][n] = local_code[local_code_chip_index_]; + } + + // adjacent correlators + unsigned int shift_samples = 0; + for (current_correlator_tap = 1; current_correlator_tap < num_out_vectors; current_correlator_tap++) + { + shift_samples += (int)round((shifts_chips[current_correlator_tap] - shifts_chips[current_correlator_tap - 1]) / code_phase_step_chips); + memcpy(&_result[current_correlator_tap][0], &_result[0][shift_samples], (num_points - shift_samples) * sizeof(float)); + memcpy(&_result[current_correlator_tap][num_points - shift_samples], &_result[0][0], shift_samples * sizeof(float)); + } +} + +#endif // // //#ifdef LV_HAVE_NEONV7