signal_energy.c 7.64 KB
Newer Older
ghaddab's avatar
ghaddab committed
1
/*******************************************************************************
2
    OpenAirInterface
ghaddab's avatar
ghaddab committed
3 4 5 6 7 8 9 10 11 12 13 14 15 16
    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
17 18
    along with OpenAirInterface.The full GNU General Public License is
   included in this distribution in the file called "COPYING". If not,
ghaddab's avatar
ghaddab committed
19 20 21 22 23
   see <http://www.gnu.org/licenses/>.

  Contact Information
  OpenAirInterface Admin: openair_admin@eurecom.fr
  OpenAirInterface Tech : openair_tech@eurecom.fr
24
  OpenAirInterface Dev  : openair4g-devel@lists.eurecom.fr
25

ghaddab's avatar
ghaddab committed
26
  Address      : Eurecom, Campus SophiaTech, 450 Route des Chappes, CS 50193 - 06904 Biot Sophia Antipolis cedex, FRANCE
ghaddab's avatar
ghaddab committed
27 28

 *******************************************************************************/
29 30
#include "defs.h"

31
#include "PHY/sse_intrin.h"
32 33 34 35 36 37

// 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
38
//#define shift_DC 0
39

40
#if defined(__x86_64__) || defined(__i386__)
41
#ifdef LOCALIZATION
42 43
int32_t subcarrier_energy(int32_t *input,uint32_t length, int32_t *subcarrier_energy, uint16_t rx_power_correction)
{
44 45 46

  int32_t i, subcarrier_pwr;
  register __m64 mm0,mm1, subcarrier;
47
  subcarrier = _mm_setzero_si64();//_m_pxor(subcarrier,subcarrier);
48 49 50 51 52 53
  __m64 *in = (__m64 *)input;

#ifdef MAIN
  int16_t *printb;
#endif

54
  mm0 = _mm_setzero_si64();//pxor(mm0,mm0);
55

56 57 58
  for (i=0; i<length>>1; i++) {

    mm1 = in[i];
59 60 61 62 63 64 65 66 67 68
    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;
  }
69

roux's avatar
roux committed
70 71 72
  _mm_empty();
  _m_empty();

73 74 75
  return i;
}
#endif
76

77 78
int32_t signal_energy(int32_t *input,uint32_t length)
{
79

gauthier's avatar
gauthier committed
80 81
  int32_t i;
  int32_t temp,temp2;
82 83 84 85
  register __m64 mm0,mm1,mm2,mm3;
  __m64 *in = (__m64 *)input;


86 87
  mm0 = _mm_setzero_si64();//pxor(mm0,mm0);
  mm3 = _mm_setzero_si64();//pxor(mm3,mm3);
88

89 90 91
  for (i=0; i<length>>1; i++) {

    mm1 = in[i];
92 93 94 95
    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
96
    //    mm2 = _m_psrawi(mm2,shift_DC);
97 98 99 100 101 102 103 104 105 106 107
    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
108

109 110 111 112 113 114

  mm2 = _m_psrlqi(mm3,32);
  mm2 = _m_paddw(mm2,mm3);
  mm2 = _m_pmaddwd(mm2,mm2);
  temp2 = _m_to_int(mm2);
  temp2/=(length*length);
115
  //  temp2<<=(2*shift_DC);
116 117
  temp -= temp2;

roux's avatar
roux committed
118 119 120
  _mm_empty();
  _m_empty();

121 122 123
  return((temp>0)?temp:1);
}

124 125
int32_t signal_energy_nodc(int32_t *input,uint32_t length)
{
126

gauthier's avatar
gauthier committed
127 128
  int32_t i;
  int32_t temp;
129
  register __m64 mm0,mm1;//,mm2,mm3;
130 131 132
  __m64 *in = (__m64 *)input;

#ifdef MAIN
gauthier's avatar
gauthier committed
133
  int16_t *printb;
134 135
#endif

136 137
  mm0 = _mm_setzero_si64();//_pxor(mm0,mm0);
  //  mm3 = _mm_setzero_si64();//pxor(mm3,mm3);
138

139 140 141
  for (i=0; i<length>>1; i++) {

    mm1 = in[i];
142 143 144 145 146 147 148
    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]);


gauthier's avatar
gauthier committed
149
    //    printb = (int16_t *)&mm2;
150 151 152 153 154 155
    //    printf("mm2 %d : %d %d %d %d\n",i,printb[0],printb[1],printb[2],printb[3]);


  }

  /*
156
  #ifdef MAIN
gauthier's avatar
gauthier committed
157
  printb = (int16_t *)&mm3;
158
  printf("%d %d %d %d\n",printb[0],printb[1],printb[2],printb[3]);
159
  #endif
160 161 162 163 164 165 166 167 168 169 170 171 172
  */
  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
