diff --git a/ci-scripts/conf_files/gnb.band66.tm1.106PRB.usrpn300.conf b/ci-scripts/conf_files/gnb.band66.tm1.106PRB.usrpn300.conf
index 32de0aa4f5..fc32d65a23 100644
--- a/ci-scripts/conf_files/gnb.band66.tm1.106PRB.usrpn300.conf
+++ b/ci-scripts/conf_files/gnb.band66.tm1.106PRB.usrpn300.conf
@@ -185,7 +185,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/ci-scripts/conf_files/gnb.band78.tm1.106PRB.usrpn300.conf b/ci-scripts/conf_files/gnb.band78.tm1.106PRB.usrpn300.conf
index a11509a3f0..b1d72c2269 100644
--- a/ci-scripts/conf_files/gnb.band78.tm1.106PRB.usrpn300.conf
+++ b/ci-scripts/conf_files/gnb.band78.tm1.106PRB.usrpn300.conf
@@ -187,7 +187,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/cmake_targets/CMakeLists.txt b/cmake_targets/CMakeLists.txt
index b026a2eb04..7643fedb9d 100644
--- a/cmake_targets/CMakeLists.txt
+++ b/cmake_targets/CMakeLists.txt
@@ -1975,6 +1975,7 @@ set(NR_PDCP_SRC
   ${OPENAIR2_DIR}/LAYER2/nr_pdcp/nr_pdcp_sdu.c
   ${OPENAIR2_DIR}/LAYER2/nr_pdcp/nr_pdcp_timer_thread.c
   ${OPENAIR2_DIR}/LAYER2/nr_pdcp/nr_pdcp_security_nea2.c
+  ${OPENAIR2_DIR}/LAYER2/nr_pdcp/nr_pdcp_integrity_nia2.c
   ${OPENAIR2_DIR}/LAYER2/nr_pdcp/asn1_utils.c
   )
 
@@ -3326,7 +3327,7 @@ add_executable(nr_dlsim
   )   
 target_link_libraries(nr_dlsim
   -Wl,--start-group UTIL SIMU_COMMON SIMU PHY_COMMON PHY_NR_COMMON PHY_NR PHY_NR_UE SCHED_NR_LIB SCHED_NR_UE_LIB MAC_NR MAC_UE_NR MAC_NR_COMMON RRC_LIB NR_RRC_LIB CONFIG_LIB L2_LTE_NR L2_NR HASHTABLE X2AP_ENB X2AP_LIB SECU_CN NGAP_GNB -Wl,--end-group
-  m pthread ${ATLAS_LIBRARIES} ${T_LIB} ${ITTI_LIB} dl
+  m pthread ${ATLAS_LIBRARIES} ${T_LIB} ${ITTI_LIB} ${OPENSSL_LIBRARIES} dl
   )
 target_compile_definitions(nr_dlsim PUBLIC -DPHYSICAL_SIMULATOR)
 
