diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_resampler_16ic.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_resampler_16ic.h index 0f3a8160a..5afd62f61 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_resampler_16ic.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_resampler_16ic.h @@ -233,4 +233,88 @@ static inline void volk_gnsssdr_16ic_resampler_16ic_u_sse2(lv_16sc_t* result, co #endif /* LV_HAVE_SSE2 */ +#ifdef LV_HAVE_NEON +#include +static inline void volk_gnsssdr_16ic_resampler_16ic_neon(lv_16sc_t* result, const lv_16sc_t* local_code, float rem_code_phase_chips, float code_phase_step_chips, int code_length_chips, unsigned int num_output_samples)//, int* scratch_buffer, float* scratch_buffer_float) +{ + unsigned int number; + const unsigned int quarterPoints = num_output_samples / 4; + float32x4_t half = vdupq_n_f32(0.5f); + + lv_16sc_t* _result = result; + + __attribute__((aligned(16))) int local_code_chip_index[4]; + float32x4_t _rem_code_phase, _code_phase_step_chips; + int32x4_t _code_length_chips, _code_length_chips_minus1; + float32x4_t _code_phase_out, _code_phase_out_with_offset; + rem_code_phase_chips = rem_code_phase_chips - 0.5f; + float32x4_t sign, PlusHalf, Round; + + _rem_code_phase = vld1q_dup_f32(&rem_code_phase_chips); //load float to all four float values in m128 register + _code_phase_step_chips = vld1q_dup_f32(&code_phase_step_chips); //load float to all four float values in m128 register + __attribute__((aligned(16))) int four_times_code_length_chips_minus1[4]; + four_times_code_length_chips_minus1[0] = code_length_chips - 1; + four_times_code_length_chips_minus1[1] = code_length_chips - 1; + four_times_code_length_chips_minus1[2] = code_length_chips - 1; + four_times_code_length_chips_minus1[3] = code_length_chips - 1; + + __attribute__((aligned(16))) int four_times_code_length_chips[4]; + four_times_code_length_chips[0] = code_length_chips; + four_times_code_length_chips[1] = code_length_chips; + four_times_code_length_chips[2] = code_length_chips; + four_times_code_length_chips[3] = code_length_chips; + + _code_length_chips = vld1q_s32((int32_t*)&four_times_code_length_chips); //load float to all four float values in m128 register + _code_length_chips_minus1 = vld1q_s32((int32_t*)&four_times_code_length_chips_minus1); //load float to all four float values in m128 register + + int32x4_t _code_phase_out_int, _code_phase_out_int_neg, _code_phase_out_int_over; + uint32x4_t negative_indexes, overflow_indexes; + int32x4_t zero = vmovq_n_s32(0); + + __attribute__((aligned(16))) float init_idx_float[4] = { 0.0f, 1.0f, 2.0f, 3.0f }; + float32x4_t _4output_index = vld1q_f32(init_idx_float); + __attribute__((aligned(16))) float init_4constant_float[4] = { 4.0f, 4.0f, 4.0f, 4.0f }; + float32x4_t _4constant_float = vld1q_f32(init_4constant_float); + + + for(number = 0; number < quarterPoints; number++) + { + _code_phase_out = vmulq_f32(_code_phase_step_chips, _4output_index); //compute the code phase point with the phase step + _code_phase_out_with_offset = vaddq_f32(_code_phase_out, _rem_code_phase); //add the phase offset + //_code_phase_out_int = _mm_cvtps_epi32(_code_phase_out_with_offset); //convert to integer int32x4_t = f(float32x4_t) + sign = vcvtq_f32_u32((vshrq_n_u32(vreinterpretq_u32_f32(_code_phase_out_with_offset), 31))); + PlusHalf = vaddq_f32(_code_phase_out_with_offset, half); + Round = vsubq_f32(PlusHalf, sign); + _code_phase_out_int = vcvtq_s32_f32(Round); + + negative_indexes = vcltq_s32(_code_phase_out_int, zero); //test for negative values + _code_phase_out_int_neg = vaddq_s32(_code_phase_out_int, _code_length_chips); //the negative values branch + _code_phase_out_int_neg = veorq_s32(_code_phase_out_int, vandq_s32( (int32x4_t)negative_indexes, veorq_s32( _code_phase_out_int_neg, _code_phase_out_int ))); + + overflow_indexes = vcgtq_s32(_code_phase_out_int_neg, _code_length_chips_minus1); //test for overflow values + _code_phase_out_int_over = vsubq_s32(_code_phase_out_int_neg, _code_length_chips); //the negative values branch + _code_phase_out_int_over = veorq_s32(_code_phase_out_int_neg, vandq_s32( (int32x4_t)overflow_indexes, veorq_s32( _code_phase_out_int_over, _code_phase_out_int_neg ))); + + vst1q_s32((int32_t*)local_code_chip_index, _code_phase_out_int_over); // Store the results back + + //todo: optimize the local code lookup table with intrinsics, if possible + *_result++ = local_code[local_code_chip_index[0]]; + *_result++ = local_code[local_code_chip_index[1]]; + *_result++ = local_code[local_code_chip_index[2]]; + *_result++ = local_code[local_code_chip_index[3]]; + + _4output_index = vaddq_f32(_4output_index, _4constant_float); + } + + for(number = quarterPoints * 4; number < num_output_samples; number++) + { + local_code_chip_index[0] = (int)(code_phase_step_chips * (float)number + rem_code_phase_chips + 0.5f); + if (local_code_chip_index[0] < 0.0) local_code_chip_index[0] += code_length_chips - 1; + if (local_code_chip_index[0] > (code_length_chips - 1)) local_code_chip_index[0] -= code_length_chips; + *_result++ = local_code[local_code_chip_index[0]]; + } +} + +#endif /* LV_HAVE_NEON */ + #endif /*INCLUDED_volk_gnsssdr_16ic_resampler_16ic_H*/ diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_resamplerpuppet_16ic.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_resamplerpuppet_16ic.h index 03d45e4e5..9e74960af 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_resamplerpuppet_16ic.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_resamplerpuppet_16ic.h @@ -72,4 +72,16 @@ static inline void volk_gnsssdr_16ic_resamplerpuppet_16ic_u_sse2(lv_16sc_t* resu #endif /* LV_HAVE_SSE2 */ +#ifdef LV_HAVE_NEON + +static inline void volk_gnsssdr_16ic_resamplerpuppet_16ic_neon(lv_16sc_t* result, const lv_16sc_t* local_code, unsigned int num_points) +{ + float rem_code_phase_chips = -0.123; + float code_phase_step_chips = 0.1; + int code_length_chips = 1023; + volk_gnsssdr_16ic_resampler_16ic_neon(result, local_code, rem_code_phase_chips, code_phase_step_chips, code_length_chips, num_points ); +} + +#endif /* LV_HAVE_NEON */ + #endif // INCLUDED_volk_gnsssdr_16ic_resamplerpuppet_16ic_H