173
  printf("E x^2 = %d\n",temp);
174 175 176 177 178 179 180 181 182
#endif
  _mm_empty();
  _m_empty();



  return((temp>0)?temp:1);
}

183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257
#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<length>>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<length>>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
258 259
double signal_energy_fp(double **s_re,double **s_im,uint32_t nb_antennas,uint32_t length,uint32_t offset)
{
260

gauthier's avatar
gauthier committed
261
  int32_t aa,i;
262 263
  double V=0.0;

264 265 266
  for (i=0; i<length; i++) {
    for (aa=0; aa<nb_antennas; aa++) {
      V= V + (s_re[aa][i+offset]*s_re[aa][i+offset]) + (s_im[aa][i+offset]*s_im[aa][i+offset]);
267 268
    }
  }
269

270 271 272
  return(V/length/nb_antennas);
}

273 274
double signal_energy_fp2(struct complex *s,uint32_t length)
{
275

gauthier's avatar
gauthier committed
276
  int32_t i;
277 278
  double V=0.0;

279
  for (i=0; i<length; i++) {
280
    //    printf("signal_energy_fp2 : %f,%f => %f\n",s[i].x,s[i].y,V);
281 282 283 284
    //      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);
  }

285 286 287 288 289 290 291 292 293
  return(V/length);
}

#ifdef MAIN
#define LENGTH 256
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
294 295
main(int argc,char **argv)
{
296 297 298

  int input[LENGTH];
  int energy=0,dc_r=0,dc_i=0;
gauthier's avatar
gauthier committed
299
  int16_t s=1,i;
300 301 302
  int amp;

  amp = atoi(argv[1]);// arguments to integer
303

304 305 306
  if (argc>1)
    printf("Amp = %d\n",amp);

307
  for (i=0; i<LENGTH; i++) {
308
    s = -s;
gauthier's avatar
gauthier committed
309 310 311 312 313
    ((int16_t*)input)[2*i]     = 31 + (int16_t)(amp*sin(2*M_PI*i/LENGTH));
    ((int16_t*)input)[1+(2*i)] = 30 + (int16_t)(amp*cos(2*M_PI*i/LENGTH));
    energy += (((int16_t*)input)[2*i]*((int16_t*)input)[2*i]) + (((int16_t*)input)[1+(2*i)]*((int16_t*)input)[1+(2*i)]);
    dc_r += ((int16_t*)input)[2*i];
    dc_i += ((int16_t*)input)[1+(2*i)];
314 315 316


  }
317

318 319 320 321 322 323 324 325 326
  energy/=LENGTH;
  dc_r/=LENGTH;
  dc_i/=LENGTH;

  printf("signal_energy = %d dB(%d,%d,%d,%d)\n",dB_fixed(signal_energy(input,LENGTH)),signal_energy(input,LENGTH),energy-(dc_r*dc_r+dc_i*dc_i),energy,(dc_r*dc_r+dc_i*dc_i));
  printf("dc = (%d,%d)\n",dc_r,dc_i);
}
#endif