signal_energy.c 6.81 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 31
#include "defs.h"

#ifndef EXPRESSMIMO_TARGET
32
#include "PHY/sse_intrin.h"
33 34 35 36 37 38 39 40 41 42 43
#endif //EXPRESSMIMO_TARGET

// 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


#ifndef EXPRESSMIMO_TARGET
44
#ifdef LOCALIZATION
45 46
int32_t subcarrier_energy(int32_t *input,uint32_t length, int32_t *subcarrier_energy, uint16_t rx_power_correction)
{
47 48 49

  int32_t i, subcarrier_pwr;
  register __m64 mm0,mm1, subcarrier;
50
  subcarrier = _mm_setzero_si64();//_m_pxor(subcarrier,subcarrier);
51 52 53 54 55 56
  __m64 *in = (__m64 *)input;

#ifdef MAIN
  int16_t *printb;
#endif

57
  mm0 = _mm_setzero_si64();//pxor(mm0,mm0);
58

59 60 61
  for (i=0; i<length>>1; i++) {

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

73 74 75
  return i;
}
#endif
76 77
int32_t signal_energy(int32_t *input,uint32_t length)
{
78

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

#ifdef MAIN
gauthier's avatar
gauthier committed
85
  int16_t *printb;
86 87
#endif

88 89
  mm0 = _mm_setzero_si64();//pxor(mm0,mm0);
  mm3 = _mm_setzero_si64();//pxor(mm3,mm3);
90

91 92 93
  for (i=0; i<length>>1; i++) {

    mm1 = in[i];
94 95 96 97 98 99 100 101
    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
    //    temp2 = mm0;
    //    printf("%d %d\n",((int *)&temp2)[0],((int *)&temp2)[1]);


gauthier's avatar
gauthier committed
102
    //    printb = (int16_t *)&mm2;
103 104 105 106 107
    //    printf("mm2 %d : %d %d %d %d\n",i,printb[0],printb[1],printb[2],printb[3]);

    mm2 = _m_psrawi(mm2,shift_DC);
    mm3 = _m_paddw(mm3,mm2);// add the two 64 bits words 2 bytes by 2 bytes

gauthier's avatar
gauthier committed
108
    //    printb = (int16_t *)&mm3;
109 110 111 112 113
    //    printf("mm3 %d : %d %d %d %d\n",i,printb[0],printb[1],printb[2],printb[3]);

  }

  /*
114
  #ifdef MAIN
gauthier's avatar
gauthier committed
115
  printb = (int16_t *)&mm3;
116
  printf("%d %d %d %d\n",printb[0],printb[1],printb[2],printb[3]);
117
  #endif
118 119 120 121 122 123 124 125 126 127 128 129 130
  */
  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
131

132 133 134 135 136 137 138 139 140 141 142 143

  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);
#ifdef MAIN
144
  printf("E x^2 = %d\n",temp);
145 146 147 148 149 150 151 152 153 154 155 156 157
#endif
  temp -= temp2;
#ifdef MAIN
  printf("(E x)^2=%d\n",temp2);
#endif
  _mm_empty();
  _m_empty();



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

158 159
int32_t signal_energy_nodc(int32_t *input,uint32_t length)
{
160

gauthier's avatar
gauthier committed
161 162
  int32_t i;
  int32_t temp;
163
  register __m64 mm0,mm1;//,mm2,mm3;
164 165 166
  __m64 *in = (__m64 *)input;

#ifdef MAIN
gauthier's avatar
gauthier committed
167
  int16_t *printb;
168 169
#endif

170 171
  mm0 = _mm_setzero_si64();//_pxor(mm0,mm0);
  //  mm3 = _mm_setzero_si64();//pxor(mm3,mm3);
172

173 174 175
  for (i=0; i<length>>1; i++) {

    mm1 = in[i];
176 177 178 179 180 181 182
    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
183
    //    printb = (int16_t *)&mm2;
184 185 186 187 188 189
    //    printf("mm2 %d : %d %d %d %d\n",i,printb[0],printb[1],printb[2],printb[3]);


  }

  /*
190
  #ifdef MAIN
gauthier's avatar
gauthier committed
191
  printb = (int16_t *)&mm3;
192
  printf("%d %d %d %d\n",printb[0],printb[1],printb[2],printb[3]);
193
  #endif
194 195 196 197 198 199 200 201 202 203 204 205 206
  */
  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
207
  printf("E x^2 = %d\n",temp);
208 209 210 211 212 213 214 215 216
#endif
  _mm_empty();
  _m_empty();



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

217 218
double signal_energy_fp(double **s_re,double **s_im,uint32_t nb_antennas,uint32_t length,uint32_t offset)
{
219

gauthier's avatar
gauthier committed
220
  int32_t aa,i;
221 222
  double V=0.0;

223 224 225
  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]);
226 227
    }
  }
228

229 230 231
  return(V/length/nb_antennas);
}

232 233
double signal_energy_fp2(struct complex *s,uint32_t length)
{
234

gauthier's avatar
gauthier committed
235
  int32_t i;
236 237
  double V=0.0;

238
  for (i=0; i<length; i++) {
239
    //    printf("signal_energy_fp2 : %f,%f => %f\n",s[i].x,s[i].y,V);
240 241 242 243
    //      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);
  }

244 245 246 247
  return(V/length);
}
#else

248 249
int32_t signal_energy(int32_t *input,uint32_t length)
{
250 251 252 253 254 255 256 257 258 259
}

#endif

#ifdef MAIN
#define LENGTH 256
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
260 261
main(int argc,char **argv)
{
262 263 264

  int input[LENGTH];
  int energy=0,dc_r=0,dc_i=0;
gauthier's avatar
gauthier committed
265
  int16_t s=1,i;
266 267 268
  int amp;

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

270 271 272
  if (argc>1)
    printf("Amp = %d\n",amp);

273
  for (i=0; i<LENGTH; i++) {
274
    s = -s;
gauthier's avatar
gauthier committed
275 276 277 278 279
    ((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)];
280 281 282


  }
283

284 285 286 287 288 289 290 291 292
  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