@@ -3345,7 +3346,7 @@ add_executable(nr_prachsim
   ${SHLIB_LOADER_SOURCES})
 target_link_libraries(nr_prachsim  
   -Wl,--start-group UTIL SIMU_COMMON SIMU PHY_COMMON PHY_NR_COMMON PHY_NR PHY_RU PHY_NR_UE MAC_NR_COMMON SCHED_NR_LIB SCHED_NR_UE_LIB MAC_NR MAC_UE_NR MAC_NR_COMMON RRC_LIB NR_RRC_LIB CONFIG_LIB L2_LTE_NR L2_NR HASHTABLE X2AP_ENB X2AP_LIB SECU_CN NGAP_GNB -Wl,--end-group
-  m pthread ${ATLAS_LIBRARIES} ${T_LIB} ${ITTI_LIB} dl)
+  m pthread ${ATLAS_LIBRARIES} ${T_LIB} ${ITTI_LIB} ${OPENSSL_LIBRARIES} dl)
 
 add_executable(nr_ulschsim
   ${OPENAIR1_DIR}/SIMULATION/NR_PHY/ulschsim.c
@@ -3378,7 +3379,7 @@ add_executable(nr_ulsim
   )
 target_link_libraries(nr_ulsim
   -Wl,--start-group UTIL SIMU_COMMON SIMU PHY_COMMON PHY_NR_COMMON PHY_NR PHY_NR_UE SCHED_NR_LIB SCHED_NR_UE_LIB MAC_NR MAC_UE_NR MAC_NR_COMMON RRC_LIB NR_RRC_LIB CONFIG_LIB L2_LTE_NR L2_NR HASHTABLE X2AP_ENB X2AP_LIB SECU_CN NGAP_GNB -Wl,--end-group
-  m pthread ${ATLAS_LIBRARIES} ${T_LIB} ${ITTI_LIB} dl
+  m pthread ${ATLAS_LIBRARIES} ${T_LIB} ${ITTI_LIB} ${OPENSSL_LIBRARIES} dl
   )
 target_compile_definitions(nr_ulsim PUBLIC -DPHYSICAL_SIMULATOR)
 
diff --git a/nfapi/open-nFAPI/nfapi/public_inc/fapi_nr_ue_interface.h b/nfapi/open-nFAPI/nfapi/public_inc/fapi_nr_ue_interface.h
index 9df9be4cbd..8c2118cd3d 100644
--- a/nfapi/open-nFAPI/nfapi/public_inc/fapi_nr_ue_interface.h
+++ b/nfapi/open-nFAPI/nfapi/public_inc/fapi_nr_ue_interface.h
@@ -972,7 +972,7 @@ typedef struct
 
 typedef struct 
 {
-  uint32_t ss_pbch_power;//SSB Block Power Value: TBD (-60..50 dBm)
+  int ss_pbch_power;//SSB Block Power Value: TBD (-60..50 dBm)
   uint8_t  bch_payload;//Defines option selected for generation of BCH payload, see Table 3-13 (v0.0.011 Value: 0: MAC generates the full PBCH payload 1: PHY generates the timing PBCH bits 2: PHY generates the full PBCH payload
   uint8_t  scs_common;//subcarrierSpacing for common, used for initial access and broadcast message. [38.211 sec 4.2] Value:0->3
 
diff --git a/nfapi/open-nFAPI/nfapi/public_inc/nfapi_nr_interface_scf.h b/nfapi/open-nFAPI/nfapi/public_inc/nfapi_nr_interface_scf.h
index da6516ac41..444140509c 100644
--- a/nfapi/open-nFAPI/nfapi/public_inc/nfapi_nr_interface_scf.h
+++ b/nfapi/open-nFAPI/nfapi/public_inc/nfapi_nr_interface_scf.h
@@ -357,7 +357,7 @@ typedef struct
 //table 3-23
 typedef struct 
 {
-  nfapi_uint32_tlv_t ss_pbch_power;//SSB Block Power Value: TBD (-60..50 dBm)
+  nfapi_int32_tlv_t ss_pbch_power;//SSB Block Power Value: TBD (-60..50 dBm)
   nfapi_uint8_tlv_t  bch_payload;//Defines option selected for generation of BCH payload, see Table 3-13 (v0.0.011 Value: 0: MAC generates the full PBCH payload 1: PHY generates the timing PBCH bits 2: PHY generates the full PBCH payload
   nfapi_uint8_tlv_t  scs_common;//subcarrierSpacing for common, used for initial access and broadcast message. [38.211 sec 4.2] Value:0->3
 
diff --git a/openair1/PHY/NR_ESTIMATION/nr_measurements_gNB.c b/openair1/PHY/NR_ESTIMATION/nr_measurements_gNB.c
index a54aec7bc2..d3e33a91a8 100644
--- a/openair1/PHY/NR_ESTIMATION/nr_measurements_gNB.c
+++ b/openair1/PHY/NR_ESTIMATION/nr_measurements_gNB.c
@@ -20,12 +20,12 @@
  */
 
 /*! \file PHY/NR_ESTIMATION/nr_measurements_gNB.c
-* \brief TA estimation for TA updates
-* \author Ahmed Hussein
+* \brief gNB measurement routines
+* \author Ahmed Hussein, G. Casati, K. Saaifan
 * \date 2019
 * \version 0.1
 * \company Fraunhofer IIS
-* \email: ahmed.hussein@iis.fraunhofer.de
+* \email: ahmed.hussein@iis.fraunhofer.de, guido.casati@iis.fraunhofer.de, khodr.saaifan@iis.fraunhofer.de
 * \note
 * \warning
 */
@@ -35,6 +35,8 @@
 #include "PHY/phy_extern.h"
 #include "nr_ul_estimation.h"
 
+extern openair0_config_t openair0_cfg[MAX_CARDS];
+
 int nr_est_timing_advance_pusch(PHY_VARS_gNB* gNB, int UE_id)
 {
   int i, aa, max_pos = 0, max_val = 0;
@@ -73,14 +75,17 @@ void gNB_I0_measurements(PHY_VARS_gNB *gNB) {
   NR_DL_FRAME_PARMS *frame_parms = &gNB->frame_parms;
   NR_gNB_COMMON *common_vars = &gNB->common_vars;
   PHY_MEASUREMENTS_gNB *measurements = &gNB->measurements;
+  NR_DL_FRAME_PARMS *fp = &gNB->frame_parms;
+  double rx_gain = openair0_cfg[0].rx_gain[0];
+  double rx_gain_offset = openair0_cfg[0].rx_gain_offset[0];
   uint32_t *rb_mask = gNB->rb_mask_ul;
   int symbol = gNB->ulmask_symb;
   int rb, offset, nb_rb;
-  uint32_t n0_power_tot, n0_subband_power_temp=0;
+  uint32_t n0_subband_power_temp = 0;
   int32_t *ul_ch;
 
   if (symbol>-1) {
-    n0_power_tot = 0;
+    measurements->n0_power_tot = 0;
     for (int aarx=0; aarx<frame_parms->nb_antennas_rx; aarx++) {
       nb_rb = 0;
       for (rb=0; rb<frame_parms->N_RB_UL; rb++) {
@@ -93,11 +98,76 @@ void gNB_I0_measurements(PHY_VARS_gNB *gNB) {
           n0_subband_power_temp += signal_energy_nodc(ul_ch,12);
         }
       }
-      measurements->n0_power[aarx] = n0_subband_power_temp/nb_rb;
-      measurements->n0_power_dB[aarx] = dB_fixed(measurements->n0_power[aarx]);
-      n0_power_tot += measurements->n0_power[aarx];
+
+      if (nb_rb != 0) {
+        measurements->n0_power[aarx] = n0_subband_power_temp/nb_rb;
+        measurements->n0_power_dB[aarx] = dB_fixed(measurements->n0_power[aarx]);
+        measurements->n0_power_tot += measurements->n0_power[aarx];
+      }
+
     }
-    measurements->n0_power_tot_dB = dB_fixed(n0_power_tot);
+
+    measurements->n0_power_tot_dB = dB_fixed(measurements->n0_power_tot);
+    measurements->n0_power_tot_dBm = measurements->n0_power_tot_dB + 30 - 10 * log10(pow(2, 30)) - (rx_gain - rx_gain_offset) - dB_fixed(fp->ofdm_symbol_size);
+
+    LOG_D(PHY, "In %s: tot n0 power %d dBm for %d RBs (tot N0 power = %d)\n", __FUNCTION__, measurements->n0_power_tot_dBm, nb_rb, measurements->n0_power_tot);
+
   }
 }
 
+// Scope: This function computes the UL SNR from the UL channel estimates
+//
+// Todo:
+// - averaging IIR filter for RX power and noise
+void nr_gnb_measurements(PHY_VARS_gNB *gNB, uint8_t ulsch_id, unsigned char harq_pid, unsigned char symbol){
+
+  int rx_power_tot[NUMBER_OF_NR_ULSCH_MAX];
+  int rx_power[NUMBER_OF_NR_ULSCH_MAX][NB_ANTENNAS_RX];
+  unsigned short rx_power_avg_dB[NUMBER_OF_NR_ULSCH_MAX];
+  unsigned short rx_power_tot_dB[NUMBER_OF_NR_ULSCH_MAX];
+
+  double             rx_gain = openair0_cfg[0].rx_gain[0];
+  double      rx_gain_offset = openair0_cfg[0].rx_gain_offset[0];
+  PHY_MEASUREMENTS_gNB *meas = &gNB->measurements;
+  NR_DL_FRAME_PARMS      *fp = &gNB->frame_parms;
+  int              ch_offset = fp->ofdm_symbol_size * symbol;
+  int                N_RB_UL = gNB->ulsch[ulsch_id][0]->harq_processes[harq_pid]->ulsch_pdu.rb_size;
+
+  rx_power_tot[ulsch_id] = 0;
+
+  for (int aarx = 0; aarx < fp->nb_antennas_rx; aarx++){
+
+    rx_power[ulsch_id][aarx] = 0;
+
+    for (int aatx = 0; aatx < fp->nb_antennas_tx; aatx++){
+
+      meas->rx_spatial_power[ulsch_id][aatx][aarx] = (signal_energy_nodc(&gNB->pusch_vars[ulsch_id]->ul_ch_estimates[aarx][ch_offset], N_RB_UL * NR_NB_SC_PER_RB));
+
+      if (meas->rx_spatial_power[ulsch_id][aatx][aarx] < 0) {
+        meas->rx_spatial_power[ulsch_id][aatx][aarx] = 0;
+      }
+
+      meas->rx_spatial_power_dB[ulsch_id][aatx][aarx] = (unsigned short) dB_fixed(meas->rx_spatial_power[ulsch_id][aatx][aarx]);
+      rx_power[ulsch_id][aarx] += meas->rx_spatial_power[ulsch_id][aatx][aarx];
+
+    }
+
+    rx_power_tot[ulsch_id] += rx_power[ulsch_id][aarx];
+
+  }
+
+  rx_power_tot_dB[ulsch_id] = (unsigned short) dB_fixed(rx_power_tot[ulsch_id]);
+  rx_power_avg_dB[ulsch_id] = rx_power_tot_dB[ulsch_id];
+
+  meas->wideband_cqi_tot[ulsch_id] = dB_fixed2(rx_power_tot[ulsch_id], meas->n0_power_tot);
+  meas->rx_rssi_dBm[ulsch_id] = rx_power_avg_dB[ulsch_id] + 30 - 10 * log10(pow(2, 30)) - (rx_gain - rx_gain_offset) - dB_fixed(fp->ofdm_symbol_size);
+
+  LOG_D(PHY, "[ULSCH %d] RSSI %d dBm/RE, RSSI (digital) %d dB (N_RB_UL %d), WBand CQI tot %d dB, N0 Power tot %d\n",
+    ulsch_id,
+    meas->rx_rssi_dBm[ulsch_id],
+    rx_power_avg_dB[ulsch_id],
+    N_RB_UL,
+    meas->wideband_cqi_tot[ulsch_id],
+    meas->n0_power_tot);
+
+}
diff --git a/openair1/PHY/NR_ESTIMATION/nr_ul_estimation.h b/openair1/PHY/NR_ESTIMATION/nr_ul_estimation.h
index a463e21839..7bd004a4d4 100644
--- a/openair1/PHY/NR_ESTIMATION/nr_ul_estimation.h
+++ b/openair1/PHY/NR_ESTIMATION/nr_ul_estimation.h
@@ -49,8 +49,9 @@ int nr_pusch_channel_estimation(PHY_VARS_gNB *gNB,
 
 void gNB_I0_measurements(PHY_VARS_gNB *gNB);
 
-int nr_est_timing_advance_pusch(PHY_VARS_gNB* phy_vars_gNB, int UE_id);
+void nr_gnb_measurements(PHY_VARS_gNB *gNB, uint8_t ulsch_id, unsigned char harq_pid, unsigned char symbol);
 
+int nr_est_timing_advance_pusch(PHY_VARS_gNB* phy_vars_gNB, int UE_id);
 
 void nr_pusch_ptrs_processing(PHY_VARS_gNB *gNB,
                               NR_DL_FRAME_PARMS *frame_parms,
diff --git a/openair1/PHY/NR_TRANSPORT/nr_ulsch_demodulation.c b/openair1/PHY/NR_TRANSPORT/nr_ulsch_demodulation.c
index f25aef8813..edca81d501 100644
--- a/openair1/PHY/NR_TRANSPORT/nr_ulsch_demodulation.c
+++ b/openair1/PHY/NR_TRANSPORT/nr_ulsch_demodulation.c
@@ -1217,6 +1217,8 @@ int nr_rx_pusch(PHY_VARS_gNB *gNB,
                                 bwp_start_subcarrier,
                                 rel15_ul);
 
+    nr_gnb_measurements(gNB, ulsch_id, harq_pid, symbol);
+
     for (aarx = 0; aarx < frame_parms->nb_antennas_rx; aarx++) {
       gNB->pusch_vars[ulsch_id]->ulsch_power[aarx] = signal_energy_nodc(&gNB->pusch_vars[ulsch_id]->ul_ch_estimates[aarx][symbol*frame_parms->ofdm_symbol_size],
                                                                         rel15_ul->rb_size*12);
diff --git a/openair1/PHY/NR_UE_ESTIMATION/nr_ue_measurements.c b/openair1/PHY/NR_UE_ESTIMATION/nr_ue_measurements.c
index 61fc40ad3f..3f52b29281 100644
--- a/openair1/PHY/NR_UE_ESTIMATION/nr_ue_measurements.c
+++ b/openair1/PHY/NR_UE_ESTIMATION/nr_ue_measurements.c
@@ -21,16 +21,17 @@
 
 /*! \file nr_ue_measurements.c
  * \brief UE measurements routines
- * \author  R. Knopp, G. Casati
+ * \author  R. Knopp, G. Casati, K. Saaifan
  * \date 2020
  * \version 0.1
  * \company Eurecom, Fraunhofer IIS
- * \email: knopp@eurecom.fr, guido.casati@iis.fraunhofer.de
+ * \email: knopp@eurecom.fr, guido.casati@iis.fraunhofer.de, khodr.saaifan@iis.fraunhofer.de
  * \note
  * \warning
  */
 
 #include "executables/softmodem-common.h"
+#include "executables/nr-softmodem-common.h"
 #include "PHY/defs_nr_UE.h"
 #include "PHY/phy_extern_nr_ue.h"
 #include "common/utils/LOG/log.h"
@@ -45,7 +46,7 @@
 //#define DEBUG_MEAS_UE
 //#define DEBUG_RANK_EST
 
-// Returns the pathloss in dBm for the active UL BWP on the selected carrier based on the DL RS associated with the PRACH transmission
+// Returns the pathloss in dB for the active UL BWP on the selected carrier based on the DL RS associated with the PRACH transmission
 // computation according to clause 7.4 (Physical random access channel) of 3GPP TS 38.213 version 16.3.0 Release 16
 // Assumptions:
 // - PRACH transmission from a UE is not in response to a detection of a PDCCH order by the UE
@@ -59,18 +60,17 @@ int16_t get_nr_PL(uint8_t Mod_id, uint8_t CC_id, uint8_t gNB_index){
   if (get_softmodem_params()->do_ra){
 
     long referenceSignalPower = ue->nrUE_config.ssb_config.ss_pbch_power;
-    double rsrp_dBm = 10*log10(ue->measurements.rsrp[gNB_index]) + 30 - ue->rx_total_gain_dB;
 
-    pathloss = (int16_t)(10*log10(pow(10, (double)(referenceSignalPower)/10) - pow(10, (double)(rsrp_dBm)/10)));
+    pathloss = (int16_t)(referenceSignalPower - ue->measurements.rsrp_dBm[gNB_index]);
 
-    LOG_D(MAC, "In %s: pathloss %d dBm, UE RX total gain %d dB, referenceSignalPower %ld dBm (%f mW), RSRP %f dBm (%f mW)\n",
+    LOG_D(MAC, "In %s: pathloss %d dB, UE RX total gain %d dB, referenceSignalPower %ld dBm/RE (%f mW), RSRP %d dBm (%f mW)\n",
       __FUNCTION__,
       pathloss,
       ue->rx_total_gain_dB,
       referenceSignalPower,
       pow(10, referenceSignalPower/10),
-      rsrp_dBm,
-      pow(10, rsrp_dBm/10));
+      ue->measurements.rsrp_dBm[gNB_index],
+      pow(10, ue->measurements.rsrp_dBm[gNB_index]/10));
 
   } else {
 
@@ -178,9 +178,9 @@ void nr_ue_measurements(PHY_VARS_NR_UE *ue,
     ue->measurements.rx_power_avg_dB[gNB_id] = dB_fixed( ue->measurements.rx_power_avg[gNB_id]);
     ue->measurements.wideband_cqi_tot[gNB_id] = dB_fixed2(ue->measurements.rx_power_tot[gNB_id], ue->measurements.n0_power_tot);
     ue->measurements.wideband_cqi_avg[gNB_id] = dB_fixed2(ue->measurements.rx_power_avg[gNB_id], ue->measurements.n0_power_avg);
-    ue->measurements.rx_rssi_dBm[gNB_id] = ue->measurements.rx_power_avg_dB[gNB_id] - ue->rx_total_gain_dB;
+    ue->measurements.rx_rssi_dBm[gNB_id] = ue->measurements.rx_power_avg_dB[gNB_id] + 30 - 10*log10(pow(2, 30)) - ((int)openair0_cfg[0].rx_gain[0] - (int)openair0_cfg[0].rx_gain_offset[0]) - dB_fixed(ue->frame_parms.ofdm_symbol_size);
 
-    LOG_D(PHY, "[gNB %d] Slot %d, RSSI %d dBm, RSSI (digital) %d dB, WBandCQI %d dB, rxPwrAvg %d, n0PwrAvg %d\n",
+    LOG_I(PHY, "[gNB %d] Slot %d, RSSI %d dBm/RE, RSSI (digital) %d dB, WBandCQI %d dB, rxPwrAvg %d, n0PwrAvg %d \n",
       gNB_id,
       slot,
       ue->measurements.rx_rssi_dBm[gNB_id],
@@ -258,15 +258,20 @@ void nr_ue_rsrp_measurements(PHY_VARS_NR_UE *ue,
 
   ue->measurements.rsrp_filtered[gNB_id] = ue->measurements.rsrp[gNB_id];
 
-  LOG_D(PHY, "In %s: [UE %d] slot %d SS-RSRP: %3.1f dBm/RE (%d W)\n",
+  ue->measurements.rsrp_dBm[gNB_id] = 10*log10(ue->measurements.rsrp[gNB_id]) + 30 - 10*log10(pow(2,30)) - ((int)openair0_cfg[0].rx_gain[0] - (int)openair0_cfg[0].rx_gain_offset[0]) - dB_fixed(ue->frame_parms.ofdm_symbol_size);
+
+  LOG_D(PHY, "In %s: [UE %d] slot %d SS-RSRP: %d dBm/RE (%d)\n",
     __FUNCTION__,
     ue->Mod_id,
     slot,
-    10*log10(ue->measurements.rsrp[gNB_id]) + 30 - ue->rx_total_gain_dB,
+    ue->measurements.rsrp_dBm[gNB_id],
     ue->measurements.rsrp[gNB_id]);
 
 }
 
+// This function computes the received noise power
+// Measurement units:
+// - psd_awgn (AWGN power spectral density):     dBm/Hz
 void nr_ue_rrc_measurements(PHY_VARS_NR_UE *ue,
                             UE_nr_rxtx_proc_t *proc,
                             uint8_t slot){
@@ -279,6 +284,9 @@ void nr_ue_rrc_measurements(PHY_VARS_NR_UE *ue,
   uint8_t k_length = 8;
   uint8_t l_sss = ue->symbol_offset + 2;
   unsigned int ssb_offset = ue->frame_parms.first_carrier_offset + ue->frame_parms.ssb_start_subcarrier;
+  double rx_gain = openair0_cfg[0].rx_gain[0];
+  double rx_gain_offset = openair0_cfg[0].rx_gain_offset[0];
+
   ue->measurements.n0_power_tot = 0;
 
   LOG_D(PHY, "In %s doing measurements for ssb_offset %d l_sss %d \n", __FUNCTION__, ssb_offset, l_sss);
@@ -322,10 +330,17 @@ void nr_ue_rrc_measurements(PHY_VARS_NR_UE *ue,
   ue->measurements.n0_power_tot_dB = (unsigned short) dB_fixed(ue->measurements.n0_power_tot/aarx);
 
   #ifdef DEBUG_MEAS_RRC
-  int nf_usrp = ue->measurements.n0_power_tot_dB + 30 - ((int)openair0_cfg[0].rx_gain[0] - (int)openair0_cfg[0].rx_gain_offset[0]) - 90 - (-174 + dB_fixed(30000/*scs*/) + dB_fixed(ue->frame_parms.ofdm_symbol_size));
-  LOG_D(PHY, "In %s slot %d NF USRP %d dBm\n", __FUNCTION__, nf_usrp);
+  const int psd_awgn = -174;
+  const int scs = 15000 * (1 << ue->frame_parms.numerology_index);
+  const int nf_usrp = ue->measurements.n0_power_tot_dB + 3 + 30 - ((int)rx_gain - (int)rx_gain_offset) - 10 * log10(pow(2, 30)) - (psd_awgn + dB_fixed(scs) + dB_fixed(ue->frame_parms.ofdm_symbol_size));
+  LOG_D(PHY, "In [%s][slot:%d] NF USRP %d dB\n", __FUNCTION__, slot, nf_usrp);
   #endif
 
-  LOG_D(PHY, "In %s slot %d Noise Level %d ue->measurements.n0_power_tot_dB %d \n", __FUNCTION__, slot, ue->measurements.n0_power_tot, ue->measurements.n0_power_tot_dB);
+  LOG_D(PHY, "In [%s][slot:%d] Noise Level %d (digital level %d dB, noise power spectral density %f dBm/RE)\n",
+    __FUNCTION__,
+    slot,
+    ue->measurements.n0_power_tot,
+    ue->measurements.n0_power_tot_dB,
+    ue->measurements.n0_power_tot_dB + 30 - 10*log10(pow(2, 30)) - dB_fixed(ue->frame_parms.ofdm_symbol_size) - ((int)rx_gain - (int)rx_gain_offset));
 
 }
diff --git a/openair1/PHY/TOOLS/oai_dfts.c b/openair1/PHY/TOOLS/oai_dfts.c
index 6569f7eab5..6e85bca384 100644
--- a/openair1/PHY/TOOLS/oai_dfts.c
+++ b/openair1/PHY/TOOLS/oai_dfts.c
@@ -18,7 +18,7 @@
  * For more information about the OpenAirInterface (OAI) Software Alliance:
  *      contact@openairinterface.org
  */
-
+ 
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -3103,10 +3103,12 @@ void dft128(int16_t *x,int16_t *y,unsigned char scale)
 
   dft64((int16_t*)(xtmp),(int16_t*)ytmp,1);
   dft64((int16_t*)(xtmp+32),(int16_t*)(ytmp+16),1);
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {
     LOG_M("dft128a.m","dfta",ytmp,64,1,1);
     LOG_M("dft128b.m","dftb",ytmp+16,64,1,1);
   }
+#endif
   for (i=0; i<16; i++) {
     bfly2_16(ytmpp,ytmpp+16,
              y128p,y128p+16,
@@ -3155,10 +3157,12 @@ void dft128(int16_t *x,int16_t *y,unsigned char scale)
 
 
   }
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {
      LOG_M("dft128out.m","dft128",y,128,1,1);
      exit(-1);
   }
+#endif
   _mm_empty();
   _m_empty();
 
@@ -3183,18 +3187,20 @@ void dft128(int16_t *x,int16_t *y,unsigned char scale)
   transpose4_ooff_simd256(x256+10,xtmp+5,8);
   transpose4_ooff_simd256(x256+12,xtmp+6,8);
   transpose4_ooff_simd256(x256+14,xtmp+7,8);
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {  
      LOG_M("dft128ina_256.m","dftina",xtmp,64,1,1);
      LOG_M("dft128inb_256.m","dftinb",xtmp+8,64,1,1);
   }
-
+#endif
   dft64((int16_t*)(xtmp),(int16_t*)ytmp,1);
   dft64((int16_t*)(xtmp+8),(int16_t*)(ytmp+8),1);
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {  
     LOG_M("dft128outa_256.m","dftouta",ytmp,64,1,1);
     LOG_M("dft128outb_256.m","dftoutb",ytmp+8,64,1,1);
   }
-
+#endif
   for (i=0; i<8; i++) {
     bfly2_16_256(ytmpp,ytmpp+8,
 		 y256p,y256p+8,
@@ -3226,10 +3232,12 @@ void dft128(int16_t *x,int16_t *y,unsigned char scale)
     y256[15] = mulhi_int16_simd256(y256[15],ONE_OVER_SQRT2_Q15_256);
 
   }
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {  
    LOG_M("dft128.m","dft",y256,128,1,1);
    exit(-1);
   }
+#endif
 }
 
 #endif
@@ -6031,11 +6039,13 @@ void dft1536(int16_t *input, int16_t *output, unsigned char scale)
     tmpo[1][i] = tmpo[1][i<<1];
     tmpo[2][i] = tmpo[2][i<<1];
     }*/
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {
     LOG_M("dft1536out0.m","o0",tmpo[0],2048,1,1);
     LOG_M("dft1536out1.m","o1",tmpo[1],2048,1,1);
     LOG_M("dft1536out2.m","o2",tmpo[2],2048,1,1);
   }
+#endif
   for (i=0,i2=0; i<1024; i+=8,i2+=4)  {
     bfly3((simd_q15_t*)(&tmpo[0][i2]),(simd_q15_t*)(&tmpo[1][i2]),(simd_q15_t*)(&tmpo[2][i2]),
           (simd_q15_t*)(output+i),(simd_q15_t*)(output+1024+i),(simd_q15_t*)(output+2048+i),
@@ -6193,14 +6203,14 @@ void idft6144(int16_t *input, int16_t *output,unsigned char scale)
   idft2048((int16_t*)(tmp[0]),(int16_t*)(tmpo[0]),1);
   idft2048((int16_t*)(tmp[1]),(int16_t*)(tmpo[1]),1);
   idft2048((int16_t*)(tmp[2]),(int16_t*)(tmpo[2]),1);
-
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {
     LOG_M("idft6144in.m","in",input,6144,1,1);
     LOG_M("idft6144out0.m","o0",tmpo[0],2048,1,1);
     LOG_M("idft6144out1.m","o1",tmpo[1],2048,1,1);
     LOG_M("idft6144out2.m","o2",tmpo[2],2048,1,1);
   }
-
+#endif
   for (i=0,i2=0; i<4096; i+=8,i2+=4)  {
     ibfly3((simd_q15_t*)(&tmpo[0][i2]),(simd_q15_t*)(&tmpo[1][i2]),((simd_q15_t*)&tmpo[2][i2]),
 	   (simd_q15_t*)(output+i),(simd_q15_t*)(output+4096+i),(simd_q15_t*)(output+8192+i),
@@ -6260,11 +6270,13 @@ void dft6144(int16_t *input, int16_t *output,unsigned char scale)
     tmpo[1][i] = tmpo[1][i<<1];
     tmpo[2][i] = tmpo[2][i<<1];
     }*/
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {
     LOG_M("ft6144out0.m","o0",tmpo[0],2048,1,1);
     LOG_M("ft6144out1.m","o1",tmpo[1],2048,1,1);
     LOG_M("ft6144out2.m","o2",tmpo[2],2048,1,1);
   }
+#endif
   for (i=0,i2=0; i<4096; i+=8,i2+=4)  {
     bfly3((simd_q15_t*)(&tmpo[0][i2]),(simd_q15_t*)(&tmpo[1][i2]),(simd_q15_t*)(&tmpo[2][i2]),
           (simd_q15_t*)(output+i),(simd_q15_t*)(output+4096+i),(simd_q15_t*)(output+8192+i),
@@ -6336,11 +6348,13 @@ void dft12288(int16_t *input, int16_t *output,unsigned char scale)
     tmpo[1][i] = tmpo[1][i<<1];
     tmpo[2][i] = tmpo[2][i<<1];
     }*/
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {
     LOG_M("dft12288out0.m","o0",tmpo[0],4096,1,1);
     LOG_M("dft12288out1.m","o1",tmpo[1],4096,1,1);
     LOG_M("dft12288out2.m","o2",tmpo[2],4096,1,1);
   }
+#endif
   for (i=0,i2=0; i<8192; i+=8,i2+=4)  {
     bfly3((simd_q15_t*)(&tmpo[0][i2]),(simd_q15_t*)(&tmpo[1][i2]),(simd_q15_t*)(&tmpo[2][i2]),
           (simd_q15_t*)(output+i),(simd_q15_t*)(output+8192+i),(simd_q15_t*)(output+16384+i),
@@ -6392,14 +6406,14 @@ void idft12288(int16_t *input, int16_t *output,unsigned char scale)
   idft4096((int16_t*)(tmp[0]),(int16_t*)(tmpo[0]),scale);
   idft4096((int16_t*)(tmp[1]),(int16_t*)(tmpo[1]),scale);
   idft4096((int16_t*)(tmp[2]),(int16_t*)(tmpo[2]),scale);
-
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {
     LOG_M("idft12288in.m","in",input,12288,1,1);
     LOG_M("idft12288out0.m","o0",tmpo[0],4096,1,1);
     LOG_M("idft12288out1.m","o1",tmpo[1],4096,1,1);
     LOG_M("idft12288out2.m","o2",tmpo[2],4096,1,1);
   }
-
+#endif
   for (i=0,i2=0; i<8192; i+=8,i2+=4)  {
     ibfly3((simd_q15_t*)(&tmpo[0][i2]),(simd_q15_t*)(&tmpo[1][i2]),((simd_q15_t*)&tmpo[2][i2]),
           (simd_q15_t*)(output+i),(simd_q15_t*)(output+8192+i),(simd_q15_t*)(output+16384+i),
@@ -6429,9 +6443,11 @@ void idft12288(int16_t *input, int16_t *output,unsigned char scale)
   }
   _mm_empty();
   _m_empty();
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {
      LOG_M("idft12288out.m","out",output,6144,1,1);
   }
+#endif
 }
 
 int16_t twa18432[12288] __attribute__((aligned(32)));
@@ -6560,11 +6576,13 @@ void dft24576(int16_t *input, int16_t *output,unsigned char scale)
     tmpo[1][i] = tmpo[1][i<<1];
     tmpo[2][i] = tmpo[2][i<<1];
     }*/
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {
     LOG_M("dft24576out0.m","o0",tmpo[0],8192,1,1);
     LOG_M("dft24576out1.m","o1",tmpo[1],8192,1,1);
     LOG_M("dft24576out2.m","o2",tmpo[2],8192,1,1);
   }
+#endif
   for (i=0,i2=0; i<16384; i+=8,i2+=4)  {
     bfly3((simd_q15_t*)(&tmpo[0][i2]),(simd_q15_t*)(&tmpo[1][i2]),(simd_q15_t*)(&tmpo[2][i2]),
           (simd_q15_t*)(output+i),(simd_q15_t*)(output+16384+i),(simd_q15_t*)(output+32768+i),
@@ -6595,9 +6613,11 @@ void dft24576(int16_t *input, int16_t *output,unsigned char scale)
   }
   _mm_empty();
   _m_empty();
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {
      LOG_M("out.m","out",output,24576,1,1);
   }
+#endif
 }
 
 void idft24576(int16_t *input, int16_t *output,unsigned char scale)
@@ -6617,14 +6637,14 @@ void idft24576(int16_t *input, int16_t *output,unsigned char scale)
   idft8192((int16_t*)(tmp[0]),(int16_t*)(tmpo[0]),1);
   idft8192((int16_t*)(tmp[1]),(int16_t*)(tmpo[1]),1);
   idft8192((int16_t*)(tmp[2]),(int16_t*)(tmpo[2]),1);
-  
+ #ifndef MR_MAIN 
   if (LOG_DUMPFLAG(DEBUG_DFT)) {
     LOG_M("idft24576in.m","in",input,24576,1,1);
     LOG_M("idft24576out0.m","o0",tmpo[0],8192,1,1);
     LOG_M("idft24576out1.m","o1",tmpo[1],8192,1,1);
     LOG_M("idft24576out2.m","o2",tmpo[2],8192,1,1);
   }
-
+#endif
   for (i=0,i2=0; i<16384; i+=8,i2+=4)  {
     ibfly3((simd_q15_t*)(&tmpo[0][i2]),(simd_q15_t*)(&tmpo[1][i2]),((simd_q15_t*)&tmpo[2][i2]),
           (simd_q15_t*)(output+i),(simd_q15_t*)(output+16384+i),(simd_q15_t*)(output+32768+i),
@@ -6653,10 +6673,11 @@ void idft24576(int16_t *input, int16_t *output,unsigned char scale)
   }
   _mm_empty();
   _m_empty();
-
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {
     LOG_M("idft24576out.m","out",output,24576,1,1);
   }
+#endif
 }
 
 int16_t twa36864[24576] __attribute__((aligned(32)));
@@ -6680,13 +6701,13 @@ void dft36864(int16_t *input, int16_t *output,uint8_t scale) {
   dft12288((int16_t*)(tmp[0]),(int16_t*)(tmpo[0]),1);
   dft12288((int16_t*)(tmp[1]),(int16_t*)(tmpo[1]),1);
   dft12288((int16_t*)(tmp[2]),(int16_t*)(tmpo[2]),1);
-
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {
     LOG_M("dft36864out0.m","o0",tmpo[0],12288,1,1);
     LOG_M("dft36864out1.m","o1",tmpo[1],12288,1,1);
     LOG_M("dft36864out2.m","o2",tmpo[2],12288,1,1);
   }
-
+#endif
   for (i=0,i2=0; i<24576; i+=8,i2+=4)  {
     bfly3((simd_q15_t*)(&tmpo[0][i2]),(simd_q15_t*)(&tmpo[1][i2]),(simd_q15_t*)(&tmpo[2][i2]),
           (simd_q15_t*)(output+i),(simd_q15_t*)(output+24576+i),(simd_q15_t*)(output+49152+i),
@@ -6716,9 +6737,11 @@ void dft36864(int16_t *input, int16_t *output,uint8_t scale) {
   }
   _mm_empty();
   _m_empty();
+#ifndef MR_MAIN
   if (LOG_DUMPFLAG(DEBUG_DFT)) {
      LOG_M("out.m","out",output,36864,1,1);
   }
+#endif
 }
 
 void idft36864(int16_t *input, int16_t *output,uint8_t scale) {
diff --git a/openair1/PHY/defs_nr_UE.h b/openair1/PHY/defs_nr_UE.h
index a82c63d9a5..bf0a7bc9e5 100644
--- a/openair1/PHY/defs_nr_UE.h
+++ b/openair1/PHY/defs_nr_UE.h
@@ -142,6 +142,7 @@ typedef struct {
   uint32_t rsrp[7];
   float rsrp_filtered[7]; // after layer 3 filtering
   float rsrq_filtered[7];
+  short rsrp_dBm[7];
   // common measurements
   //! estimated noise power (linear)
   unsigned int   n0_power[NB_ANTENNAS_RX];
diff --git a/openair1/SCHED_NR_UE/phy_procedures_nr_ue.c b/openair1/SCHED_NR_UE/phy_procedures_nr_ue.c
index 0ce5fc8e0e..df66217e49 100644
--- a/openair1/SCHED_NR_UE/phy_procedures_nr_ue.c
+++ b/openair1/SCHED_NR_UE/phy_procedures_nr_ue.c
@@ -2072,10 +2072,10 @@ void nr_ue_prach_procedures(PHY_VARS_NR_UE *ue, UE_nr_rxtx_proc_t *proc, uint8_t
 
     if (ue->mac_enabled == 1) {
       int16_t pathloss = get_nr_PL(mod_id, ue->CC_id, gNB_id);
-      int16_t ra_preamble_rx_power = (int16_t)(10*log10(pow(10, (double)(prach_resources->ra_PREAMBLE_RECEIVED_TARGET_POWER)/10) + pow(10, (double)(pathloss)/10)));
+      int16_t ra_preamble_rx_power = (int16_t)(prach_resources->ra_PREAMBLE_RECEIVED_TARGET_POWER - pathloss + 30);
       ue->tx_power_dBm[nr_slot_tx] = min(nr_get_Pcmax(mod_id), ra_preamble_rx_power);
 
-      LOG_I(PHY,"[UE %d][RAPROC][%d.%d]: Generating PRACH Msg1 (preamble %d, PL %d, P0_PRACH %d, TARGET_RECEIVED_POWER %d dBm, RA-RNTI %x)\n",
+      LOG_I(PHY,"DEBUG [UE %d][RAPROC][%d.%d]: Generating PRACH Msg1 (preamble %d, PL %d dB, P0_PRACH %d, TARGET_RECEIVED_POWER %d dBm, RA-RNTI %x)\n",
         mod_id,
         frame_tx,
         nr_slot_tx,
diff --git a/openair2/LAYER2/nr_pdcp/nr_pdcp_entity.c b/openair2/LAYER2/nr_pdcp/nr_pdcp_entity.c
index eda6888ecd..1822239171 100644
--- a/openair2/LAYER2/nr_pdcp/nr_pdcp_entity.c
+++ b/openair2/LAYER2/nr_pdcp/nr_pdcp_entity.c
@@ -26,6 +26,7 @@
 #include <string.h>
 
 #include "nr_pdcp_security_nea2.h"
+#include "nr_pdcp_integrity_nia2.h"
 #include "nr_pdcp_sdu.h"
 
 #include "LOG/log.h"
@@ -54,17 +55,21 @@ static void nr_pdcp_entity_recv_pdu(nr_pdcp_entity_t *entity,
   }
 
   if (entity->sn_size == 12) {
-    rcvd_sn = (((unsigned char)buffer[0] & 0xf) <<  8) |
-                (unsigned char)buffer[1];
+    rcvd_sn = ((buffer[0] & 0xf) <<  8) |
+                buffer[1];
     header_size = 2;
   } else {
-    rcvd_sn = (((unsigned char)buffer[0] & 0x3) << 16) |
-               ((unsigned char)buffer[1]        <<  8) |
-                (unsigned char)buffer[2];
+    rcvd_sn = ((buffer[0] & 0x3) << 16) |
+               (buffer[1]        <<  8) |
+                buffer[2];
     header_size = 3;
   }
 
-  integrity_size = 0;
+  if (entity->has_integrity) {
+    integrity_size = 4;
+  } else {
+    integrity_size = 0;
+  }
 
   if (size < header_size + integrity_size + 1) {
     LOG_E(PDCP, "bad PDU received (size = %d)\n", size);
@@ -86,9 +91,20 @@ static void nr_pdcp_entity_recv_pdu(nr_pdcp_entity_t *entity,
 
   if (entity->has_ciphering)
     entity->cipher(entity->security_context,
-                   (unsigned char *)buffer+header_size, size-header_size,
+                   buffer+header_size, size-header_size,
                    entity->rb_id, rcvd_count, entity->is_gnb ? 0 : 1);
 
+  if (entity->has_integrity) {
+    unsigned char integrity[4];
+    entity->integrity(entity->integrity_context, integrity,
+                      buffer, size - integrity_size,
+                      entity->rb_id, rcvd_count, entity->is_gnb ? 0 : 1);
+    if (memcmp(integrity, buffer, 4) != 0) {
+      LOG_E(PDCP, "discard NR PDU, integrity failed\n");
+      return;
+    }
+  }
+
   if (rcvd_count < entity->rx_deliv
       || nr_pdcp_sdu_in_list(entity->rx_list, rcvd_count)) {
     LOG_D(PDCP, "discard NR PDU rcvd_count=%d\n", rcvd_count);
@@ -139,39 +155,62 @@ static void nr_pdcp_entity_recv_sdu(nr_pdcp_entity_t *entity,
   uint32_t count;
   int      sn;
   int      header_size;
-  char     buf[size+3+4];
+  int      integrity_size;
+  char     buf[size + 3 + 4];
+  int      dc_bit;
 
   count = entity->tx_next;
   sn = entity->tx_next & entity->sn_max;
 
+  /* D/C bit is only to be set for DRBs */
+  if (entity->type == NR_PDCP_DRB_AM || entity->type == NR_PDCP_DRB_UM) {
+    dc_bit = 0x80;
+  } else {
+    dc_bit = 0;
+  }
+
   if (entity->sn_size == 12) {
-    buf[0] = 0x80 | ((sn >> 8) & 0xf);
+    buf[0] = dc_bit | ((sn >> 8) & 0xf);
     buf[1] = sn & 0xff;
     header_size = 2;
   } else {
-    buf[0] = 0x80 | ((sn >> 16) & 0x3);
+    buf[0] = dc_bit | ((sn >> 16) & 0x3);
     buf[1] = (sn >> 8) & 0xff;
     buf[2] = sn & 0xff;
     header_size = 3;
   }
 
-  memcpy(buf+header_size, buffer, size);
+  if (entity->has_integrity) {
+    integrity_size = 4;
+  } else {
+    integrity_size = 0;
+  }
+
+  memcpy(buf + header_size, buffer, size);
+
+  if (entity->has_integrity)
+    entity->integrity(entity->integrity_context,
+                      (unsigned char *)buf + header_size + size,
+                      (unsigned char *)buf, header_size + size,
+                      entity->rb_id, count, entity->is_gnb ? 1 : 0);
 
   if (entity->has_ciphering)
     entity->cipher(entity->security_context,
-                   (unsigned char *)buf+header_size, size,
+                   (unsigned char *)buf + header_size, size + integrity_size,
                    entity->rb_id, count, entity->is_gnb ? 1 : 0);
 
   entity->tx_next++;
 
   entity->deliver_pdu(entity->deliver_pdu_data, entity, buf,
-                      size+header_size, sdu_id);
+                      header_size + size + integrity_size, sdu_id);
 }
 
 static void nr_pdcp_entity_set_integrity_key(nr_pdcp_entity_t *entity,
                                              char *key)
 {
-  memcpy(entity->integrity_key, key, 16);
+  LOG_E(PDCP, "%s: %d: %s: TODO? to remove?\n", __FILE__, __LINE__, __FUNCTION__);
+  exit(1);
+  //memcpy(entity->integrity_key, key, 16);
 }
 
 static void check_t_reordering(nr_pdcp_entity_t *entity)
@@ -232,6 +271,8 @@ void nr_pdcp_entity_delete(nr_pdcp_entity_t *entity)
   }
   if (entity->free_security != NULL)
     entity->free_security(entity->security_context);
+  if (entity->free_integrity != NULL)
+    entity->free_integrity(entity->integrity_context);
   free(entity);
 }
 
@@ -298,9 +339,18 @@ nr_pdcp_entity_t *new_nr_pdcp_entity(
   }
   ret->is_gnb = is_gnb;
 
-  if (integrity_key != NULL) {
-    printf("%s:%d:%s: TODO\n", __FILE__, __LINE__, __FUNCTION__);
-    exit(1);
+  if (integrity_key != NULL && integrity_algorithm != 0) {
+    if (integrity_algorithm != 2) {
+      LOG_E(PDCP, "FATAL: only nia2 supported for the moment\n");
+      exit(1);
+    }
+    ret->has_integrity = 1;
+    ret->integrity_algorithm = integrity_algorithm;
+    memcpy(ret->integrity_key, integrity_key, 16);
+
+    ret->integrity_context = nr_pdcp_integrity_nia2_init(integrity_key);
+    ret->integrity = nr_pdcp_integrity_nia2_integrity;
+    ret->free_integrity = nr_pdcp_integrity_nia2_free_integrity;
   }
 
   return ret;
diff --git a/openair2/LAYER2/nr_pdcp/nr_pdcp_entity.h b/openair2/LAYER2/nr_pdcp/nr_pdcp_entity.h
index 36e7a540c4..4ae7a45abb 100644
--- a/openair2/LAYER2/nr_pdcp/nr_pdcp_entity.h
+++ b/openair2/LAYER2/nr_pdcp/nr_pdcp_entity.h
@@ -85,7 +85,12 @@ typedef struct nr_pdcp_entity_t {
                  unsigned char *buffer, int length,
                  int bearer, int count, int direction);
   void (*free_security)(void *security_context);
-  /* security algorithms need to know uplink/downlink information
+  void *integrity_context;
+  void (*integrity)(void *integrity_context, unsigned char *out,
+                 unsigned char *buffer, int length,
+                 int bearer, int count, int direction);
+  void (*free_integrity)(void *integrity_context);
+  /* security/integrity algorithms need to know uplink/downlink information
    * which is reverse for gnb and ue, so we need to know if this
    * pdcp entity is for a gnb or an ue
    */
diff --git a/openair2/LAYER2/nr_pdcp/nr_pdcp_integrity_nia2.c b/openair2/LAYER2/nr_pdcp/nr_pdcp_integrity_nia2.c
new file mode 100644
index 0000000000..5553662ac5
--- /dev/null
+++ b/openair2/LAYER2/nr_pdcp/nr_pdcp_integrity_nia2.c
@@ -0,0 +1,81 @@
+/*
+ * 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.1  (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
+ */
+
+#include "nr_pdcp_integrity_nia2.h"
+
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+#include <openssl/cmac.h>
+
+void *nr_pdcp_integrity_nia2_init(unsigned char *integrity_key)
+{
+  CMAC_CTX *ctx;
+
+  ctx = CMAC_CTX_new();
+  if (ctx == NULL) abort();
+
+  CMAC_Init(ctx, integrity_key, 16, EVP_aes_128_cbc(), NULL);
+
+  return ctx;
+}
+
+static void compute_t(unsigned char *t, uint32_t count, int bearer,
+                      int direction)
+{
+  t[0] = (count >> 24) & 255;
+  t[1] = (count >> 16) & 255;
+  t[2] = (count >>  8) & 255;
+  t[3] = (count      ) & 255;
+  t[4] = (bearer << 3) | (direction << 2);
+  memset(&t[5], 0, 8-5);
+}
+
+void nr_pdcp_integrity_nia2_integrity(void *integrity_context,
+                            unsigned char *out,
+                            unsigned char *buffer, int length,
+                            int bearer, int count, int direction)
+{
+  CMAC_CTX *ctx = integrity_context;
+  unsigned char t[8];
+  unsigned char mac[16];
+  size_t maclen;
+
+  /* see 33.401 B.2.3 for the input to 128-EIA2
+   * (which is identical to 128-NIA2, see 33.501 D.3.1.3) */
+  compute_t(t, count, bearer, direction);
+
+  CMAC_Update(ctx, t, 8);
+  CMAC_Update(ctx, buffer, length);
+  CMAC_Final(ctx, mac, &maclen);
+
+  /* AES CMAC (RFC 4493) outputs 128 bits but NR PDCP PDUs have a MAC-I of
+   * 32 bits (see 38.323 6.2). RFC 4493 2.1 says to truncate most significant
+   * bit first (so seems to say 33.401 B.2.3)
+   */
+  memcpy(out, mac, 4);
+}
+
+void nr_pdcp_integrity_nia2_free_integrity(void *integrity_context)
+{
+  CMAC_CTX *ctx = integrity_context;
+  CMAC_CTX_free(ctx);
+}
diff --git a/openair2/LAYER2/nr_pdcp/nr_pdcp_integrity_nia2.h b/openair2/LAYER2/nr_pdcp/nr_pdcp_integrity_nia2.h
new file mode 100644
index 0000000000..9593a2825c
--- /dev/null
+++ b/openair2/LAYER2/nr_pdcp/nr_pdcp_integrity_nia2.h
@@ -0,0 +1,34 @@
+/*
+ * 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.1  (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
+ */
+
+#ifndef _NR_PDCP_INTEGRITY_NIA2_H_
+#define _NR_PDCP_INTEGRITY_NIA2_H_
+
+void *nr_pdcp_integrity_nia2_init(unsigned char *integrity_key);
+
+void nr_pdcp_integrity_nia2_integrity(void *integrity_context,
+                            unsigned char *out,
+                            unsigned char *buffer, int length,
+                            int bearer, int count, int direction);
+
+void nr_pdcp_integrity_nia2_free_integrity(void *integrity_context);
+
+#endif /* _NR_PDCP_INTEGRITY_NIA2_H_ */
diff --git a/openair2/UTIL/OSA/osa_defs.h b/openair2/UTIL/OSA/osa_defs.h
index 6334658683..e845315922 100644
--- a/openair2/UTIL/OSA/osa_defs.h
+++ b/openair2/UTIL/OSA/osa_defs.h
@@ -48,7 +48,8 @@ typedef enum {
   NAS_INT_ALG = 0x02,
   RRC_ENC_ALG = 0x03,
   RRC_INT_ALG = 0x04,
-  UP_ENC_ALG  = 0x05
+  UP_ENC_ALG  = 0x05,
+  UP_INT_ALG  = 0x06
 } algorithm_type_dist_t;
 
 //int derive_keNB(const uint8_t kasme[32], const uint32_t nas_count, uint8_t **keNB);
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band257.tm1.32PRB.usrpx300.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band257.tm1.32PRB.usrpx300.conf
index 25edddde86..8b1079350b 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band257.tm1.32PRB.usrpx300.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band257.tm1.32PRB.usrpx300.conf
@@ -188,7 +188,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band257.tm1.66PRB.usrpn300.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band257.tm1.66PRB.usrpn300.conf
index 182398cd88..3bfcc01703 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band257.tm1.66PRB.usrpn300.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band257.tm1.66PRB.usrpn300.conf
@@ -177,7 +177,7 @@ gNBs =
       nrofUplinkSlots                                               = 10;
       nrofUplinkSymbols                                             = 0;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band261.tm1.32PRB.usrpn300.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band261.tm1.32PRB.usrpn300.conf
index 535c1fce5a..e1bf1b9818 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band261.tm1.32PRB.usrpn300.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band261.tm1.32PRB.usrpn300.conf
@@ -188,7 +188,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band66.tm1.106PRB.usrpn300.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band66.tm1.106PRB.usrpn300.conf
index 632ae126cd..ceba30dacf 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band66.tm1.106PRB.usrpn300.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band66.tm1.106PRB.usrpn300.conf
@@ -186,7 +186,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band66.tm1.106PRB.usrpx300.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band66.tm1.106PRB.usrpx300.conf
index 46868d0185..115ed6f6f0 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band66.tm1.106PRB.usrpx300.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band66.tm1.106PRB.usrpx300.conf
@@ -186,7 +186,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.106PRB.30kHz,usrpb2x0.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.106PRB.30kHz,usrpb2x0.conf
index cb52494230..b83999f25e 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.106PRB.30kHz,usrpb2x0.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.106PRB.30kHz,usrpb2x0.conf
@@ -177,7 +177,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.PTRS.usrpx300.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.PTRS.usrpx300.conf
index a02cf1485a..ffced2055a 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.PTRS.usrpx300.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.PTRS.usrpx300.conf
@@ -186,7 +186,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-      ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.usrpb210.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.usrpb210.conf
index 3b384dd072..b517bea8ee 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.usrpb210.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.usrpb210.conf
@@ -177,7 +177,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.usrpn300.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.usrpn300.conf
index 0ff7e4aa80..7080aff8ea 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.usrpn300.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.usrpn300.conf
@@ -187,7 +187,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.usrpx300.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.usrpx300.conf
index d0775384c3..4039012fe1 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.usrpx300.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.106PRB.usrpx300.conf
@@ -187,7 +187,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;  // -25 dBm/RE is the maximum measured power from USRP corresponding to att_tx = 0 dB
   }
 
   );
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.217PRB.usrpn300.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.217PRB.usrpn300.conf
index d6110e19d5..20e8aabf98 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.217PRB.usrpn300.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.217PRB.usrpn300.conf
@@ -187,7 +187,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.217PRB.usrpx300.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.217PRB.usrpx300.conf
index 07d04e65f9..fc8bda5d53 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.217PRB.usrpx300.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.217PRB.usrpx300.conf
@@ -182,7 +182,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.24PRB.usrpb210.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.24PRB.usrpb210.conf
index 17b7421376..89dda14e20 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.24PRB.usrpb210.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.24PRB.usrpb210.conf
@@ -187,7 +187,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.24PRB.usrpn300.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.24PRB.usrpn300.conf
index 38ce771e06..7954b3c201 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.24PRB.usrpn300.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.24PRB.usrpn300.conf
@@ -187,7 +187,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.24PRB.usrpx300.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.24PRB.usrpx300.conf
index c17174d1b6..bd88ed88c7 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.24PRB.usrpx300.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.24PRB.usrpx300.conf
@@ -187,7 +187,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );
diff --git a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.273PRB.usrpn300.conf b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.273PRB.usrpn300.conf
index a42da67df3..694b78e08d 100644
--- a/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.273PRB.usrpn300.conf
+++ b/targets/PROJECTS/GENERIC-LTE-EPC/CONF/gnb.band78.tm1.273PRB.usrpn300.conf
@@ -187,7 +187,7 @@ gNBs =
       nrofUplinkSlots                                               = 2;
       nrofUplinkSymbols                                             = 4;
 
-  ssPBCH_BlockPower                                             = 10;
+      ssPBCH_BlockPower                                             = -25;
   }
 
   );