signal_energy.c 7.41 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21
/*
 * Licensed to the OpenAirInterface (OAI) Software Alliance under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The OpenAirInterface Software Alliance licenses this file to You under
 * the OAI Public License, Version 1.0  (the "License"); you may not use this file
 * except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.openairinterface.org/?page_id=698
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *-------------------------------------------------------------------------------
 * For more information about the OpenAirInterface (OAI) Software Alliance:
 *      contact@openairinterface.org
 */

22 23
#include "defs.h"

24
#include "PHY/sse_intrin.h"
25 26 27 28 29 30

// 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
31
//#define shift_DC 0
32

33
#if defined(__x86_64__) || defined(__i386__)
34
#ifdef LOCALIZATION
35 36
int32_t subcarrier_energy(int32_t *input,uint32_t length, int32_t *subcarrier_energy, uint16_t rx_power_correction)
{
37 38 39

  int32_t i, subcarrier_pwr;
  register __m64 mm0,mm1, subcarrier;
40
  subcarrier = _mm_setzero_si64();//_m_pxor(subcarrier,subcarrier);
41 42 43 44 45 46
  __m64 *in = (__m64 *)input;

#ifdef MAIN
  int16_t *printb;
#endif

47
  mm0 = _mm_setzero_si64();//pxor(mm0,mm0);
48

49 50 51
  for (i=0; i<length>>1; i++) {

    mm1 = in[i];
52 53 54 55 56 57 58 59 60 61
    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;
  }
62

63 64 65
  _mm_empty();
  _m_empty();

66 67 68
  return i;
}
#endif
69

70 71
int32_t signal_energy(int32_t *input,uint32_t length)
{
72

73 74
  int32_t i;
  int32_t temp,temp2;
75 76 77 78
  register __m64 mm0,mm1,mm2,mm3;
  __m64 *in = (__m64 *)input;


79 80
  mm0 = _mm_setzero_si64();//pxor(mm0,mm0);
  mm3 = _mm_setzero_si64();//pxor(mm3,mm3);
81

82 83 84
  for (i=0; i<length>>1; i++) {

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

102 103 104 105 106 107

  mm2 = _m_psrlqi(mm3,32);
  mm2 = _m_paddw(mm2,mm3);
  mm2 = _m_pmaddwd(mm2,mm2);
  temp2 = _m_to_int(mm2);
  temp2/=(length*length);
108
  //  temp2<<=(2*shift_DC);
109 110
  temp -= temp2;

111 112 113
  _mm_empty();
  _m_empty();

114 115 116
  return((temp>0)?temp:1);
}

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

120 121
  int32_t i;
  int32_t temp;
122
  register __m64 mm0,mm1;//,mm2,mm3;
123 124 125
  __m64 *in = (__m64 *)input;

#ifdef MAIN
126
  int16_t *printb;
127 128
#endif

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

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

    mm1 = in[i];
135 136 137 138 139 140 141
    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]);


142
    //    printb = (int16_t *)&mm2;
143 144 145 146 147 148
    //    printf("mm2 %d : %d %d %d %d\n",i,printb[0],printb[1],printb[2],printb[3]);


  }

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



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

176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192
#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));
193
    //tmpDC = vaddw_s16(tmpDC,vshr_n_s16(*in++,shift_DC));
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

  }

  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
knopp's avatar
knopp committed
251
double signal_energy_fp(double *s_re[2],double *s_im[2],uint32_t nb_antennas,uint32_t length,uint32_t offset)
252
{
253

254
  int32_t aa,i;
255 256
  double V=0.0;

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

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

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

269
  int32_t i;
270 271
  double V=0.0;

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

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

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

  int input[LENGTH];
  int energy=0,dc_r=0,dc_i=0;
292
  int16_t s=1,i;
293 294 295
  int amp;

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

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

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


  }
310

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