Add DC removing to signal power measurement.

openair1/PHY/TOOLS/signal_energy.c

@@ -29,6 +29,7 @@
 
 #define shift 4
 //#define shift_DC 0
+#define SHRT_MIN -32768
 
 #if defined(__x86_64__) || defined(__i386__)
 #ifdef LOCALIZATION
@@ -67,110 +68,171 @@ int32_t subcarrier_energy(int32_t *input,uint32_t length, int32_t *subcarrier_en
 }
 #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<length>>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);
+
+//-----------------------------------------------------------------
+// Average Power calculation with DC removing
+//-----------------------------------------------------------------
 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<length>>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
+              uint32_t i;
+              int32_t temp;
+              __m128i in, in_clp, i16_min, coe1;
+              __m128 num0, num1, num2, num3, recp1;
+
+              //init
+              num0 = _mm_setzero_ps();
+              num1 = _mm_setzero_ps();
+              i16_min = _mm_set1_epi16(SHRT_MIN);
+              coe1 = _mm_set1_epi16(1);
+              recp1 = _mm_rcp_ps(_mm_cvtepi32_ps(_mm_set1_epi32(length)));
+
+              //Acc
+              for (i = 0; i < (length >> 2); i++) {
+                            in = _mm_loadu_si128((__m128i *)input);
+                            in_clp = _mm_subs_epi16(in, _mm_cmpeq_epi16(in, i16_min));//if in=SHRT_MIN in+1, else in
+                            num0 = _mm_add_ps(num0, _mm_cvtepi32_ps(_mm_madd_epi16(in_clp, in_clp)));
+                            num1 = _mm_add_ps(num1, _mm_cvtepi32_ps(_mm_madd_epi16(in, coe1)));//DC
+                            input += 4;
   }
+              //Ave
+              num2 = _mm_dp_ps(num0, recp1, 0xFF);//AC power
+              num3 = _mm_dp_ps(num1, recp1, 0xFF);//DC
+              num3 = _mm_mul_ps(num3, num3);      //DC power
+              //remove DC
+              temp = _mm_cvtsi128_si32(_mm_cvttps_epi32(_mm_sub_ps(num2, num3)));
 
-  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
+              return temp;
 
-
-  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;
+//  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<length>>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);
 
-#ifdef MAIN
-  int16_t *printb;
-#endif
-
-  mm0 = _mm_setzero_si64();//_pxor(mm0,mm0);
-  //  mm3 = _mm_setzero_si64();//pxor(mm3,mm3);
 
-  for (i=0; i<length>>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]);
+  int32_t i;
+  int32_t temp;
 
+  __m128i in;
+  __m128  mm0;
 
+//init
+  mm0 = _mm_setzero_ps();
+//Acc
+  for (i=0; i<(length>>2); i++) {
+      in = _mm_loadu_si128((__m128i *)input);
+      mm0 = _mm_add_ps(mm0,_mm_cvtepi32_ps(_mm_madd_epi16(in,in)));
+      input += 4;
   }
+  //Ave
+  temp = (int)((((float*)&mm0)[0] +
+                 ((float*)&mm0)[1] +
+                 ((float*)&mm0)[2] +
+                 ((float*)&mm0)[3])/(float)length);
 
-  /*
-  #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);
+  return temp;
 
-  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__)