From c38a485949181bcfa9cdf26b1dae927416fe9fb1 Mon Sep 17 00:00:00 2001
From: Khalid Ahmed <ahmedkd@iis.fraunhofer.de>
Date: Thu, 11 Apr 2019 12:10:34 +0200
Subject: [PATCH] adding signal_energy_amp_shift function

---
 openair1/PHY/TOOLS/signal_energy.c | 53 +++++++++++++++++++++++++++---
 openair1/PHY/TOOLS/tools_defs.h    |  7 ++++
 openair1/SIMULATION/NR_PHY/ulsim.c |  2 +-
 3 files changed, 57 insertions(+), 5 deletions(-)

diff --git a/openair1/PHY/TOOLS/signal_energy.c b/openair1/PHY/TOOLS/signal_energy.c
index 4b12bb8673..83bcb7b764 100644
--- a/openair1/PHY/TOOLS/signal_energy.c
+++ b/openair1/PHY/TOOLS/signal_energy.c
@@ -20,7 +20,7 @@
  */
 
 #include "tools_defs.h"
-
+#include "PHY/impl_defs_top.h"
 #include "PHY/sse_intrin.h"
 
 // Compute Energy of a complex signal vector, removing the DC component!
@@ -84,7 +84,7 @@ int32_t signal_energy(int32_t *input,uint32_t length)
     mm1 = in[i];
     mm2 = mm1;
     mm1 = _m_pmaddwd(mm1,mm1);
-    mm1 = _m_psradi(mm1,9);//shift);// shift any 32 bits blocs of the word by the value shift
+    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
@@ -95,7 +95,7 @@ int32_t signal_energy(int32_t *input,uint32_t length)
   mm0 = _m_paddd(mm0,mm1);
   temp = _m_to_int(mm0);
   temp/=length;
-  //temp<<=shift;   // this is the average of x^2
+  temp<<=shift;   // this is the average of x^2
 
 
   // now remove the DC component
@@ -104,7 +104,6 @@ int32_t signal_energy(int32_t *input,uint32_t length)
   mm2 = _m_psrlqi(mm3,32);
   mm2 = _m_paddw(mm2,mm3);
   mm2 = _m_pmaddwd(mm2,mm2);
-  mm2 = _m_psradi(mm2,9); // fixed point representation of elements
   temp2 = _m_to_int(mm2);
   temp2/=(length*length);
   //  temp2<<=(2*shift_DC);
@@ -116,6 +115,52 @@ int32_t signal_energy(int32_t *input,uint32_t length)
   return((temp>0)?temp:1);
 }
 
+int32_t signal_energy_amp_shift(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();
+  mm3 = _mm_setzero_si64();
+
+  for (i=0; i<length>>1; i++) {
+
+    mm1 = in[i];
+    mm2 = mm1;
+    mm1 = _m_pmaddwd(mm1,mm1);
+    mm1 = _m_psradi(mm1,AMP_SHIFT);// shift any 32 bits blocs of the word by the value shift_p9
+    mm0 = _m_paddd(mm0,mm1);// add the two 64 bits words 4 bytes by 4 bytes
+    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; // 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);
+  mm2 = _m_psradi(mm2,AMP_SHIFT); // fixed point representation of elements
+  temp2 = _m_to_int(mm2);
+  temp2/=(length*length);
+
+  temp -= temp2;
+
+  _mm_empty();
+  _m_empty();
+
+  return((temp>0)?temp:1);
+}
+
 int32_t signal_energy_nodc(int32_t *input,uint32_t length)
 {
 
diff --git a/openair1/PHY/TOOLS/tools_defs.h b/openair1/PHY/TOOLS/tools_defs.h
index 8ba2ac0d40..3c1a395d0d 100644
--- a/openair1/PHY/TOOLS/tools_defs.h
+++ b/openair1/PHY/TOOLS/tools_defs.h
@@ -295,6 +295,13 @@ void mmxcopy(void *dest,void *src,int size);
 */
 int32_t signal_energy(int32_t *,uint32_t);
 
+/*!\fn int32_t signal_energy_fixed_p9(int *input, uint32_t length);
+\brief Computes the signal energy per subcarrier
+\ the input signal has a fixed point representation of AMP_SHIFT bits
+\ the ouput energy has a fixed point representation of AMP_SHIFT bits
+*/
+int32_t signal_energy_amp_shift(int32_t *input, uint32_t length);
+
 #ifdef LOCALIZATION
 /*!\fn int32_t signal_energy(int *,uint32_t);
 \brief Computes the signal energy per subcarrier
diff --git a/openair1/SIMULATION/NR_PHY/ulsim.c b/openair1/SIMULATION/NR_PHY/ulsim.c
index dabc89d35b..7d31f7ae7c 100644
--- a/openair1/SIMULATION/NR_PHY/ulsim.c
+++ b/openair1/SIMULATION/NR_PHY/ulsim.c
@@ -716,7 +716,7 @@ int main(int argc, char **argv) {
     }
   }
 
-  txlev = (double) signal_energy(&txdata[0][tx_offset + 5*frame_parms->ofdm_symbol_size + 4*frame_parms->nb_prefix_samples + frame_parms->nb_prefix_samples0],
+  txlev = (double) signal_energy_amp_shift(&txdata[0][tx_offset + 5*frame_parms->ofdm_symbol_size + 4*frame_parms->nb_prefix_samples + frame_parms->nb_prefix_samples0],
           frame_parms->ofdm_symbol_size + frame_parms->nb_prefix_samples);
 
   txlev = txlev/512.0; // output of signal_energy is fixed point representation