/*******************************************************************************
OpenAirInterface
Copyright(c) 1999 - 2014 Eurecom
OpenAirInterface 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.
OpenAirInterface 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 OpenAirInterface.The full GNU General Public License is
included in this distribution in the file called "COPYING". If not,
see .
Contact Information
OpenAirInterface Admin: openair_admin@eurecom.fr
OpenAirInterface Tech : openair_tech@eurecom.fr
OpenAirInterface Dev : openair4g-devel@lists.eurecom.fr
Address : Eurecom, Campus SophiaTech, 450 Route des Chappes, CS 50193 - 06904 Biot Sophia Antipolis cedex, FRANCE
*******************************************************************************/
#include "defs.h"
#include "PHY/sse_intrin.h"
// Compute Energy of a complex signal vector, removing the DC component!
// input : points to vector
// length : length of vector in complex samples
#define shift 4
//#define shift_DC 0
#if defined(__x86_64__) || defined(__i386__)
#ifdef LOCALIZATION
int32_t subcarrier_energy(int32_t *input,uint32_t length, int32_t *subcarrier_energy, uint16_t rx_power_correction)
{
int32_t i, subcarrier_pwr;
register __m64 mm0,mm1, subcarrier;
subcarrier = _mm_setzero_si64();//_m_pxor(subcarrier,subcarrier);
__m64 *in = (__m64 *)input;
#ifdef MAIN
int16_t *printb;
#endif
mm0 = _mm_setzero_si64();//pxor(mm0,mm0);
for (i=0; i>1; i++) {
mm1 = in[i];
mm1 = _m_pmaddwd(mm1,mm1);
mm1 = _m_psradi(mm1,shift);// shift any 32 bits blocs of the word by the value shift
subcarrier = mm1;
subcarrier = _m_psrlqi(subcarrier,32);
subcarrier = _m_paddd(subcarrier,mm1);
subcarrier_pwr = _m_to_int(subcarrier);
subcarrier_pwr<<=shift;
subcarrier_pwr = (unsigned short) dB_fixed(subcarrier_pwr);
subcarrier_energy[i] = subcarrier_pwr*rx_power_correction;
}
_mm_empty();
_m_empty();
return i;
}
#endif
int32_t signal_energy(int32_t *input,uint32_t length)
{
int32_t i;
int32_t temp,temp2;
register __m64 mm0,mm1,mm2,mm3;
__m64 *in = (__m64 *)input;
mm0 = _mm_setzero_si64();//pxor(mm0,mm0);
mm3 = _mm_setzero_si64();//pxor(mm3,mm3);
for (i=0; i>1; i++) {
mm1 = in[i];
mm2 = mm1;
mm1 = _m_pmaddwd(mm1,mm1);
mm1 = _m_psradi(mm1,shift);// shift any 32 bits blocs of the word by the value shift
mm0 = _m_paddd(mm0,mm1);// add the two 64 bits words 4 bytes by 4 bytes
// mm2 = _m_psrawi(mm2,shift_DC);
mm3 = _m_paddw(mm3,mm2);// add the two 64 bits words 2 bytes by 2 bytes
}
mm1 = mm0;
mm0 = _m_psrlqi(mm0,32);
mm0 = _m_paddd(mm0,mm1);
temp = _m_to_int(mm0);
temp/=length;
temp<<=shift; // this is the average of x^2
// now remove the DC component
mm2 = _m_psrlqi(mm3,32);
mm2 = _m_paddw(mm2,mm3);
mm2 = _m_pmaddwd(mm2,mm2);
temp2 = _m_to_int(mm2);
temp2/=(length*length);
// temp2<<=(2*shift_DC);
temp -= temp2;
_mm_empty();
_m_empty();
return((temp>0)?temp:1);
}
int32_t signal_energy_nodc(int32_t *input,uint32_t length)
{
int32_t i;
int32_t temp;
register __m64 mm0,mm1;//,mm2,mm3;
__m64 *in = (__m64 *)input;
#ifdef MAIN
int16_t *printb;
#endif
mm0 = _mm_setzero_si64();//_pxor(mm0,mm0);
// mm3 = _mm_setzero_si64();//pxor(mm3,mm3);
for (i=0; i>1; i++) {
mm1 = in[i];
mm1 = _m_pmaddwd(mm1,mm1);// SIMD complex multiplication
mm1 = _m_psradi(mm1,shift);
mm0 = _m_paddd(mm0,mm1);
// temp2 = mm0;
// printf("%d %d\n",((int *)&in[i])[0],((int *)&in[i])[1]);
// printb = (int16_t *)&mm2;
// printf("mm2 %d : %d %d %d %d\n",i,printb[0],printb[1],printb[2],printb[3]);
}
/*
#ifdef MAIN
printb = (int16_t *)&mm3;
printf("%d %d %d %d\n",printb[0],printb[1],printb[2],printb[3]);
#endif
*/
mm1 = mm0;
mm0 = _m_psrlqi(mm0,32);
mm0 = _m_paddd(mm0,mm1);
temp = _m_to_int(mm0);
temp/=length;
temp<<=shift; // this is the average of x^2
#ifdef MAIN
printf("E x^2 = %d\n",temp);
#endif
_mm_empty();
_m_empty();
return((temp>0)?temp:1);
}
#elif defined(__arm__)
int32_t signal_energy(int32_t *input,uint32_t length)
{
int32_t i;
int32_t temp,temp2;
register int32x4_t tmpE,tmpDC;
int32x2_t tmpE2,tmpDC2;
int16x4_t *in = (int16x4_t *)input;
tmpE = vdupq_n_s32(0);
tmpDC = vdupq_n_s32(0);
for (i=0; i>1; i++) {
tmpE = vqaddq_s32(tmpE,vshrq_n_s32(vmull_s16(*in,*in),shift));
//tmpDC = vaddw_s16(tmpDC,vshr_n_s16(*in++,shift_DC));
}
tmpE2 = vpadd_s32(vget_low_s32(tmpE),vget_high_s32(tmpE));
temp=(vget_lane_s32(tmpE2,0)+vget_lane_s32(tmpE2,1))/length;
temp<<=shift; // this is the average of x^2
// now remove the DC component
tmpDC2 = vpadd_s32(vget_low_s32(tmpDC),vget_high_s32(tmpDC));
temp2=(vget_lane_s32(tmpDC2,0)+vget_lane_s32(tmpDC2,1))/(length*length);
// temp2<<=(2*shift_DC);
#ifdef MAIN
printf("E x^2 = %d\n",temp);
#endif
temp -= temp2;
#ifdef MAIN
printf("(E x)^2=%d\n",temp2);
#endif
return((temp>0)?temp:1);
}
int32_t signal_energy_nodc(int32_t *input,uint32_t length)
{
int32_t i;
int32_t temp;
register int32x4_t tmpE;
int32x2_t tmpE2;
int16x4_t *in = (int16x4_t *)input;
tmpE = vdupq_n_s32(0);
for (i=0; i>1; i++) {
tmpE = vqaddq_s32(tmpE,vshrq_n_s32(vmull_s16(*in,*in),shift));
}
tmpE2 = vpadd_s32(vget_low_s32(tmpE),vget_high_s32(tmpE));
temp=(vget_lane_s32(tmpE2,0)+vget_lane_s32(tmpE2,1))/length;
temp<<=shift; // this is the average of x^2
#ifdef MAIN
printf("E x^2 = %d\n",temp);
#endif
return((temp>0)?temp:1);
}
#endif
double signal_energy_fp(double **s_re,double **s_im,uint32_t nb_antennas,uint32_t length,uint32_t offset)
{
int32_t aa,i;
double V=0.0;
for (i=0; i %f\n",s[i].x,s[i].y,V);
// V= V + (s[i].y*s[i].x) + (s[i].y*s[i].x);
V= V + (s[i].x*s[i].x) + (s[i].y*s[i].y);
}
return(V/length);
}
#ifdef MAIN
#define LENGTH 256
#include
#include
#include
#include
main(int argc,char **argv)
{
int input[LENGTH];
int energy=0,dc_r=0,dc_i=0;
int16_t s=1,i;
int amp;
amp = atoi(argv[1]);// arguments to integer
if (argc>1)
printf("Amp = %d\n",amp);
for (i=0; i