From c5f10cd56c847d92f9f76c5623fb9e4861209dd2 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 9 Aug 2018 21:08:58 +0200 Subject: [PATCH] Add sse4_1 implementation --- ...k_gnsssdr_32f_fast_resamplerxnpuppet_32f.h | 121 +++---- ...olk_gnsssdr_32f_xn_fast_resampler_32f_xn.h | 298 ++++++++++-------- 2 files changed, 226 insertions(+), 193 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 8a0e20ffb..c63c643da 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 @@ -133,65 +133,68 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_sse3(float* res } #endif -// -// -//#ifdef LV_HAVE_SSE4_1 -//static inline void volk_gnsssdr_32f_resamplerxnpuppet_32f_u_sse4_1(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_sse4_1(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_SSE4_1 -//static inline void volk_gnsssdr_32f_resamplerxnpuppet_32f_a_sse4_1(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_sse4_1(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_SSE4_1 +static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_sse4_1(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_sse4_1(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_SSE4_1 +static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_sse4_1(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_sse4_1(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_resamplerxnpuppet_32f_a_avx(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 be2eb5423..a3728ea16 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 @@ -276,140 +276,170 @@ static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_sse3(float** resu } #endif -// -// -//#ifdef LV_HAVE_SSE4_1 -//#include -//static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_sse4_1(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 quarterPoints = num_points / 4; -// int current_correlator_tap; -// unsigned int n; -// unsigned int k; -// const __m128 fours = _mm_set1_ps(4.0f); -// const __m128 rem_code_phase_chips_reg = _mm_set_ps1(rem_code_phase_chips); -// const __m128 code_phase_step_chips_reg = _mm_set_ps1(code_phase_step_chips); -// -// __VOLK_ATTR_ALIGNED(16) -// int local_code_chip_index[4]; -// int local_code_chip_index_; -// -// const __m128i zeros = _mm_setzero_si128(); -// const __m128 code_length_chips_reg_f = _mm_set_ps1((float)code_length_chips); -// const __m128i code_length_chips_reg_i = _mm_set1_epi32((int)code_length_chips); -// __m128i local_code_chip_index_reg, aux_i, negatives, i; -// __m128 aux, aux2, shifts_chips_reg, c, cTrunc, base; -// -// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++) -// { -// shifts_chips_reg = _mm_set_ps1((float)shifts_chips[current_correlator_tap]); -// aux2 = _mm_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg); -// __m128 indexn = _mm_set_ps(3.0f, 2.0f, 1.0f, 0.0f); -// for (n = 0; n < quarterPoints; n++) -// { -// aux = _mm_mul_ps(code_phase_step_chips_reg, indexn); -// aux = _mm_add_ps(aux, aux2); -// // floor -// aux = _mm_floor_ps(aux); -// -// // fmod -// c = _mm_div_ps(aux, code_length_chips_reg_f); -// i = _mm_cvttps_epi32(c); -// cTrunc = _mm_cvtepi32_ps(i); -// base = _mm_mul_ps(cTrunc, code_length_chips_reg_f); -// local_code_chip_index_reg = _mm_cvtps_epi32(_mm_sub_ps(aux, base)); -// -// negatives = _mm_cmplt_epi32(local_code_chip_index_reg, zeros); -// aux_i = _mm_and_si128(code_length_chips_reg_i, negatives); -// local_code_chip_index_reg = _mm_add_epi32(local_code_chip_index_reg, aux_i); -// _mm_store_si128((__m128i*)local_code_chip_index, local_code_chip_index_reg); -// for (k = 0; k < 4; ++k) -// { -// _result[current_correlator_tap][n * 4 + k] = local_code[local_code_chip_index[k]]; -// } -// indexn = _mm_add_ps(indexn, fours); -// } -// for (n = quarterPoints * 4; 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_SSE4_1 -//#include -//static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_sse4_1(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 quarterPoints = num_points / 4; -// int current_correlator_tap; -// unsigned int n; -// unsigned int k; -// const __m128 fours = _mm_set1_ps(4.0f); -// const __m128 rem_code_phase_chips_reg = _mm_set_ps1(rem_code_phase_chips); -// const __m128 code_phase_step_chips_reg = _mm_set_ps1(code_phase_step_chips); -// -// __VOLK_ATTR_ALIGNED(16) -// int local_code_chip_index[4]; -// int local_code_chip_index_; -// -// const __m128i zeros = _mm_setzero_si128(); -// const __m128 code_length_chips_reg_f = _mm_set_ps1((float)code_length_chips); -// const __m128i code_length_chips_reg_i = _mm_set1_epi32((int)code_length_chips); -// __m128i local_code_chip_index_reg, aux_i, negatives, i; -// __m128 aux, aux2, shifts_chips_reg, c, cTrunc, base; -// -// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++) -// { -// shifts_chips_reg = _mm_set_ps1((float)shifts_chips[current_correlator_tap]); -// aux2 = _mm_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg); -// __m128 indexn = _mm_set_ps(3.0f, 2.0f, 1.0f, 0.0f); -// for (n = 0; n < quarterPoints; n++) -// { -// aux = _mm_mul_ps(code_phase_step_chips_reg, indexn); -// aux = _mm_add_ps(aux, aux2); -// // floor -// aux = _mm_floor_ps(aux); -// -// // fmod -// c = _mm_div_ps(aux, code_length_chips_reg_f); -// i = _mm_cvttps_epi32(c); -// cTrunc = _mm_cvtepi32_ps(i); -// base = _mm_mul_ps(cTrunc, code_length_chips_reg_f); -// local_code_chip_index_reg = _mm_cvtps_epi32(_mm_sub_ps(aux, base)); -// -// negatives = _mm_cmplt_epi32(local_code_chip_index_reg, zeros); -// aux_i = _mm_and_si128(code_length_chips_reg_i, negatives); -// local_code_chip_index_reg = _mm_add_epi32(local_code_chip_index_reg, aux_i); -// _mm_store_si128((__m128i*)local_code_chip_index, local_code_chip_index_reg); -// for (k = 0; k < 4; ++k) -// { -// _result[current_correlator_tap][n * 4 + k] = local_code[local_code_chip_index[k]]; -// } -// indexn = _mm_add_ps(indexn, fours); -// } -// for (n = quarterPoints * 4; 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_SSE4_1 +#include +static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_sse4_1(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 quarterPoints = num_points / 4; + // int current_correlator_tap; + unsigned int n; + unsigned int k; + unsigned int current_correlator_tap; + const __m128 ones = _mm_set1_ps(1.0f); + const __m128 fours = _mm_set1_ps(4.0f); + const __m128 rem_code_phase_chips_reg = _mm_set_ps1(rem_code_phase_chips); + const __m128 code_phase_step_chips_reg = _mm_set_ps1(code_phase_step_chips); + const __m128 code_phase_rate_step_chips_reg = _mm_set_ps1(code_phase_rate_step_chips); + + __VOLK_ATTR_ALIGNED(16) + int local_code_chip_index[4]; + int local_code_chip_index_; + const __m128i zeros = _mm_setzero_si128(); + const __m128 code_length_chips_reg_f = _mm_set_ps1((float)code_length_chips); + const __m128i code_length_chips_reg_i = _mm_set1_epi32((int)code_length_chips); + __m128i local_code_chip_index_reg, aux_i, negatives, i; + __m128 aux, aux2, aux3, indexnn, shifts_chips_reg, c, cTrunc, base; + __m128 indexn = _mm_set_ps(3.0f, 2.0f, 1.0f, 0.0f); + + shifts_chips_reg = _mm_set_ps1((float)shifts_chips[0]); + aux2 = _mm_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg); + + for (n = 0; n < quarterPoints; n++) + { + aux = _mm_mul_ps(code_phase_step_chips_reg, indexn); + indexnn = _mm_mul_ps(indexn, indexn); + aux3 = _mm_mul_ps(code_phase_rate_step_chips_reg, indexnn); + aux = _mm_add_ps(aux, aux3); + aux = _mm_add_ps(aux, aux2); + // floor + aux = _mm_floor_ps(aux); + + // Correct negative shift + c = _mm_div_ps(aux, code_length_chips_reg_f); + aux3 = _mm_add_ps(c, ones); + i = _mm_cvttps_epi32(aux3); + cTrunc = _mm_cvtepi32_ps(i); + base = _mm_mul_ps(cTrunc, code_length_chips_reg_f); + local_code_chip_index_reg = _mm_cvtps_epi32(_mm_sub_ps(aux, base)); + negatives = _mm_cmplt_epi32(local_code_chip_index_reg, zeros); + aux_i = _mm_and_si128(code_length_chips_reg_i, negatives); + local_code_chip_index_reg = _mm_add_epi32(local_code_chip_index_reg, aux_i); + + _mm_store_si128((__m128i*)local_code_chip_index, local_code_chip_index_reg); + + for (k = 0; k < 4; ++k) + { + _result[0][n * 4 + k] = local_code[local_code_chip_index[k]]; + } + indexn = _mm_add_ps(indexn, fours); + } + + for (n = quarterPoints * 4; 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_SSE4_1 +#include +static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_sse4_1(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 quarterPoints = num_points / 4; + // int current_correlator_tap; + unsigned int n; + unsigned int k; + unsigned int current_correlator_tap; + const __m128 ones = _mm_set1_ps(1.0f); + const __m128 fours = _mm_set1_ps(4.0f); + const __m128 rem_code_phase_chips_reg = _mm_set_ps1(rem_code_phase_chips); + const __m128 code_phase_step_chips_reg = _mm_set_ps1(code_phase_step_chips); + const __m128 code_phase_rate_step_chips_reg = _mm_set_ps1(code_phase_rate_step_chips); + + __VOLK_ATTR_ALIGNED(16) + int local_code_chip_index[4]; + int local_code_chip_index_; + const __m128i zeros = _mm_setzero_si128(); + const __m128 code_length_chips_reg_f = _mm_set_ps1((float)code_length_chips); + const __m128i code_length_chips_reg_i = _mm_set1_epi32((int)code_length_chips); + __m128i local_code_chip_index_reg, aux_i, negatives, i; + __m128 aux, aux2, aux3, indexnn, shifts_chips_reg, c, cTrunc, base; + __m128 indexn = _mm_set_ps(3.0f, 2.0f, 1.0f, 0.0f); + + shifts_chips_reg = _mm_set_ps1((float)shifts_chips[0]); + aux2 = _mm_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg); + + for (n = 0; n < quarterPoints; n++) + { + aux = _mm_mul_ps(code_phase_step_chips_reg, indexn); + indexnn = _mm_mul_ps(indexn, indexn); + aux3 = _mm_mul_ps(code_phase_rate_step_chips_reg, indexnn); + aux = _mm_add_ps(aux, aux3); + aux = _mm_add_ps(aux, aux2); + // floor + aux = _mm_floor_ps(aux); + + // Correct negative shift + c = _mm_div_ps(aux, code_length_chips_reg_f); + aux3 = _mm_add_ps(c, ones); + i = _mm_cvttps_epi32(aux3); + cTrunc = _mm_cvtepi32_ps(i); + base = _mm_mul_ps(cTrunc, code_length_chips_reg_f); + local_code_chip_index_reg = _mm_cvtps_epi32(_mm_sub_ps(aux, base)); + negatives = _mm_cmplt_epi32(local_code_chip_index_reg, zeros); + aux_i = _mm_and_si128(code_length_chips_reg_i, negatives); + local_code_chip_index_reg = _mm_add_epi32(local_code_chip_index_reg, aux_i); + + _mm_store_si128((__m128i*)local_code_chip_index, local_code_chip_index_reg); + + for (k = 0; k < 4; ++k) + { + _result[0][n * 4 + k] = local_code[local_code_chip_index[k]]; + } + indexn = _mm_add_ps(indexn, fours); + } + + for (n = quarterPoints * 4; 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