signal_energy.c 7.58 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 24
   see <http://www.gnu.org/licenses/>.

  Contact Information
  OpenAirInterface Admin: openair_admin@eurecom.fr
  OpenAirInterface Tech : openair_tech@eurecom.fr
  OpenAirInterface Dev  : openair4g-devel@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

70 71 72
  return i;
}
#endif
73

74 75
int32_t signal_energy(int32_t *input,uint32_t length)
{
76

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


83 84
  mm0 = _mm_setzero_si64();//pxor(mm0,mm0);
  mm3 = _mm_setzero_si64();//pxor(mm3,mm3);
85

86 87 88
  for (i=0; i<length>>1; i++) {

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

106 107 108 109 110 111

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

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

118 119
int32_t signal_energy_nodc(int32_t *input,uint32_t length)
{
120

gauthier's avatar
gauthier committed
121 122
  int32_t i;
  int32_t temp;
123
  register __m64 mm0,mm1;//,mm2,mm3;
124 125 126
  __m64 *in = (__m64 *)input;

#ifdef MAIN
gauthier's avatar
gauthier committed
127
  int16_t *printb;
128 129
#endif

130 131
  mm0 = _mm_setzero_si64();//_pxor(mm0,mm0);
  //  mm3 = _mm_setzero_si64();//pxor(mm3,mm3);
132

133 134 135
  for (i=0; i<length>>1; i++) {

    mm1 = in[i];
136 137 138 139 140 141 142
    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
143
    //    printb = (int16_t *)&mm2;
144 145 146 147 148 149
    //    printf("mm2 %d : %d %d %d %d\n",i,printb[0],printb[1],printb[2],printb[3]);


  }

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



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

177 178 179 180 181 182 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
#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
252 253
double signal_energy_fp(double **s_re,double **s_im,uint32_t nb_antennas,uint32_t length,uint32_t offset)
{
254

gauthier's avatar
gauthier committed
255
  int32_t aa,i;
256 257
  double V=0.0;

258 259 260
  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]);
261 262
    }
  }
263

264 265 266
  return(V/length/nb_antennas);
}

267 268
double signal_energy_fp2(struct complex *s,uint32_t length)
{
269

gauthier's avatar
gauthier committed
270
  int32_t i;
271 272
  double V=0.0;

273
  for (i=0; i<length; i++) {
274
    //    printf("signal_energy_fp2 : %f,%f => %f\n",s[i].x,s[i].y,V);
275 276 277 278
    //      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);
  }

279 280 281 282 283 284 285 286 287
  return(V/length);
}

#ifdef MAIN
#define LENGTH 256
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
288 289
main(int argc,char **argv)
{
290 291 292

  int input[LENGTH];
  int energy=0,dc_r=0,dc_i=0;
gauthier's avatar
gauthier committed
293
  int16_t s=1,i;
294 295 296
  int amp;

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

298 299 300
  if (argc>1)
    printf("Amp = %d\n",amp);

301
  for (i=0; i<LENGTH; i++) {
302
    s = -s;
gauthier's avatar
gauthier committed
303 304 305 306 307
    ((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)];
308 309 310


  }
311

312 313 314 315 316 317 318 319 320
  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