mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-16 13:10:35 +00:00
Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next
This commit is contained in:
Carles Fernandez
commit
4b21954be3
@ -71,9 +71,9 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_generic(float* re
|
|||||||
volk_gnsssdr_free(result_aux);
|
volk_gnsssdr_free(result_aux);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
#endif /* LV_HAVE_GENERIC */
|
#endif /* LV_HAVE_GENERIC */
|
||||||
|
|
||||||
|
|
||||||
#ifdef LV_HAVE_SSE3
|
#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)
|
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
|
#endif
|
||||||
|
|
||||||
|
|
||||||
#ifdef LV_HAVE_SSE3
|
#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)
|
static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_sse3(float* result, const float* local_code, unsigned int num_points)
|
||||||
{
|
{
|
||||||
@ -133,122 +134,128 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_sse3(float* res
|
|||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
//
|
|
||||||
//
|
|
||||||
//#ifdef LV_HAVE_SSE4_1
|
#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)
|
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;
|
int code_length_chips = 2046;
|
||||||
// float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
|
float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
|
||||||
// int num_out_vectors = 3;
|
int num_out_vectors = 3;
|
||||||
// float rem_code_phase_chips = -0.234;
|
float rem_code_phase_chips = -0.8234;
|
||||||
// unsigned int n;
|
float code_phase_rate_step_chips = 1.0 / powf(2.0, 33.0);
|
||||||
// float shifts_chips[3] = {-0.1, 0.0, 0.1};
|
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++)
|
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());
|
{
|
||||||
// }
|
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);
|
|
||||||
//
|
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);
|
|
||||||
//
|
memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
|
||||||
// for (n = 0; n < num_out_vectors; n++)
|
|
||||||
// {
|
for (n = 0; n < num_out_vectors; n++)
|
||||||
// volk_gnsssdr_free(result_aux[n]);
|
{
|
||||||
// }
|
volk_gnsssdr_free(result_aux[n]);
|
||||||
// volk_gnsssdr_free(result_aux);
|
}
|
||||||
//}
|
volk_gnsssdr_free(result_aux);
|
||||||
//
|
}
|
||||||
//#endif
|
|
||||||
//
|
#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)
|
|
||||||
//{
|
#ifdef LV_HAVE_SSE4_1
|
||||||
// int code_length_chips = 2046;
|
static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_sse4_1(float* result, const float* local_code, unsigned int num_points)
|
||||||
// float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
|
{
|
||||||
// int num_out_vectors = 3;
|
int code_length_chips = 2046;
|
||||||
// float rem_code_phase_chips = -0.234;
|
float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
|
||||||
// unsigned int n;
|
int num_out_vectors = 3;
|
||||||
// float shifts_chips[3] = {-0.1, 0.0, 0.1};
|
float rem_code_phase_chips = -0.8234;
|
||||||
//
|
float code_phase_rate_step_chips = 1.0 / powf(2.0, 33.0);
|
||||||
// float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment());
|
unsigned int n;
|
||||||
// for (n = 0; n < num_out_vectors; n++)
|
float shifts_chips[3] = {-0.1, 0.0, 0.1};
|
||||||
// {
|
|
||||||
// result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
|
float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment());
|
||||||
// }
|
for (n = 0; n < num_out_vectors; n++)
|
||||||
//
|
{
|
||||||
// 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);
|
result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
|
||||||
//
|
}
|
||||||
// memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
|
|
||||||
//
|
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);
|
||||||
// for (n = 0; n < num_out_vectors; n++)
|
|
||||||
// {
|
memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
|
||||||
// volk_gnsssdr_free(result_aux[n]);
|
|
||||||
// }
|
for (n = 0; n < num_out_vectors; n++)
|
||||||
// volk_gnsssdr_free(result_aux);
|
{
|
||||||
//}
|
volk_gnsssdr_free(result_aux[n]);
|
||||||
//
|
}
|
||||||
//#endif
|
volk_gnsssdr_free(result_aux);
|
||||||
//
|
}
|
||||||
//#ifdef LV_HAVE_AVX
|
|
||||||
//static inline void volk_gnsssdr_32f_resamplerxnpuppet_32f_a_avx(float* result, const float* local_code, unsigned int num_points)
|
#endif
|
||||||
//{
|
|
||||||
// int code_length_chips = 2046;
|
|
||||||
// float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
|
#ifdef LV_HAVE_AVX
|
||||||
// int num_out_vectors = 3;
|
static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_avx(float* result, const float* local_code, unsigned int num_points)
|
||||||
// float rem_code_phase_chips = -0.234;
|
{
|
||||||
// unsigned int n;
|
int code_length_chips = 2046;
|
||||||
// float shifts_chips[3] = {-0.1, 0.0, 0.1};
|
float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
|
||||||
//
|
int num_out_vectors = 3;
|
||||||
// float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment());
|
float rem_code_phase_chips = -0.8234;
|
||||||
// for (n = 0; n < num_out_vectors; n++)
|
float code_phase_rate_step_chips = 1.0 / powf(2.0, 33.0);
|
||||||
// {
|
unsigned int n;
|
||||||
// result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
|
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());
|
||||||
// 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);
|
for (n = 0; n < num_out_vectors; n++)
|
||||||
//
|
{
|
||||||
// memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
|
result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
|
||||||
//
|
}
|
||||||
// for (n = 0; n < num_out_vectors; n++)
|
|
||||||
// {
|
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);
|
||||||
// volk_gnsssdr_free(result_aux[n]);
|
|
||||||
// }
|
memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
|
||||||
// volk_gnsssdr_free(result_aux);
|
|
||||||
//}
|
for (n = 0; n < num_out_vectors; n++)
|
||||||
//#endif
|
{
|
||||||
//
|
volk_gnsssdr_free(result_aux[n]);
|
||||||
//
|
}
|
||||||
//#ifdef LV_HAVE_AVX
|
volk_gnsssdr_free(result_aux);
|
||||||
//static inline void volk_gnsssdr_32f_resamplerxnpuppet_32f_u_avx(float* result, const float* local_code, unsigned int num_points)
|
}
|
||||||
//{
|
#endif
|
||||||
// int code_length_chips = 2046;
|
|
||||||
// float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
|
|
||||||
// int num_out_vectors = 3;
|
#ifdef LV_HAVE_AVX
|
||||||
// float rem_code_phase_chips = -0.234;
|
static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_avx(float* result, const float* local_code, unsigned int num_points)
|
||||||
// unsigned int n;
|
{
|
||||||
// float shifts_chips[3] = {-0.1, 0.0, 0.1};
|
int code_length_chips = 2046;
|
||||||
//
|
float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
|
||||||
// float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment());
|
int num_out_vectors = 3;
|
||||||
// for (n = 0; n < num_out_vectors; n++)
|
float rem_code_phase_chips = -0.8234;
|
||||||
// {
|
float code_phase_rate_step_chips = 1.0 / powf(2.0, 33.0);
|
||||||
// result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
|
unsigned int n;
|
||||||
// }
|
float shifts_chips[3] = {-0.1, 0.0, 0.1};
|
||||||
//
|
|
||||||
// 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);
|
float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment());
|
||||||
//
|
for (n = 0; n < num_out_vectors; n++)
|
||||||
// memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
|
{
|
||||||
//
|
result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
|
||||||
// for (n = 0; n < num_out_vectors; n++)
|
}
|
||||||
// {
|
|
||||||
// volk_gnsssdr_free(result_aux[n]);
|
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);
|
||||||
// }
|
|
||||||
// volk_gnsssdr_free(result_aux);
|
memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
|
||||||
//}
|
|
||||||
//#endif
|
for (n = 0; n < num_out_vectors; n++)
|
||||||
|
{
|
||||||
|
volk_gnsssdr_free(result_aux[n]);
|
||||||
|
}
|
||||||
|
volk_gnsssdr_free(result_aux);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
//
|
//
|
||||||
//#ifdef LV_HAVE_NEONV7
|
//#ifdef LV_HAVE_NEONV7
|
||||||
//static inline void volk_gnsssdr_32f_resamplerxnpuppet_32f_neon(float* result, const float* local_code, unsigned int num_points)
|
//static inline void volk_gnsssdr_32f_resamplerxnpuppet_32f_neon(float* result, const float* local_code, unsigned int num_points)
|
||||||
|
|||||||
@ -276,296 +276,346 @@ static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_sse3(float** resu
|
|||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
//
|
|
||||||
//
|
|
||||||
//#ifdef LV_HAVE_SSE4_1
|
#ifdef LV_HAVE_SSE4_1
|
||||||
//#include <smmintrin.h>
|
#include <smmintrin.h>
|
||||||
//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)
|
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;
|
float** _result = result;
|
||||||
// const unsigned int quarterPoints = num_points / 4;
|
const unsigned int quarterPoints = num_points / 4;
|
||||||
// int current_correlator_tap;
|
// int current_correlator_tap;
|
||||||
// unsigned int n;
|
unsigned int n;
|
||||||
// unsigned int k;
|
unsigned int k;
|
||||||
// const __m128 fours = _mm_set1_ps(4.0f);
|
unsigned int current_correlator_tap;
|
||||||
// const __m128 rem_code_phase_chips_reg = _mm_set_ps1(rem_code_phase_chips);
|
const __m128 ones = _mm_set1_ps(1.0f);
|
||||||
// const __m128 code_phase_step_chips_reg = _mm_set_ps1(code_phase_step_chips);
|
const __m128 fours = _mm_set1_ps(4.0f);
|
||||||
//
|
const __m128 rem_code_phase_chips_reg = _mm_set_ps1(rem_code_phase_chips);
|
||||||
// __VOLK_ATTR_ALIGNED(16)
|
const __m128 code_phase_step_chips_reg = _mm_set_ps1(code_phase_step_chips);
|
||||||
// int local_code_chip_index[4];
|
const __m128 code_phase_rate_step_chips_reg = _mm_set_ps1(code_phase_rate_step_chips);
|
||||||
// int local_code_chip_index_;
|
|
||||||
//
|
__VOLK_ATTR_ALIGNED(16)
|
||||||
// const __m128i zeros = _mm_setzero_si128();
|
int local_code_chip_index[4];
|
||||||
// const __m128 code_length_chips_reg_f = _mm_set_ps1((float)code_length_chips);
|
int local_code_chip_index_;
|
||||||
// const __m128i code_length_chips_reg_i = _mm_set1_epi32((int)code_length_chips);
|
const __m128i zeros = _mm_setzero_si128();
|
||||||
// __m128i local_code_chip_index_reg, aux_i, negatives, i;
|
const __m128 code_length_chips_reg_f = _mm_set_ps1((float)code_length_chips);
|
||||||
// __m128 aux, aux2, shifts_chips_reg, c, cTrunc, base;
|
const __m128i code_length_chips_reg_i = _mm_set1_epi32((int)code_length_chips);
|
||||||
//
|
__m128i local_code_chip_index_reg, aux_i, negatives, i;
|
||||||
// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++)
|
__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[current_correlator_tap]);
|
|
||||||
// aux2 = _mm_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg);
|
shifts_chips_reg = _mm_set_ps1((float)shifts_chips[0]);
|
||||||
// __m128 indexn = _mm_set_ps(3.0f, 2.0f, 1.0f, 0.0f);
|
aux2 = _mm_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg);
|
||||||
// for (n = 0; n < quarterPoints; n++)
|
|
||||||
// {
|
for (n = 0; n < quarterPoints; n++)
|
||||||
// aux = _mm_mul_ps(code_phase_step_chips_reg, indexn);
|
{
|
||||||
// aux = _mm_add_ps(aux, aux2);
|
aux = _mm_mul_ps(code_phase_step_chips_reg, indexn);
|
||||||
// // floor
|
indexnn = _mm_mul_ps(indexn, indexn);
|
||||||
// aux = _mm_floor_ps(aux);
|
aux3 = _mm_mul_ps(code_phase_rate_step_chips_reg, indexnn);
|
||||||
//
|
aux = _mm_add_ps(aux, aux3);
|
||||||
// // fmod
|
aux = _mm_add_ps(aux, aux2);
|
||||||
// c = _mm_div_ps(aux, code_length_chips_reg_f);
|
// floor
|
||||||
// i = _mm_cvttps_epi32(c);
|
aux = _mm_floor_ps(aux);
|
||||||
// cTrunc = _mm_cvtepi32_ps(i);
|
|
||||||
// base = _mm_mul_ps(cTrunc, code_length_chips_reg_f);
|
// Correct negative shift
|
||||||
// local_code_chip_index_reg = _mm_cvtps_epi32(_mm_sub_ps(aux, base));
|
c = _mm_div_ps(aux, code_length_chips_reg_f);
|
||||||
//
|
aux3 = _mm_add_ps(c, ones);
|
||||||
// negatives = _mm_cmplt_epi32(local_code_chip_index_reg, zeros);
|
i = _mm_cvttps_epi32(aux3);
|
||||||
// aux_i = _mm_and_si128(code_length_chips_reg_i, negatives);
|
cTrunc = _mm_cvtepi32_ps(i);
|
||||||
// local_code_chip_index_reg = _mm_add_epi32(local_code_chip_index_reg, aux_i);
|
base = _mm_mul_ps(cTrunc, code_length_chips_reg_f);
|
||||||
// _mm_store_si128((__m128i*)local_code_chip_index, local_code_chip_index_reg);
|
local_code_chip_index_reg = _mm_cvtps_epi32(_mm_sub_ps(aux, base));
|
||||||
// for (k = 0; k < 4; ++k)
|
negatives = _mm_cmplt_epi32(local_code_chip_index_reg, zeros);
|
||||||
// {
|
aux_i = _mm_and_si128(code_length_chips_reg_i, negatives);
|
||||||
// _result[current_correlator_tap][n * 4 + k] = local_code[local_code_chip_index[k]];
|
local_code_chip_index_reg = _mm_add_epi32(local_code_chip_index_reg, aux_i);
|
||||||
// }
|
|
||||||
// indexn = _mm_add_ps(indexn, fours);
|
_mm_store_si128((__m128i*)local_code_chip_index, local_code_chip_index_reg);
|
||||||
// }
|
|
||||||
// for (n = quarterPoints * 4; n < num_points; n++)
|
for (k = 0; k < 4; ++k)
|
||||||
// {
|
{
|
||||||
// // resample code for current tap
|
_result[0][n * 4 + k] = local_code[local_code_chip_index[k]];
|
||||||
// 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!
|
indexn = _mm_add_ps(indexn, fours);
|
||||||
// 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_];
|
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!
|
||||||
//#endif
|
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_];
|
||||||
//#ifdef LV_HAVE_SSE4_1
|
}
|
||||||
//#include <smmintrin.h>
|
|
||||||
//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)
|
// adjacent correlators
|
||||||
//{
|
unsigned int shift_samples = 0;
|
||||||
// float** _result = result;
|
for (current_correlator_tap = 1; current_correlator_tap < num_out_vectors; current_correlator_tap++)
|
||||||
// const unsigned int quarterPoints = num_points / 4;
|
{
|
||||||
// int current_correlator_tap;
|
shift_samples += (int)round((shifts_chips[current_correlator_tap] - shifts_chips[current_correlator_tap - 1]) / code_phase_step_chips);
|
||||||
// unsigned int n;
|
memcpy(&_result[current_correlator_tap][0], &_result[0][shift_samples], (num_points - shift_samples) * sizeof(float));
|
||||||
// unsigned int k;
|
memcpy(&_result[current_correlator_tap][num_points - shift_samples], &_result[0][0], shift_samples * sizeof(float));
|
||||||
// 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);
|
|
||||||
//
|
#endif
|
||||||
// __VOLK_ATTR_ALIGNED(16)
|
|
||||||
// int local_code_chip_index[4];
|
|
||||||
// int local_code_chip_index_;
|
#ifdef LV_HAVE_SSE4_1
|
||||||
//
|
#include <smmintrin.h>
|
||||||
// const __m128i zeros = _mm_setzero_si128();
|
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)
|
||||||
// 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);
|
float** _result = result;
|
||||||
// __m128i local_code_chip_index_reg, aux_i, negatives, i;
|
const unsigned int quarterPoints = num_points / 4;
|
||||||
// __m128 aux, aux2, shifts_chips_reg, c, cTrunc, base;
|
// int current_correlator_tap;
|
||||||
//
|
unsigned int n;
|
||||||
// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++)
|
unsigned int k;
|
||||||
// {
|
unsigned int current_correlator_tap;
|
||||||
// shifts_chips_reg = _mm_set_ps1((float)shifts_chips[current_correlator_tap]);
|
const __m128 ones = _mm_set1_ps(1.0f);
|
||||||
// aux2 = _mm_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg);
|
const __m128 fours = _mm_set1_ps(4.0f);
|
||||||
// __m128 indexn = _mm_set_ps(3.0f, 2.0f, 1.0f, 0.0f);
|
const __m128 rem_code_phase_chips_reg = _mm_set_ps1(rem_code_phase_chips);
|
||||||
// for (n = 0; n < quarterPoints; n++)
|
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);
|
||||||
// aux = _mm_mul_ps(code_phase_step_chips_reg, indexn);
|
|
||||||
// aux = _mm_add_ps(aux, aux2);
|
__VOLK_ATTR_ALIGNED(16)
|
||||||
// // floor
|
int local_code_chip_index[4];
|
||||||
// aux = _mm_floor_ps(aux);
|
int local_code_chip_index_;
|
||||||
//
|
const __m128i zeros = _mm_setzero_si128();
|
||||||
// // fmod
|
const __m128 code_length_chips_reg_f = _mm_set_ps1((float)code_length_chips);
|
||||||
// c = _mm_div_ps(aux, code_length_chips_reg_f);
|
const __m128i code_length_chips_reg_i = _mm_set1_epi32((int)code_length_chips);
|
||||||
// i = _mm_cvttps_epi32(c);
|
__m128i local_code_chip_index_reg, aux_i, negatives, i;
|
||||||
// cTrunc = _mm_cvtepi32_ps(i);
|
__m128 aux, aux2, aux3, indexnn, shifts_chips_reg, c, cTrunc, base;
|
||||||
// base = _mm_mul_ps(cTrunc, code_length_chips_reg_f);
|
__m128 indexn = _mm_set_ps(3.0f, 2.0f, 1.0f, 0.0f);
|
||||||
// local_code_chip_index_reg = _mm_cvtps_epi32(_mm_sub_ps(aux, base));
|
|
||||||
//
|
shifts_chips_reg = _mm_set_ps1((float)shifts_chips[0]);
|
||||||
// negatives = _mm_cmplt_epi32(local_code_chip_index_reg, zeros);
|
aux2 = _mm_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg);
|
||||||
// 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);
|
for (n = 0; n < quarterPoints; n++)
|
||||||
// _mm_store_si128((__m128i*)local_code_chip_index, local_code_chip_index_reg);
|
{
|
||||||
// for (k = 0; k < 4; ++k)
|
aux = _mm_mul_ps(code_phase_step_chips_reg, indexn);
|
||||||
// {
|
indexnn = _mm_mul_ps(indexn, indexn);
|
||||||
// _result[current_correlator_tap][n * 4 + k] = local_code[local_code_chip_index[k]];
|
aux3 = _mm_mul_ps(code_phase_rate_step_chips_reg, indexnn);
|
||||||
// }
|
aux = _mm_add_ps(aux, aux3);
|
||||||
// indexn = _mm_add_ps(indexn, fours);
|
aux = _mm_add_ps(aux, aux2);
|
||||||
// }
|
// floor
|
||||||
// for (n = quarterPoints * 4; n < num_points; n++)
|
aux = _mm_floor_ps(aux);
|
||||||
// {
|
|
||||||
// // resample code for current tap
|
// Correct negative shift
|
||||||
// local_code_chip_index_ = (int)floor(code_phase_step_chips * (float)n + shifts_chips[current_correlator_tap] - rem_code_phase_chips);
|
c = _mm_div_ps(aux, code_length_chips_reg_f);
|
||||||
// //Take into account that in multitap correlators, the shifts can be negative!
|
aux3 = _mm_add_ps(c, ones);
|
||||||
// if (local_code_chip_index_ < 0) local_code_chip_index_ += (int)code_length_chips * (abs(local_code_chip_index_) / code_length_chips + 1);
|
i = _mm_cvttps_epi32(aux3);
|
||||||
// local_code_chip_index_ = local_code_chip_index_ % code_length_chips;
|
cTrunc = _mm_cvtepi32_ps(i);
|
||||||
// _result[current_correlator_tap][n] = local_code[local_code_chip_index_];
|
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);
|
||||||
//#endif
|
|
||||||
//
|
_mm_store_si128((__m128i*)local_code_chip_index, local_code_chip_index_reg);
|
||||||
//
|
|
||||||
//#ifdef LV_HAVE_AVX
|
for (k = 0; k < 4; ++k)
|
||||||
//#include <immintrin.h>
|
{
|
||||||
//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)
|
_result[0][n * 4 + k] = local_code[local_code_chip_index[k]];
|
||||||
//{
|
}
|
||||||
// float** _result = result;
|
indexn = _mm_add_ps(indexn, fours);
|
||||||
// const unsigned int avx_iters = num_points / 8;
|
}
|
||||||
// int current_correlator_tap;
|
|
||||||
// unsigned int n;
|
for (n = quarterPoints * 4; n < num_points; n++)
|
||||||
// unsigned int k;
|
{
|
||||||
// const __m256 eights = _mm256_set1_ps(8.0f);
|
// resample code for first tap
|
||||||
// const __m256 rem_code_phase_chips_reg = _mm256_set1_ps(rem_code_phase_chips);
|
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);
|
||||||
// const __m256 code_phase_step_chips_reg = _mm256_set1_ps(code_phase_step_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);
|
||||||
// __VOLK_ATTR_ALIGNED(32)
|
local_code_chip_index_ = local_code_chip_index_ % code_length_chips;
|
||||||
// int local_code_chip_index[8];
|
_result[0][n] = local_code[local_code_chip_index_];
|
||||||
// int local_code_chip_index_;
|
}
|
||||||
//
|
|
||||||
// const __m256 zeros = _mm256_setzero_ps();
|
// adjacent correlators
|
||||||
// const __m256 code_length_chips_reg_f = _mm256_set1_ps((float)code_length_chips);
|
unsigned int shift_samples = 0;
|
||||||
// const __m256 n0 = _mm256_set_ps(7.0f, 6.0f, 5.0f, 4.0f, 3.0f, 2.0f, 1.0f, 0.0f);
|
for (current_correlator_tap = 1; current_correlator_tap < num_out_vectors; current_correlator_tap++)
|
||||||
//
|
{
|
||||||
// __m256i local_code_chip_index_reg, i;
|
shift_samples += (int)round((shifts_chips[current_correlator_tap] - shifts_chips[current_correlator_tap - 1]) / code_phase_step_chips);
|
||||||
// __m256 aux, aux2, aux3, shifts_chips_reg, c, cTrunc, base, negatives, indexn;
|
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));
|
||||||
// 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);
|
#endif
|
||||||
// indexn = n0;
|
|
||||||
// for (n = 0; n < avx_iters; n++)
|
|
||||||
// {
|
#ifdef LV_HAVE_AVX
|
||||||
// __VOLK_GNSSSDR_PREFETCH_LOCALITY(&_result[current_correlator_tap][8 * n + 7], 1, 0);
|
#include <immintrin.h>
|
||||||
// __VOLK_GNSSSDR_PREFETCH_LOCALITY(&local_code_chip_index[8], 1, 3);
|
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)
|
||||||
// aux = _mm256_mul_ps(code_phase_step_chips_reg, indexn);
|
{
|
||||||
// aux = _mm256_add_ps(aux, aux2);
|
float** _result = result;
|
||||||
// // floor
|
const unsigned int avx_iters = num_points / 8;
|
||||||
// aux = _mm256_floor_ps(aux);
|
int current_correlator_tap;
|
||||||
//
|
unsigned int n;
|
||||||
// // fmod
|
unsigned int k;
|
||||||
// c = _mm256_div_ps(aux, code_length_chips_reg_f);
|
const __m256 eights = _mm256_set1_ps(8.0f);
|
||||||
// i = _mm256_cvttps_epi32(c);
|
const __m256 ones = _mm256_set1_ps(1.0f);
|
||||||
// cTrunc = _mm256_cvtepi32_ps(i);
|
const __m256 rem_code_phase_chips_reg = _mm256_set1_ps(rem_code_phase_chips);
|
||||||
// base = _mm256_mul_ps(cTrunc, code_length_chips_reg_f);
|
const __m256 code_phase_step_chips_reg = _mm256_set1_ps(code_phase_step_chips);
|
||||||
// local_code_chip_index_reg = _mm256_cvttps_epi32(_mm256_sub_ps(aux, base));
|
const __m256 code_phase_rate_step_chips_reg = _mm256_set1_ps(code_phase_rate_step_chips);
|
||||||
//
|
|
||||||
// // no negatives
|
__VOLK_ATTR_ALIGNED(32)
|
||||||
// c = _mm256_cvtepi32_ps(local_code_chip_index_reg);
|
int local_code_chip_index[8];
|
||||||
// negatives = _mm256_cmp_ps(c, zeros, 0x01);
|
int local_code_chip_index_;
|
||||||
// aux3 = _mm256_and_ps(code_length_chips_reg_f, negatives);
|
|
||||||
// aux = _mm256_add_ps(c, aux3);
|
const __m256 zeros = _mm256_setzero_ps();
|
||||||
// local_code_chip_index_reg = _mm256_cvttps_epi32(aux);
|
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);
|
||||||
// _mm256_store_si256((__m256i*)local_code_chip_index, local_code_chip_index_reg);
|
|
||||||
// for (k = 0; k < 8; ++k)
|
__m256i local_code_chip_index_reg, i;
|
||||||
// {
|
__m256 aux, aux2, aux3, shifts_chips_reg, c, cTrunc, base, negatives, indexn, indexnn;
|
||||||
// _result[current_correlator_tap][n * 8 + k] = local_code[local_code_chip_index[k]];
|
|
||||||
// }
|
shifts_chips_reg = _mm256_set1_ps((float)shifts_chips[0]);
|
||||||
// indexn = _mm256_add_ps(indexn, eights);
|
aux2 = _mm256_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg);
|
||||||
// }
|
indexn = n0;
|
||||||
// }
|
for (n = 0; n < avx_iters; n++)
|
||||||
// _mm256_zeroupper();
|
{
|
||||||
// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++)
|
__VOLK_GNSSSDR_PREFETCH_LOCALITY(&_result[0][8 * n + 7], 1, 0);
|
||||||
// {
|
__VOLK_GNSSSDR_PREFETCH_LOCALITY(&local_code_chip_index[8], 1, 3);
|
||||||
// for (n = avx_iters * 8; n < num_points; n++)
|
aux = _mm256_mul_ps(code_phase_step_chips_reg, indexn);
|
||||||
// {
|
indexnn = _mm256_mul_ps(indexn, indexn);
|
||||||
// // resample code for current tap
|
aux3 = _mm256_mul_ps(code_phase_rate_step_chips_reg, indexnn);
|
||||||
// local_code_chip_index_ = (int)floor(code_phase_step_chips * (float)n + shifts_chips[current_correlator_tap] - rem_code_phase_chips);
|
aux = _mm256_add_ps(aux, aux3);
|
||||||
// //Take into account that in multitap correlators, the shifts can be negative!
|
aux = _mm256_add_ps(aux, aux2);
|
||||||
// if (local_code_chip_index_ < 0) local_code_chip_index_ += (int)code_length_chips * (abs(local_code_chip_index_) / code_length_chips + 1);
|
// floor
|
||||||
// local_code_chip_index_ = local_code_chip_index_ % code_length_chips;
|
aux = _mm256_floor_ps(aux);
|
||||||
// _result[current_correlator_tap][n] = local_code[local_code_chip_index_];
|
|
||||||
// }
|
// Correct negative shift
|
||||||
// }
|
c = _mm256_div_ps(aux, code_length_chips_reg_f);
|
||||||
//}
|
aux3 = _mm256_add_ps(c, ones);
|
||||||
//
|
i = _mm256_cvttps_epi32(aux3);
|
||||||
//#endif
|
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));
|
||||||
//#ifdef LV_HAVE_AVX
|
|
||||||
//#include <immintrin.h>
|
c = _mm256_cvtepi32_ps(local_code_chip_index_reg);
|
||||||
//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)
|
negatives = _mm256_cmp_ps(c, zeros, 0x01);
|
||||||
//{
|
aux3 = _mm256_and_ps(code_length_chips_reg_f, negatives);
|
||||||
// float** _result = result;
|
aux = _mm256_add_ps(c, aux3);
|
||||||
// const unsigned int avx_iters = num_points / 8;
|
local_code_chip_index_reg = _mm256_cvttps_epi32(aux);
|
||||||
// int current_correlator_tap;
|
|
||||||
// unsigned int n;
|
_mm256_store_si256((__m256i*)local_code_chip_index, local_code_chip_index_reg);
|
||||||
// unsigned int k;
|
for (k = 0; k < 8; ++k)
|
||||||
// const __m256 eights = _mm256_set1_ps(8.0f);
|
{
|
||||||
// const __m256 rem_code_phase_chips_reg = _mm256_set1_ps(rem_code_phase_chips);
|
_result[0][n * 8 + k] = local_code[local_code_chip_index[k]];
|
||||||
// const __m256 code_phase_step_chips_reg = _mm256_set1_ps(code_phase_step_chips);
|
}
|
||||||
//
|
indexn = _mm256_add_ps(indexn, eights);
|
||||||
// __VOLK_ATTR_ALIGNED(32)
|
}
|
||||||
// int local_code_chip_index[8];
|
|
||||||
// int local_code_chip_index_;
|
_mm256_zeroupper();
|
||||||
//
|
|
||||||
// const __m256 zeros = _mm256_setzero_ps();
|
for (n = avx_iters * 8; n < num_points; n++)
|
||||||
// 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);
|
// 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);
|
||||||
// __m256i local_code_chip_index_reg, i;
|
// Take into account that in multitap correlators, the shifts can be negative!
|
||||||
// __m256 aux, aux2, aux3, shifts_chips_reg, c, cTrunc, base, negatives, indexn;
|
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;
|
||||||
// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++)
|
_result[0][n] = local_code[local_code_chip_index_];
|
||||||
// {
|
}
|
||||||
// shifts_chips_reg = _mm256_set1_ps((float)shifts_chips[current_correlator_tap]);
|
|
||||||
// aux2 = _mm256_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg);
|
// adjacent correlators
|
||||||
// indexn = n0;
|
unsigned int shift_samples = 0;
|
||||||
// for (n = 0; n < avx_iters; n++)
|
for (current_correlator_tap = 1; current_correlator_tap < num_out_vectors; current_correlator_tap++)
|
||||||
// {
|
{
|
||||||
// __VOLK_GNSSSDR_PREFETCH_LOCALITY(&_result[current_correlator_tap][8 * n + 7], 1, 0);
|
shift_samples += (int)round((shifts_chips[current_correlator_tap] - shifts_chips[current_correlator_tap - 1]) / code_phase_step_chips);
|
||||||
// __VOLK_GNSSSDR_PREFETCH_LOCALITY(&local_code_chip_index[8], 1, 3);
|
memcpy(&_result[current_correlator_tap][0], &_result[0][shift_samples], (num_points - shift_samples) * sizeof(float));
|
||||||
// aux = _mm256_mul_ps(code_phase_step_chips_reg, indexn);
|
memcpy(&_result[current_correlator_tap][num_points - shift_samples], &_result[0][0], shift_samples * sizeof(float));
|
||||||
// aux = _mm256_add_ps(aux, aux2);
|
}
|
||||||
// // floor
|
}
|
||||||
// aux = _mm256_floor_ps(aux);
|
|
||||||
//
|
#endif
|
||||||
// // fmod
|
|
||||||
// c = _mm256_div_ps(aux, code_length_chips_reg_f);
|
|
||||||
// i = _mm256_cvttps_epi32(c);
|
#ifdef LV_HAVE_AVX
|
||||||
// cTrunc = _mm256_cvtepi32_ps(i);
|
#include <immintrin.h>
|
||||||
// base = _mm256_mul_ps(cTrunc, code_length_chips_reg_f);
|
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)
|
||||||
// local_code_chip_index_reg = _mm256_cvttps_epi32(_mm256_sub_ps(aux, base));
|
{
|
||||||
//
|
float** _result = result;
|
||||||
// // no negatives
|
const unsigned int avx_iters = num_points / 8;
|
||||||
// c = _mm256_cvtepi32_ps(local_code_chip_index_reg);
|
int current_correlator_tap;
|
||||||
// negatives = _mm256_cmp_ps(c, zeros, 0x01);
|
unsigned int n;
|
||||||
// aux3 = _mm256_and_ps(code_length_chips_reg_f, negatives);
|
unsigned int k;
|
||||||
// aux = _mm256_add_ps(c, aux3);
|
const __m256 eights = _mm256_set1_ps(8.0f);
|
||||||
// local_code_chip_index_reg = _mm256_cvttps_epi32(aux);
|
const __m256 ones = _mm256_set1_ps(1.0f);
|
||||||
//
|
const __m256 rem_code_phase_chips_reg = _mm256_set1_ps(rem_code_phase_chips);
|
||||||
// _mm256_store_si256((__m256i*)local_code_chip_index, local_code_chip_index_reg);
|
const __m256 code_phase_step_chips_reg = _mm256_set1_ps(code_phase_step_chips);
|
||||||
// for (k = 0; k < 8; ++k)
|
const __m256 code_phase_rate_step_chips_reg = _mm256_set1_ps(code_phase_rate_step_chips);
|
||||||
// {
|
|
||||||
// _result[current_correlator_tap][n * 8 + k] = local_code[local_code_chip_index[k]];
|
__VOLK_ATTR_ALIGNED(32)
|
||||||
// }
|
int local_code_chip_index[8];
|
||||||
// indexn = _mm256_add_ps(indexn, eights);
|
int local_code_chip_index_;
|
||||||
// }
|
|
||||||
// }
|
const __m256 zeros = _mm256_setzero_ps();
|
||||||
// _mm256_zeroupper();
|
const __m256 code_length_chips_reg_f = _mm256_set1_ps((float)code_length_chips);
|
||||||
// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++)
|
const __m256 n0 = _mm256_set_ps(7.0f, 6.0f, 5.0f, 4.0f, 3.0f, 2.0f, 1.0f, 0.0f);
|
||||||
// {
|
|
||||||
// for (n = avx_iters * 8; n < num_points; n++)
|
__m256i local_code_chip_index_reg, i;
|
||||||
// {
|
__m256 aux, aux2, aux3, shifts_chips_reg, c, cTrunc, base, negatives, indexn, indexnn;
|
||||||
// // 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);
|
shifts_chips_reg = _mm256_set1_ps((float)shifts_chips[0]);
|
||||||
// //Take into account that in multitap correlators, the shifts can be negative!
|
aux2 = _mm256_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg);
|
||||||
// if (local_code_chip_index_ < 0) local_code_chip_index_ += (int)code_length_chips * (abs(local_code_chip_index_) / code_length_chips + 1);
|
indexn = n0;
|
||||||
// local_code_chip_index_ = local_code_chip_index_ % code_length_chips;
|
for (n = 0; n < avx_iters; n++)
|
||||||
// _result[current_correlator_tap][n] = local_code[local_code_chip_index_];
|
{
|
||||||
// }
|
__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);
|
||||||
//#endif
|
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
|
//#ifdef LV_HAVE_NEONV7
|
||||||
|
|||||||
@ -184,6 +184,7 @@ acquisition_dump_reader::acquisition_dump_reader(const std::string& basename,
|
|||||||
d_doppler_step = doppler_step_;
|
d_doppler_step = doppler_step_;
|
||||||
d_samples_per_code = samples_per_code_;
|
d_samples_per_code = samples_per_code_;
|
||||||
d_num_doppler_bins = 0;
|
d_num_doppler_bins = 0;
|
||||||
|
num_dwells = 0;
|
||||||
|
|
||||||
acquisition_dump_reader(basename,
|
acquisition_dump_reader(basename,
|
||||||
sat_,
|
sat_,
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user