[hamradio-commits] [gnss-sdr] 185/236: Fixing resamplers
Carles Fernandez
carles_fernandez-guest at moszumanska.debian.org
Tue Apr 26 16:02:51 UTC 2016
This is an automated email from the git hooks/post-receive script.
carles_fernandez-guest pushed a commit to branch next
in repository gnss-sdr.
commit 414eaddb42c9d2da1ce26a7ebaebbd9cee92eddd
Author: Carles Fernandez <carles.fernandez at gmail.com>
Date: Tue Apr 12 01:05:47 2016 +0200
Fixing resamplers
Under some circumstances (i.e. negative Doppler) it could cause a
segmentation fault. It is now fixed for all protokernels.
---
.../volk_gnsssdr_16ic_xn_resampler2_16ic_xn.h | 66 ++++++++++++++--------
.../volk_gnsssdr_32fc_xn_resampler_32fc_xn.h | 66 ++++++++++++++--------
2 files changed, 82 insertions(+), 50 deletions(-)
diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_xn_resampler2_16ic_xn.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_xn_resampler2_16ic_xn.h
index cdd5897..9101172 100644
--- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_xn_resampler2_16ic_xn.h
+++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_xn_resampler2_16ic_xn.h
@@ -64,6 +64,7 @@
#define INCLUDED_volk_gnsssdr_16ic_xn_resampler2_16ic_xn_H
#include <math.h>
+#include <stdlib.h>
#include <volk_gnsssdr/volk_gnsssdr_common.h>
#include <volk_gnsssdr/volk_gnsssdr_complex.h>
@@ -80,7 +81,7 @@ static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_generic(lv_16sc_t** r
// 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 += code_length_chips;
+ 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];
}
@@ -144,7 +145,7 @@ static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_a_sse4_1(lv_16sc_t**
// 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_ += code_length_chips;
+ 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_];
}
@@ -208,7 +209,7 @@ static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_u_sse4_1(lv_16sc_t**
// 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_ += code_length_chips;
+ 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_];
}
@@ -276,7 +277,7 @@ static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_a_sse3(lv_16sc_t** re
// 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_ += code_length_chips;
+ 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_];
}
@@ -344,7 +345,7 @@ static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_u_sse3(lv_16sc_t** re
// 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_ += code_length_chips;
+ 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_];
}
@@ -382,14 +383,13 @@ static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_a_avx(lv_16sc_t** res
indexn = n0;
for(unsigned int n = 0; n < avx_iters; n++)
{
+ __builtin_prefetch(&_result[current_correlator_tap][8 * n + 7], 1, 0);
+ __builtin_prefetch(&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);
- negatives = _mm256_cmp_ps(aux, zeros, 0x01);
- aux3 = _mm256_and_ps(code_length_chips_reg_f, negatives);
- aux = _mm256_add_ps(aux, aux3);
// fmod
c = _mm256_div_ps(aux, code_length_chips_reg_f);
i = _mm256_cvttps_epi32(c);
@@ -397,6 +397,13 @@ static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_a_avx(lv_16sc_t** res
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(unsigned int k = 0; k < 8; ++k)
{
@@ -413,7 +420,7 @@ static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_a_avx(lv_16sc_t** res
// 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_ += code_length_chips;
+ 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_];
}
@@ -428,7 +435,7 @@ static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_a_avx(lv_16sc_t** res
static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_u_avx(lv_16sc_t** result, const lv_16sc_t* 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)
{
lv_16sc_t** _result = result;
- const unsigned int avx_iters = num_points / 8;
+ const unsigned int avx_iters = num_points / 8;
const __m256 eights = _mm256_set1_ps(8.0f);
const __m256 rem_code_phase_chips_reg = _mm256_set1_ps(rem_code_phase_chips);
@@ -451,14 +458,13 @@ static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_u_avx(lv_16sc_t** res
indexn = n0;
for(unsigned int n = 0; n < avx_iters; n++)
{
+ __builtin_prefetch(&_result[current_correlator_tap][8 * n + 7], 1, 0);
+ __builtin_prefetch(&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);
- negatives = _mm256_cmp_ps(aux, zeros, 0x01);
- aux3 = _mm256_and_ps(code_length_chips_reg_f, negatives);
- aux = _mm256_add_ps(aux, aux3);
// fmod
c = _mm256_div_ps(aux, code_length_chips_reg_f);
i = _mm256_cvttps_epi32(c);
@@ -466,6 +472,13 @@ static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_u_avx(lv_16sc_t** res
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(unsigned int k = 0; k < 8; ++k)
{
@@ -482,7 +495,7 @@ static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_u_avx(lv_16sc_t** res
// 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_ += code_length_chips;
+ 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_];
}
@@ -505,37 +518,39 @@ static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_neon(lv_16sc_t** resu
__VOLK_ATTR_ALIGNED(16) int32_t local_code_chip_index[4];
int32_t local_code_chip_index_;
+
const int32x4_t zeros = vdupq_n_s32(0);
const float32x4_t code_length_chips_reg_f = vdupq_n_f32((float)code_length_chips);
-
const int32x4_t code_length_chips_reg_i = vdupq_n_s32((int32_t)code_length_chips);
int32x4_t local_code_chip_index_reg, aux_i, negatives, i;
float32x4_t aux, aux2, shifts_chips_reg, fi, c, j, cTrunc, base, indexn, reciprocal;
__VOLK_ATTR_ALIGNED(16) const float vec[4] = { 0.0f, 1.0f, 2.0f, 3.0f };
uint32x4_t igx;
+ reciprocal = vrecpeq_f32(code_length_chips_reg_f);
+ reciprocal = vmulq_f32(vrecpsq_f32(code_length_chips_reg_f, reciprocal), reciprocal);
+ reciprocal = vmulq_f32(vrecpsq_f32(code_length_chips_reg_f, reciprocal), reciprocal); // this refinement is required!
+ float32x4_t n0 = vld1q_f32((float*)vec);
for (int current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++)
{
shifts_chips_reg = vdupq_n_f32((float)shifts_chips[current_correlator_tap]);
aux2 = vsubq_f32(shifts_chips_reg, rem_code_phase_chips_reg);
- indexn = vld1q_f32((float*)vec);
+ indexn = n0;
for(unsigned int n = 0; n < neon_iters; n++)
{
__builtin_prefetch(&_result[current_correlator_tap][4 * n + 3], 1, 0);
__builtin_prefetch(&local_code_chip_index[4]);
aux = vmulq_f32(code_phase_step_chips_reg, indexn);
aux = vaddq_f32(aux, aux2);
- // floor
+
+ //floor
i = vcvtq_s32_f32(aux);
fi = vcvtq_f32_s32(i);
igx = vcgtq_f32(fi, aux);
- j = vreinterpretq_f32_s32(vandq_s32(vreinterpretq_s32_u32(igx), ones));
+ j = vcvtq_f32_s32(vandq_s32(vreinterpretq_s32_u32(igx), ones));
aux = vsubq_f32(fi, j);
// fmod
- reciprocal = vrecpeq_f32(code_length_chips_reg_f);
- reciprocal = vmulq_f32(vrecpsq_f32(code_length_chips_reg_f, reciprocal), reciprocal);
- reciprocal = vmulq_f32(vrecpsq_f32(code_length_chips_reg_f, reciprocal), reciprocal); // this refinement is required!
c = vmulq_f32(aux, reciprocal);
i = vcvtq_s32_f32(c);
cTrunc = vcvtq_f32_s32(i);
@@ -547,7 +562,8 @@ static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_neon(lv_16sc_t** resu
aux_i = vandq_s32(code_length_chips_reg_i, negatives);
local_code_chip_index_reg = vaddq_s32(local_code_chip_index_reg, aux_i);
- vst1q_s32((int*)local_code_chip_index, local_code_chip_index_reg);
+ vst1q_s32((int32_t*)local_code_chip_index, local_code_chip_index_reg);
+
for(unsigned int k = 0; k < 4; ++k)
{
_result[current_correlator_tap][n * 4 + k] = local_code[local_code_chip_index[k]];
@@ -558,10 +574,10 @@ static inline void volk_gnsssdr_16ic_xn_resampler2_16ic_xn_neon(lv_16sc_t** resu
{
__builtin_prefetch(&_result[current_correlator_tap][n], 1, 0);
// resample code for current tap
- local_code_chip_index_ = (int32_t)floor(code_phase_step_chips * (float)n + shifts_chips[current_correlator_tap] - rem_code_phase_chips);
- local_code_chip_index_ = local_code_chip_index_ % code_length_chips;
+ 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_ += code_length_chips;
+ 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_];
}
}
diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_xn_resampler_32fc_xn.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_xn_resampler_32fc_xn.h
index 8614537..3089c9d 100644
--- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_xn_resampler_32fc_xn.h
+++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_xn_resampler_32fc_xn.h
@@ -64,6 +64,7 @@
#define INCLUDED_volk_gnsssdr_32fc_xn_resampler_32fc_xn_H
#include <math.h>
+#include <stdlib.h> /* abs */
#include <volk_gnsssdr/volk_gnsssdr_common.h>
#include <volk_gnsssdr/volk_gnsssdr_complex.h>
@@ -80,7 +81,7 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_generic(lv_32fc_t** re
// 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 += code_length_chips;
+ 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];
}
@@ -97,8 +98,8 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_a_sse3(lv_32fc_t** res
lv_32fc_t** _result = result;
const unsigned int quarterPoints = num_points / 4;
- const __m128 ones = _mm_set1_ps(1.);
- const __m128 fours = _mm_set1_ps(4.);
+ 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);
@@ -115,7 +116,7 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_a_sse3(lv_32fc_t** res
{
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., 2., 1., 0.);
+ __m128 indexn = _mm_set_ps(3.0f, 2.0f, 1.0f, 0.0f);
for(unsigned int n = 0; n < quarterPoints; n++)
{
aux = _mm_mul_ps(code_phase_step_chips_reg, indexn);
@@ -126,10 +127,9 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_a_sse3(lv_32fc_t** res
igx = _mm_cmpgt_ps(fi, aux);
j = _mm_and_ps(igx, ones);
aux = _mm_sub_ps(fi, j);
-
// fmod
c = _mm_div_ps(aux, code_length_chips_reg_f);
- i = _mm_cvtps_epi32(c);
+ 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));
@@ -149,7 +149,7 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_a_sse3(lv_32fc_t** res
// 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_ += code_length_chips;
+ 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_];
}
@@ -217,7 +217,7 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_u_sse3(lv_32fc_t** res
// 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_ += code_length_chips;
+ 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_];
}
@@ -280,7 +280,7 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_a_sse4_1(lv_32fc_t** r
// 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_ += code_length_chips;
+ 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_];
}
@@ -344,7 +344,7 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_u_sse4_1(lv_32fc_t** r
// 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_ += code_length_chips;
+ 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_];
}
@@ -382,14 +382,13 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_a_avx(lv_32fc_t** resu
indexn = n0;
for(unsigned int n = 0; n < avx_iters; n++)
{
+ __builtin_prefetch(&_result[current_correlator_tap][8 * n + 7], 1, 0);
+ __builtin_prefetch(&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);
- negatives = _mm256_cmp_ps(aux, zeros, 0x01);
- aux3 = _mm256_and_ps(code_length_chips_reg_f, negatives);
- aux = _mm256_add_ps(aux, aux3);
// fmod
c = _mm256_div_ps(aux, code_length_chips_reg_f);
i = _mm256_cvttps_epi32(c);
@@ -397,6 +396,13 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_a_avx(lv_32fc_t** resu
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(unsigned int k = 0; k < 8; ++k)
{
@@ -413,7 +419,7 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_a_avx(lv_32fc_t** resu
// 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_ += code_length_chips;
+ 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_];
}
@@ -451,14 +457,13 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_u_avx(lv_32fc_t** resu
indexn = n0;
for(unsigned int n = 0; n < avx_iters; n++)
{
+ __builtin_prefetch(&_result[current_correlator_tap][8 * n + 7], 1, 0);
+ __builtin_prefetch(&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);
- negatives = _mm256_cmp_ps(aux, zeros, 0x01);
- aux3 = _mm256_and_ps(code_length_chips_reg_f, negatives);
- aux = _mm256_add_ps(aux, aux3);
// fmod
c = _mm256_div_ps(aux, code_length_chips_reg_f);
i = _mm256_cvttps_epi32(c);
@@ -466,6 +471,13 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_u_avx(lv_32fc_t** resu
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(unsigned int k = 0; k < 8; ++k)
{
@@ -482,7 +494,7 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_u_avx(lv_32fc_t** resu
// 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_ += code_length_chips;
+ 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_];
}
@@ -510,31 +522,35 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_neon(lv_32fc_t** resul
const int32x4_t zeros = vdupq_n_s32(0);
const float32x4_t code_length_chips_reg_f = vdupq_n_f32((float)code_length_chips);
const int32x4_t code_length_chips_reg_i = vdupq_n_s32((int32_t)code_length_chips);
- int32x4_t local_code_chip_index_reg, aux_i, negatives, i;
+ int32x4_t local_code_chip_index_reg, aux_i, negatives, i;
float32x4_t aux, aux2, shifts_chips_reg, fi, c, j, cTrunc, base, indexn, reciprocal;
__VOLK_ATTR_ALIGNED(16) const float vec[4] = { 0.0f, 1.0f, 2.0f, 3.0f };
uint32x4_t igx;
+ reciprocal = vrecpeq_f32(code_length_chips_reg_f);
+ reciprocal = vmulq_f32(vrecpsq_f32(code_length_chips_reg_f, reciprocal), reciprocal);
+ reciprocal = vmulq_f32(vrecpsq_f32(code_length_chips_reg_f, reciprocal), reciprocal); // this refinement is required!
+ float32x4_t n0 = vld1q_f32((float*)vec);
+
for (int current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++)
{
shifts_chips_reg = vdupq_n_f32((float)shifts_chips[current_correlator_tap]);
aux2 = vsubq_f32(shifts_chips_reg, rem_code_phase_chips_reg);
- indexn = vld1q_f32((float*)vec);
+ indexn = n0;
for(unsigned int n = 0; n < neon_iters; n++)
{
__builtin_prefetch(&_result[current_correlator_tap][4 * n + 3], 1, 0);
__builtin_prefetch(&local_code_chip_index[4]);
aux = vmulq_f32(code_phase_step_chips_reg, indexn);
aux = vaddq_f32(aux, aux2);
- // floor
+
+ //floor
i = vcvtq_s32_f32(aux);
fi = vcvtq_f32_s32(i);
igx = vcgtq_f32(fi, aux);
j = vcvtq_f32_s32(vandq_s32(vreinterpretq_s32_u32(igx), ones));
aux = vsubq_f32(fi, j);
+
// fmod
- reciprocal = vrecpeq_f32(code_length_chips_reg_f);
- reciprocal = vmulq_f32(vrecpsq_f32(code_length_chips_reg_f, reciprocal), reciprocal);
- reciprocal = vmulq_f32(vrecpsq_f32(code_length_chips_reg_f, reciprocal), reciprocal); // this refinement is required!
c = vmulq_f32(aux, reciprocal);
i = vcvtq_s32_f32(c);
cTrunc = vcvtq_f32_s32(i);
@@ -560,7 +576,7 @@ static inline void volk_gnsssdr_32fc_xn_resampler_32fc_xn_neon(lv_32fc_t** resul
// 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_ += code_length_chips;
+ 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_];
}
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/pkg-hamradio/gnss-sdr.git
More information about the pkg-hamradio-commits
mailing list