mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-06-17 18:49:57 +00:00
172 lines
5.9 KiB
C++
172 lines
5.9 KiB
C++
/*!
|
|
* \file volk_gnsssdr_16ic_x2_dot_prod_16ic.h
|
|
* \brief Volk protokernel: multiplies two 16 bits vectors and accumulates them
|
|
* \authors <ul>
|
|
* <li> Javier Arribas, 2015. jarribas(at)cttc.es
|
|
* </ul>
|
|
*
|
|
* Volk protokernel that multiplies two 16 bits vectors (8 bits the real part
|
|
* and 8 bits the imaginary part) and accumulates them
|
|
*
|
|
* -------------------------------------------------------------------------
|
|
*
|
|
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
|
|
*
|
|
* GNSS-SDR is a software defined Global Navigation
|
|
* Satellite Systems receiver
|
|
*
|
|
* This file is part of GNSS-SDR.
|
|
*
|
|
* GNSS-SDR is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* GNSS-SDR is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
|
*
|
|
* -------------------------------------------------------------------------
|
|
*/
|
|
|
|
#ifndef INCLUDED_volk_gnsssdr_16ic_xn_dot_prod_16ic_xn_u_H
|
|
#define INCLUDED_volk_gnsssdr_16ic_xn_dot_prod_16ic_xn_u_H
|
|
|
|
|
|
#include <volk_gnsssdr/volk_gnsssdr_complex.h>
|
|
#include "saturated_arithmetic.h"
|
|
|
|
#ifdef LV_HAVE_GENERIC
|
|
/*!
|
|
\brief Multiplies the two input complex vectors and accumulates them, storing the result in the third vector
|
|
\param cVector The vector where the accumulated result will be stored
|
|
\param aVector One of the vectors to be multiplied and accumulated
|
|
\param bVector One of the vectors to be multiplied and accumulated
|
|
\param num_points The number of complex values in aVector and bVector to be multiplied together, accumulated and stored into cVector
|
|
*/
|
|
static inline void volk_gnsssdr_16ic_xn_dot_prod_16ic_xn_generic(lv_16sc_t* result, const lv_16sc_t* in_common, const lv_16sc_t** in_a, unsigned int num_points, int num_a_vectors)
|
|
{
|
|
for (int n_vec=0;n_vec<num_a_vectors;n_vec++)
|
|
{
|
|
result[n_vec]=lv_cmake(0,0);
|
|
for (unsigned int n=0;n<num_points;n++)
|
|
{
|
|
//r*a.r - i*a.i, i*a.r + r*a.i
|
|
//result[n_vec]+=in_common[n]*in_a[n_vec][n];
|
|
lv_16sc_t tmp=in_common[n]*in_a[n_vec][n];
|
|
result[n_vec]=lv_cmake(sat_adds16b(lv_creal(result[n_vec]),lv_creal(tmp)),sat_adds16b(lv_cimag(result[n_vec]),lv_cimag(tmp)));
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
#endif /*LV_HAVE_GENERIC*/
|
|
|
|
|
|
#ifdef LV_HAVE_SSE2
|
|
#include <emmintrin.h>
|
|
static inline void volk_gnsssdr_16ic_xn_dot_prod_16ic_xn_a_sse2(lv_16sc_t* out, const lv_16sc_t* in_common, const lv_16sc_t** in_a, unsigned int num_points, int num_a_vectors)
|
|
{
|
|
|
|
lv_16sc_t dotProduct=lv_cmake(0,0);
|
|
|
|
const unsigned int sse_iters = num_points / 4;
|
|
|
|
const lv_16sc_t** _in_a = in_a;
|
|
const lv_16sc_t* _in_common = in_common;
|
|
lv_16sc_t* _out = out;
|
|
|
|
if (sse_iters>0)
|
|
{
|
|
|
|
__VOLK_ATTR_ALIGNED(16) lv_16sc_t dotProductVector[4];
|
|
|
|
//todo dyn mem reg
|
|
|
|
__m128i* realcacc;
|
|
__m128i* imagcacc;
|
|
|
|
realcacc=(__m128i*)calloc(num_a_vectors,sizeof(__m128i)); //calloc also sets memory to 0
|
|
imagcacc=(__m128i*)calloc(num_a_vectors,sizeof(__m128i)); //calloc also sets memory to 0
|
|
|
|
|
|
__m128i a,b,c, c_sr, mask_imag, mask_real, real, imag, imag1,imag2, b_sl, a_sl, result;
|
|
|
|
mask_imag = _mm_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0);
|
|
mask_real = _mm_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255);
|
|
|
|
for(unsigned int number = 0;number < sse_iters; number++)
|
|
{
|
|
//std::complex<T> memory structure: real part -> reinterpret_cast<cv T*>(a)[2*i]
|
|
//imaginery part -> reinterpret_cast<cv T*>(a)[2*i + 1]
|
|
// a[127:0]=[a3.i,a3.r,a2.i,a2.r,a1.i,a1.r,a0.i,a0.r]
|
|
|
|
b = _mm_loadu_si128((__m128i*)_in_common); //load (2 byte imag, 2 byte real) x 4 into 128 bits reg
|
|
for (int n_vec=0;n_vec<num_a_vectors;n_vec++)
|
|
{
|
|
|
|
a = _mm_loadu_si128((__m128i*)&(_in_a[n_vec][number*4])); //load (2 byte imag, 2 byte real) x 4 into 128 bits reg
|
|
|
|
c=_mm_mullo_epi16 (a, b); // a3.i*b3.i, a3.r*b3.r, ....
|
|
|
|
c_sr = _mm_srli_si128 (c, 2); // Shift a right by imm8 bytes while shifting in zeros, and store the results in dst.
|
|
real = _mm_subs_epi16 (c,c_sr);
|
|
|
|
b_sl = _mm_slli_si128(b,2); // b3.r, b2.i ....
|
|
a_sl = _mm_slli_si128(a,2); // a3.r, a2.i ....
|
|
|
|
imag1 = _mm_mullo_epi16(a,b_sl); // a3.i*b3.r, ....
|
|
imag2 = _mm_mullo_epi16(b,a_sl); // b3.i*a3.r, ....
|
|
|
|
imag = _mm_adds_epi16(imag1,imag2);
|
|
|
|
realcacc[n_vec] = _mm_adds_epi16 (realcacc[n_vec], real);
|
|
imagcacc[n_vec] = _mm_adds_epi16 (imagcacc[n_vec], imag);
|
|
|
|
}
|
|
_in_common += 4;
|
|
|
|
}
|
|
|
|
for (int n_vec=0;n_vec<num_a_vectors;n_vec++)
|
|
{
|
|
realcacc[n_vec] = _mm_and_si128 (realcacc[n_vec], mask_real);
|
|
imagcacc[n_vec] = _mm_and_si128 (imagcacc[n_vec], mask_imag);
|
|
|
|
result = _mm_or_si128 (realcacc[n_vec], imagcacc[n_vec]);
|
|
|
|
_mm_storeu_si128((__m128i*)dotProductVector,result); // Store the results back into the dot product vector
|
|
dotProduct=lv_cmake(0,0);
|
|
for (int i = 0; i<4; ++i)
|
|
{
|
|
|
|
dotProduct=lv_cmake(sat_adds16b(lv_creal(dotProduct),lv_creal(dotProductVector[i])),
|
|
sat_adds16b(lv_cimag(dotProduct),lv_cimag(dotProductVector[i])));
|
|
}
|
|
_out[n_vec]=dotProduct;
|
|
}
|
|
free(realcacc);
|
|
free(imagcacc);
|
|
}
|
|
|
|
for (int n_vec=0;n_vec<num_a_vectors;n_vec++)
|
|
{
|
|
for(unsigned int n = sse_iters * 4;n < num_points; n++){
|
|
|
|
lv_16sc_t tmp=in_common[n]*in_a[n_vec][n];
|
|
|
|
_out[n_vec]=lv_cmake(sat_adds16b(lv_creal(_out[n_vec]),lv_creal(tmp)),
|
|
sat_adds16b(lv_cimag(_out[n_vec]),lv_cimag(tmp)));
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
#endif /* LV_HAVE_SSE2 */
|
|
|
|
#endif /*INCLUDED_volk_gnsssdr_16ic_xn_dot_prod_16ic_xn_u_H*/
|