diff --git a/CMakeLists.txt b/CMakeLists.txt
index bead502f9..855df3a04 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -55,6 +55,12 @@ if(NOT CMAKE_BUILD_TYPE)
 endif(NOT CMAKE_BUILD_TYPE)
 set(CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} CACHE STRING "")
 
+# Generate CMake to include build information
+configure_file(
+  ${CMAKE_SOURCE_DIR}/cmake/modules/SRSLTEbuildinfo.cmake.in
+  ${CMAKE_BINARY_DIR}/SRSLTEbuildinfo.cmake
+)
+
 ########################################################################
 # Options
 ########################################################################
diff --git a/cmake/modules/SRSLTEbuildinfo.cmake.in b/cmake/modules/SRSLTEbuildinfo.cmake.in
new file mode 100644
index 000000000..d8715afbc
--- /dev/null
+++ b/cmake/modules/SRSLTEbuildinfo.cmake.in
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.6)
+
+execute_process(
+COMMAND git rev-parse --abbrev-ref HEAD
+WORKING_DIRECTORY "@CMAKE_SOURCE_DIR@"
+OUTPUT_VARIABLE GIT_BRANCH
+OUTPUT_STRIP_TRAILING_WHITESPACE
+)
+
+execute_process(
+COMMAND git log -1 --format=%h
+WORKING_DIRECTORY "@CMAKE_SOURCE_DIR@"
+OUTPUT_VARIABLE GIT_COMMIT_HASH
+OUTPUT_STRIP_TRAILING_WHITESPACE
+)
+
+message(STATUS "Generating build_info.h")
+configure_file(
+  @CMAKE_SOURCE_DIR@/lib/include/srslte/build_info.h.in
+  @CMAKE_BINARY_DIR@/lib/include/srslte/build_info.h
+)
diff --git a/lib/examples/cell_measurement.c b/lib/examples/cell_measurement.c
index 6415257f0..573cab255 100644
--- a/lib/examples/cell_measurement.c
+++ b/lib/examples/cell_measurement.c
@@ -299,9 +299,6 @@ int main(int argc, char **argv) {
   
   float rx_gain_offset = 0;
 
-  // Set initial CFO for ue_sync
-  srslte_ue_sync_set_cfo(&ue_sync, cfo); 
-
   /* Main loop */
   while ((sf_cnt < prog_args.nof_subframes || prog_args.nof_subframes == -1) && !go_exit) {
     
diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index e8d4b1d00..b62ae263e 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -90,6 +90,8 @@ typedef struct {
   uint32_t file_nof_prb;
   uint32_t file_nof_ports;
   uint32_t file_cell_id;
+  bool enable_cfo_ref;
+  bool average_subframe;
   char *rf_args; 
   uint32_t rf_nof_rx_ant; 
   double rf_freq; 
@@ -120,7 +122,9 @@ void args_default(prog_args_t *args) {
   args->file_offset_freq = 0; 
   args->rf_args = "";
   args->rf_freq = -1.0;
-  args->rf_nof_rx_ant = 1; 
+  args->rf_nof_rx_ant = 1;
+  args->enable_cfo_ref = false;
+  args->average_subframe = false;
 #ifdef ENABLE_AGC_DEFAULT
   args->rf_gain = -1.0; 
 #else
@@ -137,7 +141,7 @@ void args_default(prog_args_t *args) {
 }
 
 void usage(prog_args_t *args, char *prog) {
-  printf("Usage: %s [agpPoOcildDnruMNv] -f rx_frequency (in Hz) | -i input_file\n", prog);
+  printf("Usage: %s [agpPoOcildFRDnruMNv] -f rx_frequency (in Hz) | -i input_file\n", prog);
 #ifndef DISABLE_RF
   printf("\t-a RF args [Default %s]\n", args->rf_args);
   printf("\t-A Number of RX antennas [Default %d]\n", args->rf_nof_rx_ant);
@@ -158,6 +162,8 @@ void usage(prog_args_t *args, char *prog) {
   printf("\t-r RNTI in Hex [Default 0x%x]\n",args->rnti);
   printf("\t-l Force N_id_2 [Default best]\n");
   printf("\t-C Disable CFO correction [Default %s]\n", args->disable_cfo?"Disabled":"Enabled");
+  printf("\t-F Enable RS-based CFO correction [Default %s]\n", args->enable_cfo_ref?"Disabled":"Enabled");
+  printf("\t-R Average channel estimates on 1 ms [Default %s]\n", args->average_subframe?"Disabled":"Enabled");
   printf("\t-t Add time offset [Default %d]\n", args->time_offset);
 #ifndef DISABLE_GRAPHICS
   printf("\t-d disable plots [Default enabled]\n");
@@ -179,7 +185,7 @@ void usage(prog_args_t *args, char *prog) {
 void parse_args(prog_args_t *args, int argc, char **argv) {
   int opt;
   args_default(args);
-  while ((opt = getopt(argc, argv, "aAoglipPcOCtdDnvrfuUsSZyWMN")) != -1) {
+  while ((opt = getopt(argc, argv, "aAoglipPcOCtdDFRnvrfuUsSZyWMN")) != -1) {
     switch (opt) {
     case 'i':
       args->input_file_name = argv[optind];
@@ -211,6 +217,12 @@ void parse_args(prog_args_t *args, int argc, char **argv) {
     case 'C':
       args->disable_cfo = true;
       break;
+    case 'F':
+      args->enable_cfo_ref = true;
+      break;
+    case 'R':
+      args->average_subframe = true;
+      break;
     case 't':
       args->time_offset = atoi(argv[optind]);
       break;
@@ -533,7 +545,10 @@ int main(int argc, char **argv) {
     fprintf(stderr, "Error initiating UE downlink processing module\n");
     exit(-1);
   }
-  
+
+  srslte_chest_dl_cfo_estimate_enable(&ue_dl.chest, prog_args.enable_cfo_ref, 0xff, 0.005);
+  srslte_chest_dl_average_subframe(&ue_dl.chest, prog_args.average_subframe);
+
   /* Configure downlink receiver for the SI-RNTI since will be the only one we'll use */
   srslte_ue_dl_set_rnti(&ue_dl, prog_args.rnti); 
   
@@ -579,11 +594,8 @@ int main(int argc, char **argv) {
   srslte_ra_dl_dci_t old_dl_dci; 
   bzero(&old_dl_dci, sizeof(srslte_ra_dl_dci_t));
 #endif
-  
-  ue_sync.correct_cfo = !prog_args.disable_cfo;
-  
-  // Set initial CFO for ue_sync
-  srslte_ue_sync_set_cfo(&ue_sync, cfo); 
+
+  ue_sync.cfo_correct_enable = !prog_args.disable_cfo;
   
   srslte_pbch_decode_reset(&ue_mib.pbch);
             
@@ -658,6 +670,7 @@ int main(int argc, char **argv) {
               decode_pdsch = false; 
             }
           }
+
           gettimeofday(&t[1], NULL);
           if (decode_pdsch) {
             if(sfidx != 1 || prog_args.mbsfn_area_id < 0){ // Not an MBSFN subframe
@@ -680,6 +693,12 @@ int main(int argc, char **argv) {
                   }
                 }
               }
+
+              // Feed-back ue_sync with chest_dl CFO estimation
+              if (sfidx == 5 && prog_args.enable_cfo_ref) {
+                srslte_ue_sync_set_cfo_ref(&ue_sync, srslte_chest_dl_get_cfo(&ue_dl.chest));
+              }
+
             }else{ // MBSFN subframe
               n = srslte_ue_dl_decode_mbsfn(&ue_dl, 
                                           sf_buffer,
@@ -776,7 +795,7 @@ int main(int argc, char **argv) {
             /* Print basic Parameters */
             PRINT_LINE("   nof layers: %d", ue_dl.pdsch_cfg.nof_layers);
             PRINT_LINE("nof codewords: %d", SRSLTE_RA_DL_GRANT_NOF_TB(&ue_dl.pdsch_cfg.grant));
-            PRINT_LINE("          CFO: %+5.2f kHz", srslte_ue_sync_get_cfo(&ue_sync) / 1000);
+            PRINT_LINE("          CFO: %+7.2f Hz", srslte_ue_sync_get_cfo(&ue_sync));
             PRINT_LINE("          SNR: %+5.1f dB | %+5.1f dB", 10 * log10(rsrp0 / noise), 10 * log10(rsrp1 / noise));
             PRINT_LINE("           Rb: %6.2f / %6.2f / %6.2f Mbps (net/maximum/processing)", uerate, enodebrate, procrate);
             PRINT_LINE("   PDCCH-Miss: %5.2f%%", 100 * (1 - (float) ue_dl.nof_detected / nof_trials));
@@ -940,7 +959,7 @@ void *plot_thread_run(void *arg) {
     plot_real_init(&pce);
     plot_real_setTitle(&pce, "Channel Response - Magnitude");
     plot_real_setLabels(&pce, "Index", "dB");
-    plot_real_setYAxisScale(&pce, -40, 40);
+    plot_real_setYAxisScale(&pce, -M_PI, M_PI);
     
     plot_real_init(&p_sync);
     plot_real_setTitle(&p_sync, "PSS Cross-Corr abs value");
@@ -976,7 +995,11 @@ void *plot_thread_run(void *arg) {
           tmp_plot2[g+i] = -80;
         }
       }
-      plot_real_setNewData(&pce, tmp_plot2, sz);        
+      uint32_t nrefs = 2*ue_dl.cell.nof_prb;
+      for (i=0;i<nrefs;i++) {
+        tmp_plot2[i] = cargf(ue_dl.chest.tmp_cfo_estimate[i]);
+      }
+      plot_real_setNewData(&pce, tmp_plot2, nrefs);
       
       if (!prog_args.input_file_name) {
         if (plot_track) {
diff --git a/lib/include/srslte/asn1/liblte_mme.h b/lib/include/srslte/asn1/liblte_mme.h
index 15799f7ca..977f309ce 100644
--- a/lib/include/srslte/asn1/liblte_mme.h
+++ b/lib/include/srslte/asn1/liblte_mme.h
@@ -2721,6 +2721,10 @@ typedef struct{
 // Functions
 LIBLTE_ERROR_ENUM liblte_mme_pack_attach_request_msg(LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT *attach_req,
                                                      LIBLTE_BYTE_MSG_STRUCT               *msg);
+LIBLTE_ERROR_ENUM liblte_mme_pack_attach_request_msg(LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT *attach_req,
+                                                     uint8                                 sec_hdr_type,
+                                                     uint32                                count,
+                                                     LIBLTE_BYTE_MSG_STRUCT               *msg);
 LIBLTE_ERROR_ENUM liblte_mme_unpack_attach_request_msg(LIBLTE_BYTE_MSG_STRUCT               *msg,
                                                        LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT *attach_req);
 
diff --git a/lib/include/srslte/build_info.h.in b/lib/include/srslte/build_info.h.in
new file mode 100644
index 000000000..a8acb306b
--- /dev/null
+++ b/lib/include/srslte/build_info.h.in
@@ -0,0 +1,56 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE 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 Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#ifndef BUILD_INFO_
+#define BUILD_INFO_
+
+# ifdef __cplusplus
+extern "C" {
+# endif
+
+#ifdef NDEBUG
+    static char build_mode[] = "Release";
+#else
+    static char build_mode[] = "Debug";
+#endif
+
+// the configured build options for srsLTE
+static char build_info[] = "commit @GIT_COMMIT_HASH@ on branch @GIT_BRANCH@";
+
+SRSLTE_API char* srslte_get_build_info() {
+  return build_info;
+};
+
+SRSLTE_API char* srslte_get_build_mode() {
+  return build_mode;
+}
+
+# ifdef __cplusplus
+}
+# endif
+
+#endif // BUILD_INFO_
diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index 88dfd285c..d382c01e6 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -52,16 +52,20 @@ class ue_interface
 class usim_interface_nas
 {
 public:
-  virtual void get_imsi_vec(uint8_t* imsi_, uint32_t n) = 0;
-  virtual void get_imei_vec(uint8_t* imei_, uint32_t n) = 0;
-  virtual int  get_home_plmn_id(LIBLTE_RRC_PLMN_IDENTITY_STRUCT *home_plmn_id) = 0;
+  virtual std::string get_imsi_str() = 0;
+  virtual std::string get_imei_str() = 0;
+  virtual bool get_imsi_vec(uint8_t* imsi_, uint32_t n) = 0;
+  virtual bool get_imei_vec(uint8_t* imei_, uint32_t n) = 0;
+  virtual bool get_home_plmn_id(LIBLTE_RRC_PLMN_IDENTITY_STRUCT *home_plmn_id) = 0;
   virtual void generate_authentication_response(uint8_t  *rand,
                                                 uint8_t  *autn_enb,
                                                 uint16_t  mcc,
                                                 uint16_t  mnc,
                                                 bool     *net_valid,
-                                                uint8_t  *res) = 0;
-  virtual void generate_nas_keys(uint8_t *k_nas_enc,
+                                                uint8_t  *res,
+                                                uint8_t  *k_asme) = 0;
+  virtual void generate_nas_keys(uint8_t *k_asme,
+                                 uint8_t *k_nas_enc,
                                  uint8_t *k_nas_int,
                                  srslte::CIPHERING_ALGORITHM_ID_ENUM cipher_algo,
                                  srslte::INTEGRITY_ALGORITHM_ID_ENUM integ_algo) = 0;
@@ -71,7 +75,8 @@ public:
 class usim_interface_rrc
 {
 public:
-  virtual void generate_as_keys(uint32_t count_ul,
+  virtual void generate_as_keys(uint8_t *k_asme,
+                                uint32_t count_ul,
                                 uint8_t *k_rrc_enc,
                                 uint8_t *k_rrc_int,
                                 uint8_t *k_up_enc,
@@ -104,6 +109,7 @@ public:
   virtual void      write_pdu(uint32_t lcid, srslte::byte_buffer_t *pdu) = 0;
   virtual uint32_t  get_ul_count() = 0;
   virtual bool      get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi) = 0;
+  virtual bool      get_k_asme(uint8_t *k_asme_, uint32_t n) = 0;
   virtual void      plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) = 0;
   virtual void      plmn_search_end() = 0;
 };
@@ -446,7 +452,15 @@ typedef struct {
   std::string snr_estim_alg; 
   bool cfo_integer_enabled; 
   float cfo_correct_tol_hz;
-  float cfo_ema;
+  float cfo_pss_ema;
+  float cfo_ref_ema;
+  float cfo_loop_bw_pss;
+  float cfo_loop_bw_ref;
+  float cfo_loop_ref_min;
+  float cfo_loop_pss_tol;
+  uint32_t cfo_loop_pss_conv;
+  uint32_t cfo_ref_mask;
+  bool average_subframe_enabled;
   int time_correct_period; 
   bool sfo_correct_disable; 
   std::string sss_algorithm; 
diff --git a/lib/include/srslte/phy/ch_estimation/chest_dl.h b/lib/include/srslte/phy/ch_estimation/chest_dl.h
index 0e43d113c..91fda41e4 100644
--- a/lib/include/srslte/phy/ch_estimation/chest_dl.h
+++ b/lib/include/srslte/phy/ch_estimation/chest_dl.h
@@ -68,7 +68,8 @@ typedef struct {
   cf_t *pilot_estimates_average; 
   cf_t *pilot_recv_signal; 
   cf_t *tmp_noise; 
-  
+  cf_t *tmp_cfo_estimate;
+
 #ifdef FREQ_SEL_SNR  
   float snr_vector[12000];
   float pilot_power[12000];
@@ -82,7 +83,12 @@ typedef struct {
   float rssi[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS]; 
   float rsrp[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS]; 
   float noise_estimate[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
-  
+  float cfo;
+
+  bool     cfo_estimate_enable;
+  uint32_t cfo_estimate_sf_mask;
+  float    cfo_ema;
+
   /* Use PSS for noise estimation in LS linear interpolation mode */
   cf_t pss_signal[SRSLTE_PSS_LEN];
   cf_t tmp_pss[SRSLTE_PSS_LEN];
@@ -91,6 +97,7 @@ typedef struct {
   srslte_chest_dl_noise_alg_t noise_alg; 
   int last_nof_antennas;
 
+  bool average_subframe;
 } srslte_chest_dl_t;
 
 
@@ -144,8 +151,18 @@ SRSLTE_API int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q,
                                              uint32_t port_id, 
                                              uint32_t rxant_id);
 
+SRSLTE_API void srslte_chest_dl_cfo_estimate_enable(srslte_chest_dl_t *q,
+                                                    bool enable,
+                                                    uint32_t mask,
+                                                    float ema);
+
+SRSLTE_API void srslte_chest_dl_average_subframe(srslte_chest_dl_t *q,
+                                                 bool enable);
+
 SRSLTE_API float srslte_chest_dl_get_noise_estimate(srslte_chest_dl_t *q); 
 
+SRSLTE_API float srslte_chest_dl_get_cfo(srslte_chest_dl_t *q);
+
 SRSLTE_API float srslte_chest_dl_get_snr(srslte_chest_dl_t *q);
 
 SRSLTE_API float srslte_chest_dl_get_rssi(srslte_chest_dl_t *q);
diff --git a/lib/include/srslte/phy/dft/dft.h b/lib/include/srslte/phy/dft/dft.h
index b3dd4378b..afe8024d2 100644
--- a/lib/include/srslte/phy/dft/dft.h
+++ b/lib/include/srslte/phy/dft/dft.h
@@ -139,22 +139,22 @@ SRSLTE_API void srslte_dft_plan_set_dc(srslte_dft_plan_t *plan,
 
 /* Compute DFT */
 
-SRSLTE_API void srslte_dft_run(srslte_dft_plan_t *plan, 
-                               void *in, 
+SRSLTE_API void srslte_dft_run(srslte_dft_plan_t *plan,
+                               const void *in,
                                void *out);
 
-SRSLTE_API void srslte_dft_run_c_zerocopy(srslte_dft_plan_t *plan, 
-                                          cf_t *in, 
+SRSLTE_API void srslte_dft_run_c_zerocopy(srslte_dft_plan_t *plan,
+                                          const cf_t *in,
                                           cf_t *out); 
 
-SRSLTE_API void srslte_dft_run_c(srslte_dft_plan_t *plan, 
-                                 cf_t *in, 
+SRSLTE_API void srslte_dft_run_c(srslte_dft_plan_t *plan,
+                                 const cf_t *in,
                                  cf_t *out);
 
 SRSLTE_API void srslte_dft_run_guru_c(srslte_dft_plan_t *plan);
 
 SRSLTE_API void srslte_dft_run_r(srslte_dft_plan_t *plan, 
-                                 float *in, 
+                                 const float *in,
                                  float *out);
 
 #endif // DFT_H_
diff --git a/lib/include/srslte/phy/sync/cfo.h b/lib/include/srslte/phy/sync/cfo.h
index fd2ab4d83..5b9b3c0dd 100644
--- a/lib/include/srslte/phy/sync/cfo.h
+++ b/lib/include/srslte/phy/sync/cfo.h
@@ -66,7 +66,7 @@ SRSLTE_API void srslte_cfo_set_tol(srslte_cfo_t *h,
                                    float tol);
 
 SRSLTE_API void srslte_cfo_correct(srslte_cfo_t *h, 
-                                   cf_t *input,
+                                   const cf_t *input,
                                    cf_t *output,
                                    float freq);
 
diff --git a/lib/include/srslte/phy/sync/cp.h b/lib/include/srslte/phy/sync/cp.h
index 9daf7d4f6..d9da03722 100644
--- a/lib/include/srslte/phy/sync/cp.h
+++ b/lib/include/srslte/phy/sync/cp.h
@@ -47,7 +47,7 @@ SRSLTE_API int srslte_cp_synch_resize(srslte_cp_synch_t *q,
                                       uint32_t symbol_sz);
 
 SRSLTE_API uint32_t srslte_cp_synch(srslte_cp_synch_t *q, 
-                                    cf_t *input, 
+                                    const cf_t *input,
                                     uint32_t max_offset, 
                                     uint32_t nof_symbols, 
                                     uint32_t cp_len);
diff --git a/lib/include/srslte/phy/sync/pss.h b/lib/include/srslte/phy/sync/pss.h
index 9760a178c..aba84f5cd 100644
--- a/lib/include/srslte/phy/sync/pss.h
+++ b/lib/include/srslte/phy/sync/pss.h
@@ -63,16 +63,12 @@
 
 #define SRSLTE_PSS_ACCUMULATE_ABS   // If enabled, accumulates the correlation absolute value on consecutive calls to srslte_pss_synch_find_pss
 
-#define SRSLTE_PSS_ABS_SQUARE   // If enabled, compute abs square, otherwise computes absolute value only 
-
 #define SRSLTE_PSS_RETURN_PSR  // If enabled returns peak to side-lobe ratio, otherwise returns absolute peak value
 
 
 /* Low-level API */
 typedef struct SRSLTE_API {
-  
-  srslte_dft_plan_t dftp_input; 
-  
+
 #ifdef CONVOLUTION_FFT
   srslte_conv_fft_cc_t conv_fft;
   srslte_filt_cc_t filter;
@@ -87,16 +83,23 @@ typedef struct SRSLTE_API {
   uint32_t N_id_2;
   uint32_t fft_size;
   cf_t *pss_signal_freq_full[3];
-  
+
   cf_t *pss_signal_time[3];
-  
+
   cf_t pss_signal_freq[3][SRSLTE_PSS_LEN]; // One sequence for each N_id_2
   cf_t *tmp_input;
   cf_t *conv_output;
   float *conv_output_abs;
-  float ema_alpha; 
+  float ema_alpha;
   float *conv_output_avg;
   float peak_value;
+
+  bool filter_pss_enable;
+  srslte_dft_plan_t dftp_input;
+  srslte_dft_plan_t idftp_input;
+  cf_t tmp_fft[SRSLTE_SYMBOL_SZ_MAX];
+  cf_t tmp_fft2[SRSLTE_SYMBOL_SZ_MAX];
+
 }srslte_pss_synch_t;
 
 typedef enum { PSS_TX, PSS_RX } pss_direction_t;
@@ -128,6 +131,13 @@ SRSLTE_API void srslte_pss_synch_free(srslte_pss_synch_t *q);
 
 SRSLTE_API void srslte_pss_synch_reset(srslte_pss_synch_t *q); 
 
+SRSLTE_API void srslte_pss_synch_filter_enable(srslte_pss_synch_t *q,
+                                               bool enable);
+
+SRSLTE_API void srslte_pss_synch_filter(srslte_pss_synch_t *q,
+                                        const cf_t *input,
+                                        cf_t *output);
+
 SRSLTE_API int srslte_pss_generate(cf_t *signal, 
                                    uint32_t N_id_2);
 
@@ -148,14 +158,14 @@ SRSLTE_API int srslte_pss_synch_set_N_id_2(srslte_pss_synch_t *q,
                                            uint32_t N_id_2);
 
 SRSLTE_API int srslte_pss_synch_find_pss(srslte_pss_synch_t *q, 
-                                         cf_t *input, 
+                                         const cf_t *input,
                                          float *corr_peak_value);
 
 SRSLTE_API int srslte_pss_synch_chest(srslte_pss_synch_t *q, 
-                                      cf_t *input, 
+                                      const cf_t *input,
                                       cf_t ce[SRSLTE_PSS_LEN]); 
 
-SRSLTE_API float srslte_pss_synch_cfo_compute(srslte_pss_synch_t* q, 
-                                              cf_t *pss_recv);
+SRSLTE_API float srslte_pss_synch_cfo_compute(srslte_pss_synch_t* q,
+                                              const cf_t *pss_recv);
 
 #endif // PSS_
diff --git a/lib/include/srslte/phy/sync/sss.h b/lib/include/srslte/phy/sync/sss.h
index 35eedac8f..4a0942d61 100644
--- a/lib/include/srslte/phy/sync/sss.h
+++ b/lib/include/srslte/phy/sync/sss.h
@@ -107,8 +107,8 @@ SRSLTE_API void srslte_sss_put_slot(float *sss,
 SRSLTE_API int srslte_sss_synch_set_N_id_2(srslte_sss_synch_t *q, 
                                            uint32_t N_id_2);
 
-SRSLTE_API int srslte_sss_synch_m0m1_partial(srslte_sss_synch_t *q, 
-                                             cf_t *input, 
+SRSLTE_API int srslte_sss_synch_m0m1_partial(srslte_sss_synch_t *q,
+                                             const cf_t *input,
                                              uint32_t M, 
                                              cf_t ce[2*SRSLTE_SSS_N],
                                              uint32_t *m0, 
@@ -117,15 +117,15 @@ SRSLTE_API int srslte_sss_synch_m0m1_partial(srslte_sss_synch_t *q,
                                              float *m1_value);
 
 SRSLTE_API int srslte_sss_synch_m0m1_diff_coh(srslte_sss_synch_t *q, 
-                                              cf_t *input, 
+                                              const cf_t *input,
                                               cf_t ce[2*SRSLTE_SSS_N],
                                               uint32_t *m0, 
                                               float *m0_value, 
                                               uint32_t *m1, 
                                               float *m1_value);
 
-SRSLTE_API int srslte_sss_synch_m0m1_diff(srslte_sss_synch_t *q, 
-                                          cf_t *input, 
+SRSLTE_API int srslte_sss_synch_m0m1_diff(srslte_sss_synch_t *q,
+                                          const cf_t *input,
                                           uint32_t *m0, 
                                           float *m0_value, 
                                           uint32_t *m1, 
diff --git a/lib/include/srslte/phy/sync/sync.h b/lib/include/srslte/phy/sync/sync.h
index 1c70b2449..151c530b1 100644
--- a/lib/include/srslte/phy/sync/sync.h
+++ b/lib/include/srslte/phy/sync/sync.h
@@ -74,19 +74,8 @@ typedef struct SRSLTE_API {
   uint32_t fft_size;
   uint32_t frame_size;
   uint32_t max_offset;
-  bool enable_cfo_corr;
-  bool mean_cfo2_isunset;
-  bool mean_cfo_isunset;
-  float mean_cfo;
-  float mean_cfo2; 
-  int cfo_i;
-  bool find_cfo_i; 
-  bool find_cfo_i_initiated; 
-  float cfo_ema_alpha;
   uint32_t nof_symbols;
   uint32_t cp_len;
-  srslte_cfo_t cfocorr;
-  srslte_cfo_t cfocorr2;
   float current_cfo_tol;
   sss_alg_t sss_alg; 
   bool detect_cp;
@@ -102,6 +91,30 @@ typedef struct SRSLTE_API {
 
   uint32_t max_frame_size;
 
+
+  // variables for various CFO estimation methods
+  bool cfo_cp_enable;
+  bool cfo_pss_enable;
+  bool cfo_i_enable;
+
+  bool cfo_cp_is_set;
+  bool cfo_pss_is_set;
+  bool cfo_i_initiated;
+
+  float cfo_cp_mean;
+  float cfo_pss_mean;
+  int   cfo_i_value;
+
+  float cfo_ema_alpha;
+
+  srslte_cfo_t cfo_corr_frame;
+  srslte_cfo_t cfo_corr_symbol;
+
+  bool sss_filtering_enabled;
+  bool pss_filtering_enabled;
+  cf_t sss_filt[SRSLTE_SYMBOL_SZ_MAX];
+  cf_t pss_filt[SRSLTE_SYMBOL_SZ_MAX];
+
 }srslte_sync_t;
 
 typedef enum {
@@ -135,29 +148,23 @@ SRSLTE_API void srslte_sync_reset(srslte_sync_t *q);
 
 /* Finds a correlation peak in the input signal around position find_offset */
 SRSLTE_API srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, 
-                                                   cf_t *input,
+                                                   const cf_t *input,
                                                    uint32_t find_offset,
                                                    uint32_t *peak_position);
 
 /* Estimates the CP length */
 SRSLTE_API srslte_cp_t srslte_sync_detect_cp(srslte_sync_t *q, 
-                                             cf_t *input, 
+                                             const cf_t *input,
                                              uint32_t peak_pos);
 
 /* Sets the threshold for peak comparison */
 SRSLTE_API void srslte_sync_set_threshold(srslte_sync_t *q, 
                                           float threshold);
 
-SRSLTE_API void srslte_sync_set_cfo_tol(srslte_sync_t *q,
-                                        float tol);
-
 /* Gets the subframe idx (0 or 5) */
 SRSLTE_API uint32_t srslte_sync_get_sf_idx(srslte_sync_t *q);
 
-/* Gets the last peak value */
-SRSLTE_API float srslte_sync_get_last_peak_value(srslte_sync_t *q);
-
-/* Gets the mean peak value */
+/* Gets the peak value */
 SRSLTE_API float srslte_sync_get_peak_value(srslte_sync_t *q);
 
 /* Choose SSS detection algorithm */
@@ -175,23 +182,40 @@ SRSLTE_API int srslte_sync_set_N_id_2(srslte_sync_t *q,
 /* Gets the Physical CellId from the last call to synch_run() */
 SRSLTE_API int srslte_sync_get_cell_id(srslte_sync_t *q);
 
+/* Enables/disables filtering of the central PRBs before PSS CFO estimation or SSS correlation*/
+SRSLTE_API void srslte_sync_set_pss_filt_enable(srslte_sync_t *q,
+                                                bool enable);
+
+SRSLTE_API void srslte_sync_set_sss_filt_enable(srslte_sync_t *q,
+                                                bool enable);
+
 /* Gets the CFO estimation from the last call to synch_run() */
 SRSLTE_API float srslte_sync_get_cfo(srslte_sync_t *q);
 
-/* Sets known CFO to avoid long transients due to average */
-SRSLTE_API void srslte_sync_set_cfo(srslte_sync_t *q, float cfo);
+/* Resets internal CFO state */
+SRSLTE_API void srslte_sync_cfo_reset(srslte_sync_t *q);
+
+/* Copies CFO internal state from another object to avoid long transients */
+SRSLTE_API void srslte_sync_copy_cfo(srslte_sync_t *q,
+                                     srslte_sync_t *src_obj);
 
-/* Set integer CFO */
-SRSLTE_API void srslte_sync_set_cfo_i(srslte_sync_t *q, 
-                                      int cfo_i); 
+/* Enable different CFO estimation stages */
+SRSLTE_API void srslte_sync_set_cfo_i_enable(srslte_sync_t *q,
+                                             bool enable);
+SRSLTE_API void srslte_sync_set_cfo_cp_enable(srslte_sync_t *q,
+                                              bool enable);
+SRSLTE_API void srslte_sync_set_cfo_pss_enable(srslte_sync_t *q,
+                                               bool enable);
 
-SRSLTE_API void srslte_sync_set_cfo_enable(srslte_sync_t *q, 
-                                           bool enable); 
+/* Sets CFO correctors tolerance (in Hz) */
+SRSLTE_API void srslte_sync_set_cfo_tol(srslte_sync_t *q,
+                                        float tol);
 
 /* Sets the exponential moving average coefficient for CFO averaging */
 SRSLTE_API void srslte_sync_set_cfo_ema_alpha(srslte_sync_t *q, 
                                               float alpha);
 
+
 /* Gets the CP length estimation from the last call to synch_run() */
 SRSLTE_API srslte_cp_t srslte_sync_get_cp(srslte_sync_t *q);
 
@@ -199,10 +223,6 @@ SRSLTE_API srslte_cp_t srslte_sync_get_cp(srslte_sync_t *q);
 SRSLTE_API void srslte_sync_set_cp(srslte_sync_t *q, 
                                    srslte_cp_t cp);
 
-/* Enable integer CFO detection */
-SRSLTE_API void srslte_sync_cfo_i_detec_en(srslte_sync_t *q, 
-                                           bool enabled); 
-
 /* Enables/Disables SSS detection  */
 SRSLTE_API void srslte_sync_sss_en(srslte_sync_t *q, 
                                    bool enabled);
@@ -211,8 +231,6 @@ SRSLTE_API srslte_pss_synch_t* srslte_sync_get_cur_pss_obj(srslte_sync_t *q);
 
 SRSLTE_API bool srslte_sync_sss_detected(srslte_sync_t *q);
 
-SRSLTE_API bool srslte_sync_sss_is_en(srslte_sync_t *q); 
-
 /* Enables/Disables CP detection  */
 SRSLTE_API void srslte_sync_cp_en(srslte_sync_t *q, 
                                   bool enabled);
diff --git a/lib/include/srslte/phy/ue/ue_sync.h b/lib/include/srslte/phy/ue/ue_sync.h
index e5877dd23..16936c062 100644
--- a/lib/include/srslte/phy/ue/ue_sync.h
+++ b/lib/include/srslte/phy/ue/ue_sync.h
@@ -110,8 +110,20 @@ typedef struct SRSLTE_API {
   uint32_t sf_idx;
       
   bool decode_sss_on_track; 
-  bool correct_cfo; 
-  
+
+  bool  cfo_is_copied;
+  bool  cfo_correct_enable;
+  float cfo_current_value;
+  float cfo_loop_bw_pss;
+  float cfo_loop_bw_ref;
+  float cfo_pss_min;
+  float cfo_ref_min;
+  float cfo_ref_max;
+
+  uint32_t pss_stable_cnt;
+  uint32_t pss_stable_timeout;
+  bool     pss_is_stable;
+
   uint32_t peak_idx;
   int next_rf_sample_offset;
   int last_sample_offset; 
@@ -165,6 +177,10 @@ SRSLTE_API void srslte_ue_sync_free(srslte_ue_sync_t *q);
 SRSLTE_API int srslte_ue_sync_set_cell(srslte_ue_sync_t *q,
                                        srslte_cell_t cell);
 
+SRSLTE_API void srslte_ue_sync_cfo_reset(srslte_ue_sync_t *q);
+
+SRSLTE_API void srslte_ue_sync_reset(srslte_ue_sync_t *q);
+
 SRSLTE_API int srslte_ue_sync_start_agc(srslte_ue_sync_t *q, 
                                         double (set_gain_callback)(void*, double), 
                                         float init_gain_value); 
@@ -175,34 +191,40 @@ SRSLTE_API void srslte_ue_sync_set_agc_period(srslte_ue_sync_t *q,
                                               uint32_t period); 
 
 /* CAUTION: input_buffer MUST have space for 2 subframes */
-SRSLTE_API int srslte_ue_sync_zerocopy(srslte_ue_sync_t *q, 
+SRSLTE_API int srslte_ue_sync_zerocopy(srslte_ue_sync_t *q,
                                        cf_t *input_buffer);
 
-SRSLTE_API int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, 
+SRSLTE_API int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q,
                                              cf_t *input_buffer[SRSLTE_MAX_PORTS]);
 
 SRSLTE_API void srslte_ue_sync_set_cfo_tol(srslte_ue_sync_t *q,
                                            float tol);
 
-SRSLTE_API void srslte_ue_sync_set_cfo(srslte_ue_sync_t *q, 
-                                       float cfo);
+SRSLTE_API void srslte_ue_sync_copy_cfo(srslte_ue_sync_t *q,
+                                        srslte_ue_sync_t *src_obj);
+
+SRSLTE_API void srslte_ue_sync_set_cfo_loop_bw(srslte_ue_sync_t *q,
+                                               float bw_pss,
+                                               float bw_ref,
+                                               float pss_tol,
+                                               float ref_tol,
+                                               float ref_max,
+                                               uint32_t pss_stable_timeout);
 
 SRSLTE_API void srslte_ue_sync_set_cfo_ema(srslte_ue_sync_t *q,
                                            float ema);
 
-SRSLTE_API void srslte_ue_sync_cfo_i_detec_en(srslte_ue_sync_t *q, 
-                                              bool enable); 
+SRSLTE_API void srslte_ue_sync_set_cfo_ref(srslte_ue_sync_t *q, float res_cfo);
 
-SRSLTE_API void srslte_ue_sync_reset(srslte_ue_sync_t *q);
+SRSLTE_API void srslte_ue_sync_set_cfo_i_enable(srslte_ue_sync_t *q,
+                                                bool enable);
 
-SRSLTE_API void srslte_ue_sync_set_N_id_2(srslte_ue_sync_t *q, 
+SRSLTE_API void srslte_ue_sync_set_N_id_2(srslte_ue_sync_t *q,
                                           uint32_t N_id_2);
 
 SRSLTE_API void srslte_ue_sync_decode_sss_on_track(srslte_ue_sync_t *q, 
                                                    bool enabled);
 
-SRSLTE_API srslte_ue_sync_state_t srslte_ue_sync_get_state(srslte_ue_sync_t *q);
-
 SRSLTE_API uint32_t srslte_ue_sync_get_sfidx(srslte_ue_sync_t *q);
 
 SRSLTE_API float srslte_ue_sync_get_cfo(srslte_ue_sync_t *q);
@@ -218,8 +240,5 @@ SRSLTE_API void srslte_ue_sync_get_last_timestamp(srslte_ue_sync_t *q,
                                                   srslte_timestamp_t *timestamp);
 
 
-
-
-
 #endif // SYNC_FRAME_
 
diff --git a/lib/include/srslte/phy/utils/convolution.h b/lib/include/srslte/phy/utils/convolution.h
index fcb321ce5..a93dfb955 100644
--- a/lib/include/srslte/phy/utils/convolution.h
+++ b/lib/include/srslte/phy/utils/convolution.h
@@ -66,18 +66,18 @@ SRSLTE_API int srslte_conv_fft_cc_replan(srslte_conv_fft_cc_t *q,
 
 SRSLTE_API void srslte_conv_fft_cc_free(srslte_conv_fft_cc_t *q);
 
-SRSLTE_API uint32_t srslte_conv_fft_cc_run(srslte_conv_fft_cc_t *q, 
-                                           cf_t *input, 
-                                           cf_t *filter, 
+SRSLTE_API uint32_t srslte_conv_fft_cc_run(srslte_conv_fft_cc_t *q,
+                                           const cf_t *input,
+                                           const cf_t *filter,
                                            cf_t *output);
 
-SRSLTE_API uint32_t srslte_conv_fft_cc_run_opt(srslte_conv_fft_cc_t *q, 
-                                           cf_t *input, 
-                                           cf_t *filter_freq,
+SRSLTE_API uint32_t srslte_conv_fft_cc_run_opt(srslte_conv_fft_cc_t *q,
+                                               const cf_t *input,
+                                               const cf_t *filter_freq,
                                            cf_t *output);
 
-SRSLTE_API uint32_t srslte_conv_cc(cf_t *input, 
-                                   cf_t *filter, 
+SRSLTE_API uint32_t srslte_conv_cc(const cf_t *input,
+                                   const cf_t *filter,
                                    cf_t *output, 
                                    uint32_t input_len, 
                                    uint32_t filter_len);
diff --git a/lib/include/srslte/phy/utils/simd.h b/lib/include/srslte/phy/utils/simd.h
index a9a79c486..226e2ecae 100644
--- a/lib/include/srslte/phy/utils/simd.h
+++ b/lib/include/srslte/phy/utils/simd.h
@@ -172,7 +172,7 @@ typedef float32x4_t simd_f_t;
 #endif /* LV_HAVE_AVX512 */
 
 /* Single precision Floating point functions */
-static inline simd_f_t srslte_simd_f_load(float *ptr) {
+static inline simd_f_t srslte_simd_f_load(const float *ptr) {
 #ifdef LV_HAVE_AVX512
   return _mm512_load_ps(ptr);
 #else /* LV_HAVE_AVX512 */
@@ -190,7 +190,7 @@ static inline simd_f_t srslte_simd_f_load(float *ptr) {
 #endif /* LV_HAVE_AVX512 */
 }
 
-static inline simd_f_t srslte_simd_f_loadu(float *ptr) {
+static inline simd_f_t srslte_simd_f_loadu(const float *ptr) {
 #ifdef LV_HAVE_AVX512
   return _mm512_loadu_ps(ptr);
 #else /* LV_HAVE_AVX512 */
@@ -477,7 +477,7 @@ typedef struct {
 #endif
 
 /* Complex Single precission Floating point functions */
-static inline simd_cf_t srslte_simd_cfi_load(cf_t *ptr) {
+static inline simd_cf_t srslte_simd_cfi_load(const cf_t *ptr) {
   simd_cf_t ret;
 #ifdef LV_HAVE_AVX512
   __m512 in1 = _mm512_load_ps((float*)(ptr));
@@ -513,7 +513,7 @@ static inline simd_cf_t srslte_simd_cfi_load(cf_t *ptr) {
 }
 
 /* Complex Single precission Floating point functions */
-static inline simd_cf_t srslte_simd_cfi_loadu(cf_t *ptr) {
+static inline simd_cf_t srslte_simd_cfi_loadu(const cf_t *ptr) {
   simd_cf_t ret;
 #ifdef LV_HAVE_AVX512
   __m512 in1 = _mm512_loadu_ps((float*)(ptr));
@@ -548,7 +548,7 @@ static inline simd_cf_t srslte_simd_cfi_loadu(cf_t *ptr) {
   return ret;
 }
 
-static inline simd_cf_t srslte_simd_cf_load(float *re, float *im) {
+static inline simd_cf_t srslte_simd_cf_load(const float *re, const float *im) {
   simd_cf_t ret;
 #ifdef LV_HAVE_AVX512
   ret.re = _mm512_load_ps(re);
@@ -572,7 +572,7 @@ static inline simd_cf_t srslte_simd_cf_load(float *re, float *im) {
   return ret;
 }
 
-static inline simd_cf_t srslte_simd_cf_loadu(float *re, float *im) {
+static inline simd_cf_t srslte_simd_cf_loadu(const float *re, const float *im) {
   simd_cf_t ret;
 #ifdef LV_HAVE_AVX512
   ret.re = _mm512_loadu_ps(re);
@@ -1074,7 +1074,7 @@ typedef int16x8_t simd_s_t;
 #endif /* LV_HAVE_AVX2 */
 #endif /* LV_HAVE_AVX512 */
 
-static inline simd_s_t srslte_simd_s_load(int16_t *ptr) {
+static inline simd_s_t srslte_simd_s_load(const int16_t *ptr) {
 #ifdef LV_HAVE_AVX512
   return _mm512_load_si512(ptr);
 #else /* LV_HAVE_AVX512 */
@@ -1092,7 +1092,7 @@ static inline simd_s_t srslte_simd_s_load(int16_t *ptr) {
 #endif /* LV_HAVE_AVX512 */
 }
 
-static inline simd_s_t srslte_simd_s_loadu(int16_t *ptr) {
+static inline simd_s_t srslte_simd_s_loadu(const int16_t *ptr) {
 #ifdef LV_HAVE_AVX512
   return _mm512_loadu_si512(ptr);
 #else /* LV_HAVE_AVX512 */
@@ -1267,7 +1267,7 @@ typedef
 } simd_c16_t;
 
 /* Fixed point precision (16-bit) functions */
-static inline simd_c16_t srslte_simd_c16i_load(c16_t *ptr) {
+static inline simd_c16_t srslte_simd_c16i_load(const c16_t *ptr) {
   simd_c16_t ret;
 #ifdef LV_HAVE_AVX512
   __m512i in1 = _mm512_load_si512((__m512i*)(ptr));
@@ -1296,7 +1296,7 @@ static inline simd_c16_t srslte_simd_c16i_load(c16_t *ptr) {
   return ret;
 }
 
-static inline simd_c16_t srslte_simd_c16_load(int16_t *re, int16_t *im) {
+static inline simd_c16_t srslte_simd_c16_load(const int16_t *re, const int16_t *im) {
   simd_c16_t ret;
 #ifdef LV_HAVE_AVX2
   ret.re.m256 = _mm256_load_si256((__m256i*)(re));
@@ -1315,7 +1315,7 @@ static inline simd_c16_t srslte_simd_c16_load(int16_t *re, int16_t *im) {
   return ret;
 }
 
-static inline simd_c16_t srslte_simd_c16_loadu(int16_t *re, int16_t *im) {
+static inline simd_c16_t srslte_simd_c16_loadu(const int16_t *re, const int16_t *im) {
   simd_c16_t ret;
 #ifdef LV_HAVE_AVX2
   ret.re.m256 = _mm256_loadu_si256((__m256i*)(re));
@@ -1506,4 +1506,119 @@ static inline simd_s_t srslte_simd_convert_2f_s(simd_f_t a, simd_f_t b) {
 
 #endif /* SRSLTE_SIMD_F_SIZE && SRSLTE_SIMD_C16_SIZE */
 
+#if SRSLTE_SIMD_B_SIZE
+/* Data types */
+#ifdef LV_HAVE_AVX512
+typedef __m512i simd_b_t;
+#else /* LV_HAVE_AVX512 */
+#ifdef LV_HAVE_AVX2
+typedef __m256i simd_b_t;
+#else /* LV_HAVE_AVX2 */
+#ifdef LV_HAVE_SSE
+typedef __m128i simd_b_t;
+#else /* HAVE_NEON */
+#ifdef HAVE_NEON
+typedef int8x16_t simd_b_t;
+#endif /* HAVE_NEON */
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX2 */
+#endif /* LV_HAVE_AVX512 */
+
+
+
+static inline simd_b_t srslte_simd_b_load(int8_t *ptr){
+#ifdef LV_HAVE_AVX512
+  return _mm512_load_si512(ptr);
+#else /* LV_HAVE_AVX512 */
+#ifdef LV_HAVE_AVX2
+  return _mm256_load_si256((__m256i*) ptr);
+#else /* LV_HAVE_AVX2 */
+#ifdef LV_HAVE_SSE
+  return _mm_load_si128((__m128i*) ptr);
+#else /* LV_HAVE_SSE */
+#ifdef HAVE_NEON
+  return vld1q_s8(ptr);
+#endif /* HAVE_NEON */
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX2 */
+#endif /* LV_HAVE_AVX512 */
+}  
+  
+static inline simd_b_t srslte_simd_b_loadu(int8_t *ptr){
+#ifdef LV_HAVE_AVX512
+  return _mm512_loadu_si512(ptr);
+#else /* LV_HAVE_AVX512 */
+#ifdef LV_HAVE_AVX2
+  return _mm256_loadu_si256((__m256i*) ptr);
+#else /* LV_HAVE_AVX2 */
+#ifdef LV_HAVE_SSE
+  return _mm_loadu_si128((__m128i*) ptr);
+#else /* LV_HAVE_SSE */
+#ifdef HAVE_NEON
+  return vld1q_s8(ptr);
+#endif /* HAVE_NEON */
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX2 */
+#endif /* LV_HAVE_AVX512 */
+}
+
+static inline void srslte_simd_b_store(int8_t *ptr, simd_b_t simdreg) {
+#ifdef LV_HAVE_AVX512
+  _mm512_store_si512(ptr, simdreg);
+#else /* LV_HAVE_AVX512 */
+#ifdef LV_HAVE_AVX2
+  _mm256_store_si256((__m256i*) ptr, simdreg);
+#else /* LV_HAVE_AVX2 */
+#ifdef LV_HAVE_SSE
+  _mm_store_si128((__m128i*) ptr, simdreg);
+#else /* LV_HAVE_SSE */
+#ifdef HAVE_NEON
+  vst1q_s8( ptr, simdreg);
+#endif /* HAVE_NEON */
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX2 */
+#endif /* LV_HAVE_AVX512 */
+}
+
+static inline void srslte_simd_b_storeu(int8_t *ptr, simd_b_t simdreg) {
+#ifdef LV_HAVE_AVX512
+  _mm512_storeu_si512(ptr, simdreg);
+#else /* LV_HAVE_AVX512 */
+#ifdef LV_HAVE_AVX2
+  _mm256_storeu_si256((__m256i*) ptr, simdreg);
+#else /* LV_HAVE_AVX2 */
+#ifdef LV_HAVE_SSE
+  _mm_storeu_si128((__m128i*) ptr, simdreg);
+#else /* LV_HAVE_SSE */
+#ifdef HAVE_NEON
+  vst1q_s8(ptr, simdreg);
+#endif /* HAVE_NEON */
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX2 */
+#endif /* LV_HAVE_AVX512 */
+}
+
+
+static inline simd_b_t srslte_simd_b_xor(simd_b_t a, simd_b_t b) {
+    
+#ifdef LV_HAVE_AVX512
+ return _mm512_xor_epi32(a, b);
+#else /* LV_HAVE_AVX512 */
+#ifdef LV_HAVE_AVX2
+ return _mm256_xor_si256(a, b);
+#else /* LV_HAVE_AVX2 */
+#ifdef LV_HAVE_SSE
+ return _mm_xor_si128 (a, b);
+#else /* LV_HAVE_SSE */
+#ifdef HAVE_NEON
+ return veorq_s8(a, b); 
+#endif /* HAVE_NEON */
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX2 */
+#endif /* LV_HAVE_AVX512 */
+}
+
+#endif /*SRSLTE_SIMD_B_SIZE */
+
+
 #endif //SRSLTE_SIMD_H_H
diff --git a/lib/include/srslte/phy/utils/vector.h b/lib/include/srslte/phy/utils/vector.h
index 4a5daefb3..a4028e495 100644
--- a/lib/include/srslte/phy/utils/vector.h
+++ b/lib/include/srslte/phy/utils/vector.h
@@ -53,98 +53,102 @@ extern "C" {
 // Exponential moving average
 #define SRSLTE_VEC_EMA(data, average, alpha) ((alpha)*(data)+(1-alpha)*(average))
 
+
+/*logical operations */
+SRSLTE_API void srslte_vec_xor_bbb(int8_t *x,int8_t *y,int8_t *z, const uint32_t len);
+
 /** Return the sum of all the elements */
-SRSLTE_API float srslte_vec_acc_ff(float *x, uint32_t len);
-SRSLTE_API cf_t srslte_vec_acc_cc(cf_t *x, uint32_t len);
+SRSLTE_API float srslte_vec_acc_ff(const float *x, const uint32_t len);
+SRSLTE_API cf_t srslte_vec_acc_cc(const cf_t *x, const uint32_t len);
 
 SRSLTE_API void *srslte_vec_malloc(uint32_t size);
 
 SRSLTE_API void *srslte_vec_realloc(void *ptr, uint32_t old_size, uint32_t new_size);
 
 /* print vectors */
-SRSLTE_API void srslte_vec_fprint_c(FILE *stream, cf_t *x, uint32_t len);
-SRSLTE_API void srslte_vec_fprint_f(FILE *stream, float *x, uint32_t len);
-SRSLTE_API void srslte_vec_fprint_b(FILE *stream, uint8_t *x, uint32_t len);
-SRSLTE_API void srslte_vec_fprint_byte(FILE *stream, uint8_t *x, uint32_t len);
-SRSLTE_API void srslte_vec_fprint_i(FILE *stream, int *x, uint32_t len);
-SRSLTE_API void srslte_vec_fprint_s(FILE *stream, short *x, uint32_t len); 
-SRSLTE_API void srslte_vec_fprint_hex(FILE *stream, uint8_t *x, uint32_t len);
-SRSLTE_API void srslte_vec_sprint_hex(char *str, uint8_t *x, uint32_t len); 
+SRSLTE_API void srslte_vec_fprint_c(FILE *stream, cf_t *x, const uint32_t len);
+SRSLTE_API void srslte_vec_fprint_f(FILE *stream, float *x, const uint32_t len);
+SRSLTE_API void srslte_vec_fprint_b(FILE *stream, uint8_t *x, const uint32_t len);
+SRSLTE_API void srslte_vec_fprint_byte(FILE *stream, uint8_t *x, const uint32_t len);
+SRSLTE_API void srslte_vec_fprint_i(FILE *stream, int *x, const uint32_t len);
+SRSLTE_API void srslte_vec_fprint_s(FILE *stream, short *x, const uint32_t len); 
+SRSLTE_API void srslte_vec_fprint_hex(FILE *stream, uint8_t *x, const uint32_t len);
+SRSLTE_API void srslte_vec_sprint_hex(char *str, uint8_t *x, const uint32_t len); 
 
 /* Saves/loads a vector to a file */
-SRSLTE_API void srslte_vec_save_file(char *filename, void *buffer, uint32_t len);
-SRSLTE_API void srslte_vec_load_file(char *filename, void *buffer, uint32_t len);
+SRSLTE_API void srslte_vec_save_file(char *filename, const void *buffer, const uint32_t len);
+SRSLTE_API void srslte_vec_load_file(char *filename, void *buffer, const uint32_t len);
 
 /* sum two vectors */
-SRSLTE_API void srslte_vec_sum_fff(float *x, float *y, float *z, uint32_t len);
-SRSLTE_API void srslte_vec_sum_ccc(cf_t *x, cf_t *y, cf_t *z, uint32_t len);
-SRSLTE_API void srslte_vec_sub_sss(int16_t *x, int16_t *y, int16_t *z, uint32_t len);
-SRSLTE_API void srslte_vec_sum_sss(int16_t *x, int16_t *y, int16_t *z, uint32_t len);
+SRSLTE_API void srslte_vec_sum_fff(const float *x, const float *y, float *z, const uint32_t len);
+SRSLTE_API void srslte_vec_sum_ccc(const cf_t *x, const cf_t *y, cf_t *z, const uint32_t len);
+SRSLTE_API void srslte_vec_sub_sss(const int16_t *x, const int16_t *y, int16_t *z, const uint32_t len);
+SRSLTE_API void srslte_vec_sum_sss(const int16_t *x, const int16_t *y, int16_t *z, const uint32_t len);
 
 /* substract two vectors z=x-y */
-SRSLTE_API void srslte_vec_sub_fff(float *x, float *y, float *z, uint32_t len); 
-SRSLTE_API void srslte_vec_sub_ccc(cf_t *x, cf_t *y, cf_t *z, uint32_t len);
+SRSLTE_API void srslte_vec_sub_fff(const float *x, const float *y, float *z, const uint32_t len);
+SRSLTE_API void srslte_vec_sub_ccc(const cf_t *x, const cf_t *y, cf_t *z, const uint32_t len);
 
 /* scalar product */
-SRSLTE_API void srslte_vec_sc_prod_cfc(cf_t *x, float h, cf_t *z, uint32_t len);
-SRSLTE_API void srslte_vec_sc_prod_ccc(cf_t *x, cf_t h, cf_t *z, uint32_t len);
-SRSLTE_API void srslte_vec_sc_prod_fff(float *x, float h, float *z, uint32_t len); 
+SRSLTE_API void srslte_vec_sc_prod_cfc(const cf_t *x, const float h, cf_t *z, const uint32_t len);
+SRSLTE_API void srslte_vec_sc_prod_ccc(const cf_t *x, const cf_t h, cf_t *z, const uint32_t len);
+SRSLTE_API void srslte_vec_sc_prod_fff(const float *x, const float h, float *z, const uint32_t len);
 
 
-SRSLTE_API void srslte_vec_convert_fi(float *x, int16_t *z, float scale, uint32_t len);
-SRSLTE_API void srslte_vec_convert_if(int16_t *x, float *z, float scale, uint32_t len);
+SRSLTE_API void srslte_vec_convert_fi(const float *x, const float scale, int16_t *z, const uint32_t len);
+SRSLTE_API void srslte_vec_convert_if(const int16_t *x, const float scale, float *z, const uint32_t len);
 
-SRSLTE_API void srslte_vec_lut_sss(short *x, unsigned short *lut, short *y, uint32_t len);
+SRSLTE_API void srslte_vec_lut_sss(const short *x, const unsigned short *lut, short *y, const uint32_t len);
 
 /* vector product (element-wise) */
-SRSLTE_API void srslte_vec_prod_ccc(cf_t *x, cf_t *y, cf_t *z, uint32_t len);
-SRSLTE_API void srslte_vec_prod_ccc_split(float *x_re, float *x_im, float *y_re, float *y_im, float *z_re, float *z_im, uint32_t len);
+SRSLTE_API void srslte_vec_prod_ccc(const cf_t *x, const cf_t *y, cf_t *z, const uint32_t len);
+SRSLTE_API void srslte_vec_prod_ccc_split(const float *x_re, const float *x_im, const float *y_re, const float *y_im, float *z_re, float *z_im, const uint32_t len);
 
 /* vector product (element-wise) */
-SRSLTE_API void srslte_vec_prod_cfc(cf_t *x, float *y, cf_t *z, uint32_t len);
+SRSLTE_API void srslte_vec_prod_cfc(const cf_t *x, const float *y, cf_t *z, const uint32_t len);
 
 /* conjugate vector product (element-wise) */
-SRSLTE_API void srslte_vec_prod_conj_ccc(cf_t *x, cf_t *y, cf_t *z, uint32_t len);
+SRSLTE_API void srslte_vec_prod_conj_ccc(const cf_t *x, const cf_t *y, cf_t *z, const uint32_t len);
 
 /* real vector product (element-wise) */
-SRSLTE_API void srslte_vec_prod_fff(float *x, float *y, float *z, uint32_t len);
-SRSLTE_API void srslte_vec_prod_sss(int16_t *x, int16_t *y, int16_t *z, uint32_t len);
+SRSLTE_API void srslte_vec_prod_fff(const float *x, const float *y, float *z, const uint32_t len);
+SRSLTE_API void srslte_vec_prod_sss(const int16_t *x, const int16_t *y, int16_t *z, const uint32_t len);
 
 /* Dot-product */
-SRSLTE_API cf_t srslte_vec_dot_prod_cfc(cf_t *x, float *y, uint32_t len);
-SRSLTE_API cf_t srslte_vec_dot_prod_ccc(cf_t *x, cf_t *y, uint32_t len);
-SRSLTE_API cf_t srslte_vec_dot_prod_conj_ccc(cf_t *x, cf_t *y, uint32_t len);
-SRSLTE_API float srslte_vec_dot_prod_fff(float *x, float *y, uint32_t len);
-SRSLTE_API int32_t srslte_vec_dot_prod_sss(int16_t *x, int16_t *y, uint32_t len); 
+SRSLTE_API cf_t srslte_vec_dot_prod_cfc(const cf_t *x, const float *y, const uint32_t len);
+SRSLTE_API cf_t srslte_vec_dot_prod_ccc(const cf_t *x, const cf_t *y, const uint32_t len);
+SRSLTE_API cf_t srslte_vec_dot_prod_conj_ccc(const cf_t *x, const cf_t *y, const uint32_t len);
+SRSLTE_API float srslte_vec_dot_prod_fff(const float *x, const float *y, const uint32_t len);
+SRSLTE_API int32_t srslte_vec_dot_prod_sss(const int16_t *x, const int16_t *y, const uint32_t len); 
 
 /* z=x/y vector division (element-wise) */
-SRSLTE_API void srslte_vec_div_ccc(cf_t *x, cf_t *y, cf_t *z, uint32_t len);
-SRSLTE_API void srslte_vec_div_cfc(cf_t *x, float *y, cf_t *z, uint32_t len);
-SRSLTE_API void srslte_vec_div_fff(float *x, float *y, float *z, uint32_t len);
+SRSLTE_API void srslte_vec_div_ccc(const cf_t *x, const cf_t *y, cf_t *z, const uint32_t len);
+SRSLTE_API void srslte_vec_div_cfc(const cf_t *x, const float *y, cf_t *z, const uint32_t len);
+SRSLTE_API void srslte_vec_div_fff(const float *x, const float *y, float *z, const uint32_t len);
 
 /* conjugate */
-SRSLTE_API void srslte_vec_conj_cc(cf_t *x, cf_t *y, uint32_t len);
+SRSLTE_API void srslte_vec_conj_cc(const cf_t *x, cf_t *y, const uint32_t len);
 
 /* average vector power */
-SRSLTE_API float srslte_vec_avg_power_cf(cf_t *x, uint32_t len);
+SRSLTE_API float srslte_vec_avg_power_cf(const cf_t *x, const uint32_t len);
 
 /* Correlation between complex vectors x and y */
-SRSLTE_API float srslte_vec_corr_ccc(cf_t *x, cf_t *y, uint32_t len);
+SRSLTE_API float srslte_vec_corr_ccc(const cf_t *x, cf_t *y, const uint32_t len);
 
 /* return the index of the maximum value in the vector */
-SRSLTE_API uint32_t srslte_vec_max_fi(float *x, uint32_t len);
-SRSLTE_API uint32_t srslte_vec_max_abs_ci(cf_t *x, uint32_t len);
+SRSLTE_API uint32_t srslte_vec_max_fi(const float *x, const uint32_t len);
+SRSLTE_API uint32_t srslte_vec_max_abs_ci(const cf_t *x, const uint32_t len);
 
 /* quantify vector of floats or int16 and convert to uint8_t */
-SRSLTE_API void srslte_vec_quant_fuc(float *in, uint8_t *out, float gain, float offset, float clip, uint32_t len);
-SRSLTE_API void srslte_vec_quant_suc(int16_t *in, uint8_t *out, float gain, int16_t offset, int16_t clip, uint32_t len); 
+SRSLTE_API void srslte_vec_quant_fuc(const float *in, uint8_t *out, const float gain, const float offset, const float clip, const uint32_t len);
+SRSLTE_API void srslte_vec_quant_suc(const int16_t *in, uint8_t *out, const float gain, const int16_t offset, const int16_t clip, const uint32_t len); 
 
 /* magnitude of each vector element */
-SRSLTE_API void srslte_vec_abs_cf(cf_t *x, float *abs, uint32_t len);
-SRSLTE_API void srslte_vec_abs_square_cf(cf_t *x, float *abs_square, uint32_t len);
+SRSLTE_API void srslte_vec_abs_cf(const cf_t *x, float *abs, const uint32_t len);
+SRSLTE_API void srslte_vec_abs_square_cf(const cf_t *x, float *abs_square, const uint32_t len);
 
 /* Copy 256 bit aligned vector */
-SRSLTE_API void srs_vec_cf_cpy(cf_t *src, cf_t *dst, int len);
+SRSLTE_API void srs_vec_cf_cpy(const cf_t *src, cf_t *dst, const int len);
 
 #ifdef __cplusplus
 }
diff --git a/lib/include/srslte/phy/utils/vector_simd.h b/lib/include/srslte/phy/utils/vector_simd.h
index 294cff50f..54ac55f98 100644
--- a/lib/include/srslte/phy/utils/vector_simd.h
+++ b/lib/include/srslte/phy/utils/vector_simd.h
@@ -53,75 +53,80 @@ extern "C" {
 #endif /* LV_HAVE_AVX */
 #endif /* LV_HAVE_AVX512 */
 
+
+/*SIMD Logical operations*/
+SRSLTE_API void srslte_vec_xor_bbb_simd(const int8_t *x, const int8_t *y, int8_t *z, int len);
+
 /* SIMD Basic vector math */
-SRSLTE_API void srslte_vec_sum_sss_simd(int16_t *x, int16_t *y, int16_t *z, int len);
+SRSLTE_API void srslte_vec_sum_sss_simd(const int16_t *x, const int16_t *y, int16_t *z, int len);
 
-SRSLTE_API void srslte_vec_sub_sss_simd(int16_t *x, int16_t *y, int16_t *z, int len);
+SRSLTE_API void srslte_vec_sub_sss_simd(const int16_t *x, const int16_t *y, int16_t *z, int len);
 
-SRSLTE_API float srslte_vec_acc_ff_simd(float *x, int len);
+SRSLTE_API float srslte_vec_acc_ff_simd(const float *x, int len);
 
-SRSLTE_API cf_t srslte_vec_acc_cc_simd(cf_t *x, int len);
+SRSLTE_API cf_t srslte_vec_acc_cc_simd(const cf_t *x, int len);
 
-SRSLTE_API void srslte_vec_add_fff_simd(float *x, float *y, float *z, int len);
+SRSLTE_API void srslte_vec_add_fff_simd(const float *x, const float *y, float *z, int len);
 
-SRSLTE_API void srslte_vec_sub_fff_simd(float *x, float *y, float *z, int len);
+SRSLTE_API void srslte_vec_sub_fff_simd(const float *x, const float *y, float *z, int len);
 
 /* SIMD Vector Scalar Product */
-SRSLTE_API void srslte_vec_sc_prod_cfc_simd(const cf_t *x,const float h,cf_t *y,const int len);
+SRSLTE_API void srslte_vec_sc_prod_cfc_simd(const cf_t *x, const float h,cf_t *y, const int len);
 
-SRSLTE_API void srslte_vec_sc_prod_fff_simd(float *x, float h, float *z, int len);
+SRSLTE_API void srslte_vec_sc_prod_fff_simd(const float *x, const float h, float *z, const int len);
 
-SRSLTE_API void srslte_vec_sc_prod_ccc_simd(cf_t *x, cf_t h, cf_t *z, int len);
+SRSLTE_API void srslte_vec_sc_prod_ccc_simd(const cf_t *x, const cf_t h, cf_t *z, const int len);
 
 /* SIMD Vector Product */
-SRSLTE_API void srslte_vec_prod_ccc_split_simd(float *a_re, float *a_im, float *b_re, float *b_im, float *r_re, float *r_im, int len);
+SRSLTE_API void srslte_vec_prod_ccc_split_simd(const float *a_re, const float *a_im, const float *b_re, const float *b_im,
+                                               float *r_re, float *r_im, const int len);
 
-SRSLTE_API void srslte_vec_prod_ccc_c16_simd(int16_t *a_re, int16_t *a_im, int16_t *b_re, int16_t *b_im, int16_t *r_re,
-                                             int16_t *r_im, int len);
+SRSLTE_API void srslte_vec_prod_ccc_c16_simd(const int16_t *a_re, const int16_t *a_im, const int16_t *b_re, const int16_t *b_im,
+                                             int16_t *r_re, int16_t *r_im, const int len);
 
-SRSLTE_API void srslte_vec_prod_sss_simd(int16_t *x, int16_t *y, int16_t *z, int len);
+SRSLTE_API void srslte_vec_prod_sss_simd(const int16_t *x, const int16_t *y, int16_t *z, const int len);
 
-SRSLTE_API void srslte_vec_prod_cfc_simd(cf_t *x, float *y, cf_t *z, int len);
+SRSLTE_API void srslte_vec_prod_cfc_simd(const cf_t *x, const float *y, cf_t *z, const int len);
 
-SRSLTE_API void srslte_vec_prod_fff_simd(float *x, float *y, float *z, int len);
+SRSLTE_API void srslte_vec_prod_fff_simd(const float *x, const float *y, float *z, const int len);
 
-SRSLTE_API void srslte_vec_prod_ccc_simd(cf_t *x,cf_t *y, cf_t *z, int len);
+SRSLTE_API void srslte_vec_prod_ccc_simd(const cf_t *x, const cf_t *y, cf_t *z, const int len);
 
-SRSLTE_API void srslte_vec_prod_conj_ccc_simd(cf_t *x,cf_t *y, cf_t *z, int len);
+SRSLTE_API void srslte_vec_prod_conj_ccc_simd(const cf_t *x, const cf_t *y, cf_t *z, const int len);
 
 /* SIMD Division */
-SRSLTE_API void srslte_vec_div_ccc_simd(cf_t *x,cf_t *y, cf_t *z, int len);
+SRSLTE_API void srslte_vec_div_ccc_simd(const cf_t *x, const cf_t *y, cf_t *z, const int len);
 
-SRSLTE_API void srslte_vec_div_cfc_simd(cf_t *x, float *y, cf_t *z, int len);
+SRSLTE_API void srslte_vec_div_cfc_simd(const cf_t *x, const float *y, cf_t *z, const int len);
 
-SRSLTE_API void srslte_vec_div_fff_simd(float *x, float *y, float *z, int len);
+SRSLTE_API void srslte_vec_div_fff_simd(const float *x, const float *y, float *z, const int len);
 
 /* SIMD Dot product */
-SRSLTE_API cf_t srslte_vec_dot_prod_conj_ccc_simd(cf_t *x, cf_t *y, int len);
+SRSLTE_API cf_t srslte_vec_dot_prod_conj_ccc_simd(const cf_t *x, const cf_t *y, const int len);
 
-SRSLTE_API cf_t srslte_vec_dot_prod_ccc_simd(cf_t *x, cf_t *y, int len);
+SRSLTE_API cf_t srslte_vec_dot_prod_ccc_simd(const cf_t *x, const cf_t *y, const int len);
 
-SRSLTE_API c16_t srslte_vec_dot_prod_ccc_c16i_simd(c16_t *x, c16_t *y, int len);
+SRSLTE_API c16_t srslte_vec_dot_prod_ccc_c16i_simd(const c16_t *x, const c16_t *y, const int len);
 
-SRSLTE_API int srslte_vec_dot_prod_sss_simd(int16_t *x, int16_t *y, int len);
+SRSLTE_API int srslte_vec_dot_prod_sss_simd(const int16_t *x, const int16_t *y, const int len);
 
 /* SIMD Modulus functions */
-SRSLTE_API void srslte_vec_abs_cf_simd(cf_t *x, float *z, int len);
+SRSLTE_API void srslte_vec_abs_cf_simd(const cf_t *x, float *z, const int len);
 
-SRSLTE_API void srslte_vec_abs_square_cf_simd(cf_t *x, float *z, int len);
+SRSLTE_API void srslte_vec_abs_square_cf_simd(const cf_t *x, float *z, const int len);
 
 /* Other Functions */
-SRSLTE_API void srslte_vec_lut_sss_simd(short *x, unsigned short *lut, short *y, int len);
+SRSLTE_API void srslte_vec_lut_sss_simd(const short *x, const unsigned short *lut, short *y, const int len);
 
-SRSLTE_API void srslte_vec_convert_fi_simd(float *x, int16_t *z, float scale, int len);
+SRSLTE_API void srslte_vec_convert_fi_simd(const float *x, int16_t *z, const float scale, const int len);
 
-SRSLTE_API void srslte_vec_cp_simd(cf_t *src, cf_t *dst, int len);
+SRSLTE_API void srslte_vec_cp_simd(const cf_t *src, cf_t *dst, int len);
 
 
 /* SIMD Find Max functions */
-SRSLTE_API uint32_t srslte_vec_max_fi_simd(float *x, int len);
+SRSLTE_API uint32_t srslte_vec_max_fi_simd(const float *x, const int len);
 
-SRSLTE_API uint32_t srslte_vec_max_ci_simd(cf_t *x, int len);
+SRSLTE_API uint32_t srslte_vec_max_ci_simd(const cf_t *x, const int len);
 
 #ifdef __cplusplus
 }
diff --git a/lib/src/asn1/liblte_mme.cc b/lib/src/asn1/liblte_mme.cc
index 4447b001f..215ffd838 100644
--- a/lib/src/asn1/liblte_mme.cc
+++ b/lib/src/asn1/liblte_mme.cc
@@ -1411,9 +1411,9 @@ LIBLTE_ERROR_ENUM liblte_mme_pack_eps_mobile_id_ie(LIBLTE_MME_EPS_MOBILE_ID_STRU
                 **ie_ptr  = (((eps_mobile_id->guti.mnc/10) % 10) << 4) | ((eps_mobile_id->guti.mnc/100) % 10);
                 *ie_ptr  += 1;
             }
-            **ie_ptr  = (eps_mobile_id->guti.mme_group_id >> 8) & 0x0F;
+            **ie_ptr  = (eps_mobile_id->guti.mme_group_id >> 8) & 0xFF;
             *ie_ptr  += 1;
-            **ie_ptr  = eps_mobile_id->guti.mme_group_id & 0x0F;
+            **ie_ptr  = eps_mobile_id->guti.mme_group_id & 0xFF;
             *ie_ptr  += 1;
             **ie_ptr  = eps_mobile_id->guti.mme_code;
             *ie_ptr  += 1;
@@ -5518,6 +5518,14 @@ LIBLTE_ERROR_ENUM liblte_mme_unpack_attach_reject_msg(LIBLTE_BYTE_MSG_STRUCT
 *********************************************************************/
 LIBLTE_ERROR_ENUM liblte_mme_pack_attach_request_msg(LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT *attach_req,
                                                      LIBLTE_BYTE_MSG_STRUCT               *msg)
+{
+    return liblte_mme_pack_attach_request_msg(attach_req, LIBLTE_MME_SECURITY_HDR_TYPE_PLAIN_NAS, 0, msg);
+}
+
+LIBLTE_ERROR_ENUM liblte_mme_pack_attach_request_msg(LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT *attach_req,
+                                                     uint8                                 sec_hdr_type,
+                                                     uint32                                count,
+                                                     LIBLTE_BYTE_MSG_STRUCT               *msg)
 {
     LIBLTE_ERROR_ENUM  err     = LIBLTE_ERROR_INVALID_INPUTS;
     uint8             *msg_ptr = msg->msg;
@@ -5525,6 +5533,20 @@ LIBLTE_ERROR_ENUM liblte_mme_pack_attach_request_msg(LIBLTE_MME_ATTACH_REQUEST_M
     if(attach_req != NULL &&
        msg        != NULL)
     {
+        if(LIBLTE_MME_SECURITY_HDR_TYPE_PLAIN_NAS != sec_hdr_type)
+        {
+            // Protocol Discriminator and Security Header Type
+            *msg_ptr = (sec_hdr_type << 4) | (LIBLTE_MME_PD_EPS_MOBILITY_MANAGEMENT);
+            msg_ptr++;
+
+            // MAC will be filled in later
+            msg_ptr += 4;
+
+            // Sequence Number
+            *msg_ptr = count & 0xFF;
+            msg_ptr++;
+        }
+
         // Protocol Discriminator and Security Header Type
         *msg_ptr = (LIBLTE_MME_SECURITY_HDR_TYPE_PLAIN_NAS << 4) | (LIBLTE_MME_PD_EPS_MOBILITY_MANAGEMENT);
         msg_ptr++;
diff --git a/lib/src/common/CMakeLists.txt b/lib/src/common/CMakeLists.txt
index 723085e24..010f8f730 100644
--- a/lib/src/common/CMakeLists.txt
+++ b/lib/src/common/CMakeLists.txt
@@ -22,7 +22,9 @@ file(GLOB CXX_SOURCES "*.cc")
 file(GLOB C_SOURCES "*.c")
 
 add_library(srslte_common STATIC ${C_SOURCES} ${CXX_SOURCES})
+add_custom_target(gen_build_info COMMAND cmake -P ${CMAKE_BINARY_DIR}/SRSLTEbuildinfo.cmake)
+add_dependencies(srslte_common gen_build_info)
+
 target_include_directories(srslte_common PUBLIC ${SEC_INCLUDE_DIRS})
 target_link_libraries(srslte_common ${SEC_LIBRARIES})
 install(TARGETS srslte_common DESTINATION ${LIBRARY_DIR})
-
diff --git a/lib/src/phy/agc/agc.c b/lib/src/phy/agc/agc.c
index 3134de729..42a6ef09d 100644
--- a/lib/src/phy/agc/agc.c
+++ b/lib/src/phy/agc/agc.c
@@ -117,7 +117,7 @@ void srslte_agc_lock(srslte_agc_t *q, bool enable) {
 void srslte_agc_process(srslte_agc_t *q, cf_t *signal, uint32_t len) {
   if (!q->lock) {
     float gain_db = 10*log10(q->gain); 
-    float gain_uhd_db = 1.0;
+    float gain_uhd_db = 50.0;
     //float gain_uhd = 1.0;  
     float y = 0; 
     // Apply current gain to input signal 
@@ -125,12 +125,12 @@ void srslte_agc_process(srslte_agc_t *q, cf_t *signal, uint32_t len) {
       srslte_vec_sc_prod_cfc(signal, q->gain, signal, len);
     } else {
       if (gain_db < 0) {
-        gain_db = 0.0; 
+        gain_db = 5.0;
       }
       if (isinf(gain_db) || isnan(gain_db)) {
-        gain_db = 10.0; 
+        gain_db = 40.0;
       } else {
-        gain_uhd_db = q->set_gain_callback(q->uhd_handler, gain_db);        
+        gain_uhd_db = q->set_gain_callback(q->uhd_handler, gain_db);
         q->gain = pow(10, gain_uhd_db/10);
       }
     }
@@ -166,7 +166,7 @@ void srslte_agc_process(srslte_agc_t *q, cf_t *signal, uint32_t len) {
       }
     }
     
-    double gg = 1.0; 
+    double gg = 1.0;
     if (q->isfirst) {
       q->y_out = y; 
       q->isfirst = false; 
@@ -177,7 +177,7 @@ void srslte_agc_process(srslte_agc_t *q, cf_t *signal, uint32_t len) {
           gg = expf(-0.5*q->bandwidth*logf(q->y_out/q->target));
           q->gain *= gg; 
         }          
-        DEBUG("AGC gain: %.2f (%.2f) y_out=%.3f, y=%.3f target=%.1f gg=%.2f\n", gain_db, gain_uhd_db, q->y_out, y, q->target, gg);      
+        DEBUG("AGC gain: %.2f (%.2f) y_out=%.3f, y=%.3f target=%.1f gg=%.2f\n", gain_db, gain_uhd_db, q->y_out, y, q->target, gg);
       }
     }
   }
diff --git a/lib/src/phy/ch_estimation/chest_dl.c b/lib/src/phy/ch_estimation/chest_dl.c
index 0f8ae8074..e04d30e69 100644
--- a/lib/src/phy/ch_estimation/chest_dl.c
+++ b/lib/src/phy/ch_estimation/chest_dl.c
@@ -102,25 +102,30 @@ int srslte_chest_dl_init(srslte_chest_dl_t *q, uint32_t max_prb)
     }
 
     q->tmp_noise = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
-
-
     if (!q->tmp_noise) {
       perror("malloc");
       goto clean_exit;
     }
-    q->pilot_estimates = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
 
+    q->tmp_cfo_estimate = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
+    if (!q->tmp_cfo_estimate) {
+      perror("malloc");
+      goto clean_exit;
+    }
+
+    q->pilot_estimates = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
     if (!q->pilot_estimates) {
       perror("malloc");
       goto clean_exit;
     }      
+
     q->pilot_estimates_average = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
     if (!q->pilot_estimates_average) {
       perror("malloc");
       goto clean_exit;
-    }      
-    q->pilot_recv_signal = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
+    }
 
+    q->pilot_recv_signal = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
     if (!q->pilot_recv_signal) {
       perror("malloc");
       goto clean_exit;
@@ -175,6 +180,9 @@ void srslte_chest_dl_free(srslte_chest_dl_t *q)
   if (q->tmp_noise) {
     free(q->tmp_noise);
   }
+  if (q->tmp_cfo_estimate) {
+    free(q->tmp_cfo_estimate);
+  }
   srslte_interp_linear_vector_free(&q->srslte_interp_linvec);
   srslte_interp_linear_free(&q->srslte_interp_lin);
   srslte_interp_linear_free(&q->srslte_interp_lin_mbsfn);
@@ -241,6 +249,10 @@ static float estimate_noise_pilots(srslte_chest_dl_t *q, uint32_t port_id)
 {
   int nref=SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id);
 
+  if (q->average_subframe) {
+    nref /= 4; 
+  }
+
   /* Substract noisy pilot estimates */
   srslte_vec_sub_ccc(q->pilot_estimates_average, q->pilot_estimates, q->tmp_noise, nref);  
   
@@ -305,9 +317,13 @@ static void interpolate_pilots(srslte_chest_dl_t *q, cf_t *pilot_estimates, cf_t
   uint32_t nsymbols = (ch_mode == SRSLTE_SF_MBSFN ) ? srslte_refsignal_mbsfn_nof_symbols() + 1 : srslte_refsignal_cs_nof_symbols(port_id);
   uint32_t fidx_offset = 0;
   /* Interpolate in the frequency domain */
- 
+
+  if (q->average_subframe) {
+    nsymbols = 1;
+  }
+
    // we add one to nsymbols to allow for inclusion of the non-mbms references in the channel estimation       
-  for (l=0;l<(nsymbols);l++) {
+  for (l=0;l<nsymbols;l++) {
     if (ch_mode == SRSLTE_SF_MBSFN) {
       if (l == 0) {
         fidx_offset = srslte_refsignal_cs_fidx(q->cell, l, port_id, 0);
@@ -329,34 +345,41 @@ static void interpolate_pilots(srslte_chest_dl_t *q, cf_t *pilot_estimates, cf_t
   }
  
   /* Now interpolate in the time domain between symbols */
-  if (ch_mode == SRSLTE_SF_MBSFN) {
-    srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(2), &cesymb(1), 2, 1);
-    srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(2), &cesymb(6), &cesymb(3), 4, 3);
-    srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(6), &cesymb(10), &cesymb(7), 4, 3);
-    srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(6), &cesymb(10), &cesymb(10), &cesymb(11), 4, 1);
-   } else {
-    if (SRSLTE_CP_ISNORM(q->cell.cp)) {
-      if (nsymbols == 4) {
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(4),  &cesymb(1), 4, 3);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(4), &cesymb(7),  &cesymb(5), 3, 2);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(7), &cesymb(11), &cesymb(8), 4, 3);
-        srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(7), &cesymb(11), &cesymb(11), &cesymb(12), 4, 2);
-      } else {
-        srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(8), &cesymb(1), &cesymb(1), &cesymb(0), 7, 1);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(8), &cesymb(2), 7, 6);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(8), &cesymb(9), 7, 5);
-      }    
+  if (q->average_subframe) {
+    // If we average per subframe, just copy the estimates in the time domain
+    for (l=0;l<2*SRSLTE_CP_NSYMB(q->cell.cp);l++) {
+      memcpy(&ce[l*SRSLTE_NRE*q->cell.nof_prb], ce, sizeof(cf_t)*SRSLTE_NRE*q->cell.nof_prb);
+    }
+  } else {
+    if (ch_mode == SRSLTE_SF_MBSFN) {
+      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(2), &cesymb(1), 2, 1);
+      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(2), &cesymb(6), &cesymb(3), 4, 3);
+      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(6), &cesymb(10), &cesymb(7), 4, 3);
+      srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(6), &cesymb(10), &cesymb(10), &cesymb(11), 4, 1);
     } else {
-      if (nsymbols == 4) {
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(3), &cesymb(1), 3, 2);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(3), &cesymb(6), &cesymb(4), 3, 2);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(6), &cesymb(9), &cesymb(7), 3, 2);
-        srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(6), &cesymb(9), &cesymb(9), &cesymb(10), 3, 2);
+      if (SRSLTE_CP_ISNORM(q->cell.cp)) {
+        if (nsymbols == 4) {
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(4),  &cesymb(1), 4, 3);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(4), &cesymb(7),  &cesymb(5), 3, 2);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(7), &cesymb(11), &cesymb(8), 4, 3);
+          srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(7), &cesymb(11), &cesymb(11), &cesymb(12), 4, 2);
+        } else {
+          srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(8), &cesymb(1), &cesymb(1), &cesymb(0), 7, 1);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(8), &cesymb(2), 7, 6);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(8), &cesymb(9), 7, 5);
+        }
       } else {
-        srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(7), &cesymb(1), &cesymb(1), &cesymb(0), 6, 1);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(7), &cesymb(2), 6, 5);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(7), &cesymb(8), 6, 4);
-      }    
+        if (nsymbols == 4) {
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(3), &cesymb(1), 3, 2);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(3), &cesymb(6), &cesymb(4), 3, 2);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(6), &cesymb(9), &cesymb(7), 3, 2);
+          srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(6), &cesymb(9), &cesymb(9), &cesymb(10), 3, 2);
+        } else {
+          srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(7), &cesymb(1), &cesymb(1), &cesymb(0), 6, 1);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(7), &cesymb(2), 6, 5);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(7), &cesymb(8), 6, 4);
+        }
+      }
     }
   }
 }
@@ -392,6 +415,15 @@ static void average_pilots(srslte_chest_dl_t *q, cf_t *input, cf_t *output, uint
   uint32_t nsymbols = (ch_mode == SRSLTE_SF_MBSFN)?srslte_refsignal_mbsfn_nof_symbols(port_id):srslte_refsignal_cs_nof_symbols(port_id);
   uint32_t nref = (ch_mode == SRSLTE_SF_MBSFN)?6*q->cell.nof_prb:2*q->cell.nof_prb;
 
+  // Average in the time domain if enabled
+  if (q->average_subframe) {
+    for (int l=1;l<nsymbols;l++) {
+      srslte_vec_sum_ccc(&input[l*nref], input, input, nref);
+    }
+    srslte_vec_sc_prod_cfc(input, 1.0/((float) nsymbols), input, nref);
+    nsymbols = 1;
+  }
+
   // Average in the frequency domain
   for (int l=0;l<nsymbols;l++) {
     srslte_conv_same_cf(&input[l*nref], q->smooth_filter, &output[l*nref], nref, q->smooth_filter_len);    
@@ -410,6 +442,30 @@ float srslte_chest_dl_rssi(srslte_chest_dl_t *q, cf_t *input, uint32_t port_id)
   return rssi/nsymbols; 
 }
 
+// CFO estimation algorithm taken from "Carrier Frequency Synchronization in the
+// Downlink of 3GPP LTE", Qi Wang, C. Mehlfuhrer, M. Rupp
+float chest_estimate_cfo(srslte_chest_dl_t *q)
+{
+  float n  = (float) srslte_symbol_sz(q->cell.nof_prb);
+  float ns = (float) SRSLTE_CP_NSYMB(q->cell.cp);
+  float ng = (float) SRSLTE_CP_LEN_NORM(1, n);
+
+  uint32_t npilots = SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, 0);
+
+  // Compute angles between slots
+  for (int i=0;i<2;i++) {
+    srslte_vec_prod_conj_ccc(&q->pilot_estimates[i*npilots/4],
+                             &q->pilot_estimates[(i+2)*npilots/4],
+                             &q->tmp_cfo_estimate[i*npilots/4],
+                             npilots/4);
+  }
+  // Average all angles
+  cf_t sum = srslte_vec_acc_cc(q->tmp_cfo_estimate, npilots/2);
+
+  // Compute CFO
+  return -cargf(sum)*n/(ns*(n+ng))/2/M_PI;
+}
+
 void chest_interpolate_noise_est(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, uint32_t sf_idx, uint32_t port_id, uint32_t rxant_id, srslte_sf_t ch_mode){
  if (ce != NULL) {
     /* Smooth estimates (if applicable) and interpolate */
@@ -433,6 +489,10 @@ void chest_interpolate_noise_est(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, ui
       }
     } 
   }
+
+  if (q->cfo_estimate_enable && ((1<<sf_idx) & q->cfo_estimate_sf_mask)) {
+    q->cfo = SRSLTE_VEC_EMA(chest_estimate_cfo(q), q->cfo, q->cfo_ema);
+  }
     
   /* Compute RSRP for the channel estimates in this port */
   q->rsrp[rxant_id][port_id] = srslte_vec_avg_power_cf(q->pilot_recv_signal, SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id));     
@@ -451,8 +511,9 @@ int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, u
   srslte_vec_prod_conj_ccc(q->pilot_recv_signal, q->csr_refs.pilots[port_id/2][sf_idx], 
               q->pilot_estimates, SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id)); 
 
+
   chest_interpolate_noise_est(q, input, ce, sf_idx, port_id, rxant_id, SRSLTE_SF_NORM);
-  
+
   return 0;
 }
 int srslte_chest_dl_estimate_port_mbsfn(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, uint32_t sf_idx, uint32_t port_id, uint32_t rxant_id, uint16_t mbsfn_area_id)
@@ -474,10 +535,6 @@ int srslte_chest_dl_estimate_port_mbsfn(srslte_chest_dl_t *q, cf_t *input, cf_t
   return 0;
 }
 
-
-
-
-
 int srslte_chest_dl_estimate_multi(srslte_chest_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t sf_idx, uint32_t nof_rx_antennas) 
 {
   for (uint32_t rxant_id=0;rxant_id<nof_rx_antennas;rxant_id++) {
@@ -517,7 +574,21 @@ int srslte_chest_dl_estimate_multi_mbsfn(srslte_chest_dl_t *q, cf_t *input[SRSLT
   return SRSLTE_SUCCESS;
 }
 
+void srslte_chest_dl_average_subframe(srslte_chest_dl_t *q, bool enable)
+{
+  q->average_subframe = enable;
+}
+
+void srslte_chest_dl_cfo_estimate_enable(srslte_chest_dl_t *q, bool enable, uint32_t mask, float ema)
+{
+  q->cfo_ema              = ema;
+  q->cfo_estimate_enable  = enable;
+  q->cfo_estimate_sf_mask = mask;
+}
 
+float srslte_chest_dl_get_cfo(srslte_chest_dl_t *q) {
+  return q->cfo;
+}
 
 float srslte_chest_dl_get_noise_estimate(srslte_chest_dl_t *q) {
   float n = 0; 
diff --git a/lib/src/phy/dft/dft_fftw.c b/lib/src/phy/dft/dft_fftw.c
index 9d6898117..e6eb70ecf 100644
--- a/lib/src/phy/dft/dft_fftw.c
+++ b/lib/src/phy/dft/dft_fftw.c
@@ -245,7 +245,7 @@ static void copy_post(uint8_t *dst, uint8_t *src, int size_d, int len,
   }
 }
 
-void srslte_dft_run(srslte_dft_plan_t *plan, void *in, void *out) {
+void srslte_dft_run(srslte_dft_plan_t *plan, const void *in, void *out) {
   if(plan->mode == SRSLTE_DFT_COMPLEX) {
     srslte_dft_run_c(plan,in,out);
   } else {
@@ -253,11 +253,11 @@ void srslte_dft_run(srslte_dft_plan_t *plan, void *in, void *out) {
   }
 }
 
-void srslte_dft_run_c_zerocopy(srslte_dft_plan_t *plan, cf_t *in, cf_t *out) {
-  fftwf_execute_dft(plan->p, in, out);  
+void srslte_dft_run_c_zerocopy(srslte_dft_plan_t *plan, const cf_t *in, cf_t *out) {
+  fftwf_execute_dft(plan->p, (cf_t*) in, out);
 }
 
-void srslte_dft_run_c(srslte_dft_plan_t *plan, cf_t *in, cf_t *out) {
+void srslte_dft_run_c(srslte_dft_plan_t *plan, const cf_t *in, cf_t *out) {
   float norm;
   int i;
   fftwf_complex *f_out = plan->out;
@@ -286,7 +286,7 @@ void srslte_dft_run_guru_c(srslte_dft_plan_t *plan) {
   }
 }
 
-void srslte_dft_run_r(srslte_dft_plan_t *plan, float *in, float *out) {
+void srslte_dft_run_r(srslte_dft_plan_t *plan, const float *in, float *out) {
   float norm;
   int i;
   int len = plan->size;
diff --git a/lib/src/phy/modem/demod_soft.c b/lib/src/phy/modem/demod_soft.c
index 259a12271..0ea3ce938 100644
--- a/lib/src/phy/modem/demod_soft.c
+++ b/lib/src/phy/modem/demod_soft.c
@@ -57,11 +57,11 @@ void demod_bpsk_lte(const cf_t *symbols, float *llr, int nsymbols) {
 }
 
 void demod_qpsk_lte_s(const cf_t *symbols, short *llr, int nsymbols) {
-  srslte_vec_convert_fi((float*) symbols, llr, -SCALE_SHORT_CONV_QPSK*sqrt(2), nsymbols*2);
+  srslte_vec_convert_fi((const float*) symbols, -SCALE_SHORT_CONV_QPSK*sqrt(2), llr, nsymbols*2);
 }
 
 void demod_qpsk_lte(const cf_t *symbols, float *llr, int nsymbols) {
-  srslte_vec_sc_prod_fff((float*) symbols, -sqrt(2), llr, nsymbols*2);
+  srslte_vec_sc_prod_fff((const float*) symbols, -sqrt(2), llr, nsymbols*2);
 }
 
 void demod_16qam_lte(const cf_t *symbols, float *llr, int nsymbols) {
diff --git a/lib/src/phy/resampling/test/resample_arb_bench.c b/lib/src/phy/resampling/test/resample_arb_bench.c
index 773291913..e9ef3c442 100644
--- a/lib/src/phy/resampling/test/resample_arb_bench.c
+++ b/lib/src/phy/resampling/test/resample_arb_bench.c
@@ -50,7 +50,7 @@ int main(int argc, char **argv) {
 
   clock_t start = clock(), diff;
   for(int xx = 0; xx<ITERATIONS;xx++){
-     int n_out = srslte_resample_arb_compute(&r, in, out, N);
+     srslte_resample_arb_compute(&r, in, out, N);
   }
   diff = clock() - start;
 
diff --git a/lib/src/phy/rf/rf_blade_imp.c b/lib/src/phy/rf/rf_blade_imp.c
index d8996925f..8eb6a4f65 100644
--- a/lib/src/phy/rf/rf_blade_imp.c
+++ b/lib/src/phy/rf/rf_blade_imp.c
@@ -464,7 +464,7 @@ int rf_blade_recv_with_time(void *h,
   }
   
   timestamp_to_secs(handler->rx_rate, meta.timestamp, secs, frac_secs);
-  srslte_vec_convert_if(handler->rx_buffer, data, 2048, 2*nsamples);
+  srslte_vec_convert_if(handler->rx_buffer, 2048, data, 2*nsamples);
   
   return nsamples;
 }
@@ -506,7 +506,7 @@ int rf_blade_send_timed(void *h,
     return -1;
   }
 
-  srslte_vec_convert_fi(data, handler->tx_buffer, 2048, 2*nsamples);
+  srslte_vec_convert_fi(data, 2048, handler->tx_buffer, 2*nsamples);
   
   memset(&meta, 0, sizeof(meta));
   if (is_start_of_burst) {
diff --git a/lib/src/phy/rf/rf_imp.c b/lib/src/phy/rf/rf_imp.c
index 7149cec9d..eea2a24d1 100644
--- a/lib/src/phy/rf/rf_imp.c
+++ b/lib/src/phy/rf/rf_imp.c
@@ -41,7 +41,7 @@ int rf_get_available_devices(char **devnames, int max_strlen) {
 
 double srslte_rf_set_rx_gain_th(srslte_rf_t *rf, double gain)
 {
-  if (gain > rf->new_rx_gain + 0.5 || gain < rf->new_rx_gain - 0.5) {
+  if (gain > rf->new_rx_gain + 2 || gain < rf->new_rx_gain - 2) {
     pthread_mutex_lock(&rf->mutex);
     rf->new_rx_gain = gain; 
     pthread_cond_signal(&rf->cond);
@@ -69,6 +69,7 @@ static void* thread_gain_fcn(void *h) {
       srslte_rf_set_rx_gain(h, rf->cur_rx_gain);
     }
     if (rf->tx_gain_same_rx) {
+      printf("setting also tx\n");
       srslte_rf_set_tx_gain(h, rf->cur_rx_gain+rf->tx_rx_gain_offset);
     }
     pthread_mutex_unlock(&rf->mutex);
diff --git a/lib/src/phy/rf/rf_utils.c b/lib/src/phy/rf/rf_utils.c
index ce288b5c5..f97784305 100644
--- a/lib/src/phy/rf/rf_utils.c
+++ b/lib/src/phy/rf/rf_utils.c
@@ -123,12 +123,7 @@ int rf_mib_decoder(srslte_rf_t *rf, uint32_t nof_rx_antennas,cell_search_cfg_t *
   
   INFO("Starting receiver...\n", 0);
   srslte_rf_start_rx_stream(rf);
-    
-  // Set CFO if available
-  if (cfo) {
-    srslte_ue_sync_set_cfo(&ue_mib.ue_sync, *cfo);
-  }
-  
+
   /* Find and decody MIB */
   ret = srslte_ue_mib_sync_decode(&ue_mib, config->max_frames_pbch, bch_payload, &cell->nof_ports, NULL); 
   if (ret < 0) {
diff --git a/lib/src/phy/scrambling/scrambling.c b/lib/src/phy/scrambling/scrambling.c
index 42f16d1e8..ca0342905 100644
--- a/lib/src/phy/scrambling/scrambling.c
+++ b/lib/src/phy/scrambling/scrambling.c
@@ -60,10 +60,8 @@ void srslte_scrambling_c_offset(srslte_sequence_t *s, cf_t *data, int offset, in
 }
 
 void scrambling_b(uint8_t *c, uint8_t *data, int len) {
-  int i;  
-  for (i = 0; i < len; i++) {
-    data[i] = (data[i] ^ c[i]);
-  }  
+
+  srslte_vec_xor_bbb((int8_t*)c,(int8_t*)data,(int8_t*)data,len);  
 }
 
 void scrambling_b_word(uint8_t *c, uint8_t *data, int len) {
diff --git a/lib/src/phy/sync/cfo.c b/lib/src/phy/sync/cfo.c
index 34498ce58..44c795c19 100644
--- a/lib/src/phy/sync/cfo.c
+++ b/lib/src/phy/sync/cfo.c
@@ -83,7 +83,7 @@ int srslte_cfo_resize(srslte_cfo_t *h, uint32_t samples) {
   return SRSLTE_SUCCESS;
 }
 
-void srslte_cfo_correct(srslte_cfo_t *h, cf_t *input, cf_t *output, float freq) {
+void srslte_cfo_correct(srslte_cfo_t *h, const cf_t *input, cf_t *output, float freq) {
   if (fabs(h->last_freq - freq) > h->tol) {
     h->last_freq = freq;
     srslte_cexptab_gen(&h->tab, h->cur_cexp, h->last_freq, h->nsamples);
diff --git a/lib/src/phy/sync/cp.c b/lib/src/phy/sync/cp.c
index a282f6dfd..80d0a2179 100644
--- a/lib/src/phy/sync/cp.c
+++ b/lib/src/phy/sync/cp.c
@@ -63,14 +63,14 @@ int srslte_cp_synch_resize(srslte_cp_synch_t *q, uint32_t symbol_sz)
 }
 
 
-uint32_t srslte_cp_synch(srslte_cp_synch_t *q, cf_t *input, uint32_t max_offset, uint32_t nof_symbols, uint32_t cp_len)
+uint32_t srslte_cp_synch(srslte_cp_synch_t *q, const cf_t *input, uint32_t max_offset, uint32_t nof_symbols, uint32_t cp_len)
 {  
   if (max_offset > q->symbol_sz) {    
     max_offset = q->symbol_sz; 
   }
   for (int i=0;i<max_offset;i++) {
     q->corr[i] = 0;
-    cf_t *inputPtr = input; 
+    const cf_t *inputPtr = input;
     for (int n=0;n<nof_symbols;n++) {
       uint32_t cplen = (n%7)?cp_len:cp_len+1;
       q->corr[i] += srslte_vec_dot_prod_conj_ccc(&inputPtr[i], &inputPtr[i+q->symbol_sz], cplen)/nof_symbols;
diff --git a/lib/src/phy/sync/find_sss.c b/lib/src/phy/sync/find_sss.c
index 082aee52a..70b300bd3 100644
--- a/lib/src/phy/sync/find_sss.c
+++ b/lib/src/phy/sync/find_sss.c
@@ -68,7 +68,7 @@ static void corr_all_sz_partial(cf_t z[SRSLTE_SSS_N], float s[SRSLTE_SSS_N][SRSL
   }    
 }
 
-static void extract_pair_sss(srslte_sss_synch_t *q, cf_t *input, cf_t *ce, cf_t y[2][SRSLTE_SSS_N]) {
+static void extract_pair_sss(srslte_sss_synch_t *q, const cf_t *input, cf_t *ce, cf_t y[2][SRSLTE_SSS_N]) {
   cf_t input_fft[SRSLTE_SYMBOL_SZ_MAX];
 
   srslte_dft_run_c(&q->dftp_input, input, input_fft);
@@ -88,7 +88,7 @@ static void extract_pair_sss(srslte_sss_synch_t *q, cf_t *input, cf_t *ce, cf_t
 
 }    
 
-int srslte_sss_synch_m0m1_diff(srslte_sss_synch_t *q, cf_t *input, uint32_t *m0, float *m0_value,
+int srslte_sss_synch_m0m1_diff(srslte_sss_synch_t *q, const cf_t *input, uint32_t *m0, float *m0_value,
     uint32_t *m1, float *m1_value) 
 {
   return srslte_sss_synch_m0m1_diff_coh(q, input, NULL, m0, m0_value, m1, m1_value);
@@ -102,7 +102,7 @@ int srslte_sss_synch_m0m1_diff(srslte_sss_synch_t *q, cf_t *input, uint32_t *m0,
 
  *
  */
-int srslte_sss_synch_m0m1_diff_coh(srslte_sss_synch_t *q, cf_t *input, cf_t ce[2*SRSLTE_SSS_N], uint32_t *m0, float *m0_value,
+int srslte_sss_synch_m0m1_diff_coh(srslte_sss_synch_t *q, const cf_t *input, cf_t ce[2*SRSLTE_SSS_N], uint32_t *m0, float *m0_value,
     uint32_t *m1, float *m1_value) 
 {
 
@@ -145,7 +145,7 @@ int srslte_sss_synch_m0m1_diff_coh(srslte_sss_synch_t *q, cf_t *input, cf_t ce[2
  *       Jung-In Kim, Jung-Su Han, Hee-Jin Roh and Hyung-Jin Choi
 
  */
-int srslte_sss_synch_m0m1_partial(srslte_sss_synch_t *q, cf_t *input, uint32_t M, cf_t ce[2*SRSLTE_SSS_N], uint32_t *m0, float *m0_value,
+int srslte_sss_synch_m0m1_partial(srslte_sss_synch_t *q, const cf_t *input, uint32_t M, cf_t ce[2*SRSLTE_SSS_N], uint32_t *m0, float *m0_value,
     uint32_t *m1, float *m1_value) 
 {
 
diff --git a/lib/src/phy/sync/pss.c b/lib/src/phy/sync/pss.c
index f7b35071d..d93fe0896 100644
--- a/lib/src/phy/sync/pss.c
+++ b/lib/src/phy/sync/pss.c
@@ -30,12 +30,8 @@
 #include <stdlib.h>
 #include <complex.h>
 #include <math.h>
-#include <srslte/phy/sync/pss.h>
 
 #include "srslte/phy/sync/pss.h"
-#include "srslte/phy/dft/dft.h"
-#include "srslte/phy/utils/vector.h"
-#include "srslte/phy/utils/convolution.h"
 #include "srslte/phy/utils/debug.h"
 
 
@@ -69,7 +65,7 @@ int srslte_pss_synch_init_N_id_2(cf_t *pss_signal_freq, cf_t *pss_signal_time,
     srslte_vec_sc_prod_cfc(pss_signal_time, 1.0/SRSLTE_PSS_LEN, pss_signal_time, fft_size);
 
     srslte_dft_plan_free(&plan);
-        
+
     ret = SRSLTE_SUCCESS;
   }
   return ret;
@@ -123,6 +119,8 @@ int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q,
 
     buffer_size = fft_size + frame_size + 1;
 
+    q->filter_pss_enable = false;
+
     if(q->decimate > 1) {
         int filter_order = 3;
         srslte_filt_decim_cc_init(&q->filter,q->decimate,filter_order);
@@ -137,9 +135,19 @@ int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q,
     }
     srslte_dft_plan_set_mirror(&q->dftp_input, true);
     srslte_dft_plan_set_dc(&q->dftp_input, true);
-    srslte_dft_plan_set_norm(&q->dftp_input, true);
+    srslte_dft_plan_set_norm(&q->dftp_input, false);
+
+    if (srslte_dft_plan(&q->idftp_input, fft_size, SRSLTE_DFT_BACKWARD, SRSLTE_DFT_COMPLEX)) {
+      fprintf(stderr, "Error creating DFT plan \n");
+      goto clean_and_exit;
+    }
+    srslte_dft_plan_set_mirror(&q->idftp_input, true);
+    srslte_dft_plan_set_dc(&q->idftp_input, true);
+    srslte_dft_plan_set_norm(&q->idftp_input, true);
 
-    q->tmp_input = srslte_vec_malloc((buffer_size + frame_size*(q->decimate - 1)) * sizeof(cf_t)); 
+    bzero(q->tmp_fft2, sizeof(cf_t)*SRSLTE_SYMBOL_SZ_MAX);
+
+    q->tmp_input = srslte_vec_malloc((buffer_size + frame_size*(q->decimate - 1)) * sizeof(cf_t));
     if (!q->tmp_input) {
       fprintf(stderr, "Error allocating memory\n");
       goto clean_and_exit;
@@ -167,7 +175,7 @@ int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q,
     }
     bzero(q->conv_output_abs, sizeof(float) * buffer_size);
 #endif
-    
+
     for (N_id_2=0;N_id_2<3;N_id_2++) {
       q->pss_signal_time[N_id_2] = srslte_vec_malloc(buffer_size * sizeof(cf_t));
       if (!q->pss_signal_time[N_id_2]) {
@@ -178,14 +186,14 @@ int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q,
       if (srslte_pss_synch_init_N_id_2(q->pss_signal_freq[N_id_2], q->pss_signal_time[N_id_2], N_id_2, fft_size, offset)) {
         fprintf(stderr, "Error initiating PSS detector for N_id_2=%d fft_size=%d\n", N_id_2, fft_size);
         goto clean_and_exit;
-      }      
+      }
       bzero(&q->pss_signal_time[N_id_2][q->fft_size], q->frame_size * sizeof(cf_t));
-    }    
+    }
     #ifdef CONVOLUTION_FFT
 
 
     for(N_id_2=0; N_id_2<3; N_id_2++)
-    q->pss_signal_freq_full[N_id_2]   = srslte_vec_malloc(buffer_size * sizeof(cf_t));
+    q->pss_signal_freq_full[N_id_2] = srslte_vec_malloc(buffer_size * sizeof(cf_t));
 
     if (srslte_conv_fft_cc_init(&q->conv_fft, frame_size, fft_size)) {
       fprintf(stderr, "Error initiating convolution FFT\n");
@@ -194,15 +202,15 @@ int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q,
     for(int i=0; i<3; i++) {
       srslte_dft_run_c(&q->conv_fft.filter_plan, q->pss_signal_time[i], q->pss_signal_freq_full[i]);
     }
-    
+
     #endif
-    
+
     srslte_pss_synch_reset(q);
-    
+
     ret = SRSLTE_SUCCESS;
   }
 
-clean_and_exit: 
+clean_and_exit:
   if (ret == SRSLTE_ERROR) {
     srslte_pss_synch_free(q);
   }
@@ -248,6 +256,13 @@ int srslte_pss_synch_resize(srslte_pss_synch_t *q, uint32_t frame_size, uint32_t
       return SRSLTE_ERROR;
     }
 
+    if (srslte_dft_replan(&q->idftp_input, fft_size)) {
+      fprintf(stderr, "Error creating DFT plan \n");
+      return SRSLTE_ERROR;
+    }
+
+    bzero(q->tmp_fft2, sizeof(cf_t)*SRSLTE_SYMBOL_SZ_MAX);
+
     bzero(&q->tmp_input[q->frame_size], q->fft_size * sizeof(cf_t));
     bzero(q->conv_output, sizeof(cf_t) * buffer_size);
     bzero(q->conv_output_avg, sizeof(float) * buffer_size);
@@ -298,7 +313,7 @@ void srslte_pss_synch_free(srslte_pss_synch_t *q) {
     }
   #ifdef CONVOLUTION_FFT
     srslte_conv_fft_cc_free(&q->conv_fft);
-    
+
   #endif
     if (q->tmp_input) {
       free(q->tmp_input);
@@ -312,9 +327,10 @@ void srslte_pss_synch_free(srslte_pss_synch_t *q) {
     if (q->conv_output_avg) {
       free(q->conv_output_avg);
     }
-    
+
     srslte_dft_plan_free(&q->dftp_input);
-    
+    srslte_dft_plan_free(&q->idftp_input);
+
     if(q->decimate > 1)
     {
         srslte_filt_decim_cc_free(&q->filter);
@@ -323,7 +339,7 @@ void srslte_pss_synch_free(srslte_pss_synch_t *q) {
     }
 
 
-    bzero(q, sizeof(srslte_pss_synch_t));    
+    bzero(q, sizeof(srslte_pss_synch_t));
   }
 }
 
@@ -379,7 +395,7 @@ void srslte_pss_put_slot(cf_t *pss_signal, cf_t *slot, uint32_t nof_prb, srslte_
 void srslte_pss_get_slot(cf_t *slot, cf_t *pss_signal, uint32_t nof_prb, srslte_cp_t cp) {
   int k;
   k = (SRSLTE_CP_NSYMB(cp) - 1) * nof_prb * SRSLTE_NRE + nof_prb * SRSLTE_NRE / 2 - 31;
-  memcpy(pss_signal, &slot[k], SRSLTE_PSS_LEN * sizeof(cf_t));  
+  memcpy(pss_signal, &slot[k], SRSLTE_PSS_LEN * sizeof(cf_t));
 }
 
 
@@ -398,51 +414,79 @@ int srslte_pss_synch_set_N_id_2(srslte_pss_synch_t *q, uint32_t N_id_2) {
 /* Sets the weight factor alpha for the exponential moving average of the PSS correlation output
  */
 void srslte_pss_synch_set_ema_alpha(srslte_pss_synch_t *q, float alpha) {
-  q->ema_alpha = alpha; 
+  q->ema_alpha = alpha;
 }
 
-/** Performs time-domain PSS correlation. 
+float compute_peak_sidelobe(srslte_pss_synch_t *q, uint32_t corr_peak_pos, uint32_t conv_output_len)
+{
+  // Find end of peak lobe to the right
+  int pl_ub = corr_peak_pos+1;
+  while(q->conv_output_avg[pl_ub+1] <= q->conv_output_avg[pl_ub] && pl_ub < conv_output_len) {
+    pl_ub ++;
+  }
+  // Find end of peak lobe to the left
+  int pl_lb;
+  if (corr_peak_pos > 2) {
+    pl_lb = corr_peak_pos-1;
+    while(q->conv_output_avg[pl_lb-1] <= q->conv_output_avg[pl_lb] && pl_lb > 1) {
+      pl_lb --;
+    }
+  } else {
+    pl_lb = 0;
+  }
+
+  int sl_distance_right = conv_output_len-1-pl_ub;
+  if (sl_distance_right < 0) {
+    sl_distance_right = 0;
+  }
+  int sl_distance_left = pl_lb;
+
+  int sl_right = pl_ub+srslte_vec_max_fi(&q->conv_output_avg[pl_ub], sl_distance_right);
+  int sl_left = srslte_vec_max_fi(q->conv_output_avg, sl_distance_left);
+  float side_lobe_value = SRSLTE_MAX(q->conv_output_avg[sl_right], q->conv_output_avg[sl_left]);
+
+  return q->conv_output_avg[corr_peak_pos]/side_lobe_value;
+}
+
+/** Performs time-domain PSS correlation.
  * Returns the index of the PSS correlation peak in a subframe.
  * The frame starts at corr_peak_pos-subframe_size/2.
  * The value of the correlation is stored in corr_peak_value.
  *
  * Input buffer must be subframe_size long.
  */
-int srslte_pss_synch_find_pss(srslte_pss_synch_t *q, cf_t *input, float *corr_peak_value) 
+int srslte_pss_synch_find_pss(srslte_pss_synch_t *q, const cf_t *input, float *corr_peak_value)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
-  
-  if (q                 != NULL  && 
+
+  if (q                 != NULL  &&
       input             != NULL)
   {
 
     uint32_t corr_peak_pos;
     uint32_t conv_output_len;
-    
+
     if (!srslte_N_id_2_isvalid(q->N_id_2)) {
       fprintf(stderr, "Error finding PSS peak, Must set N_id_2 first\n");
       return SRSLTE_ERROR;
     }
 
     /* Correlate input with PSS sequence
-     * 
+     *
      * We do not reverse time-domain PSS signal because it's conjugate is symmetric.
      * The conjugate operation on pss_signal_time has been done in srslte_pss_synch_init_N_id_2
      * This is why we can use FFT-based convolution
      */
     if (q->frame_size >= q->fft_size) {
     #ifdef CONVOLUTION_FFT
-    memcpy(q->tmp_input, input, (q->frame_size * q->decimate) * sizeof(cf_t));
-    if(q->decimate > 1)
-    {
-      srslte_filt_decim_cc_execute(&(q->filter), q->tmp_input, q->filter.downsampled_input, q->filter.filter_output , (q->frame_size * q->decimate));
-      conv_output_len = srslte_conv_fft_cc_run_opt(&q->conv_fft, q->filter.filter_output,q->pss_signal_freq_full[q->N_id_2], q->conv_output);
-    }
-    else
-    {
-      conv_output_len = srslte_conv_fft_cc_run_opt(&q->conv_fft, q->tmp_input, q->pss_signal_freq_full[q->N_id_2], q->conv_output);
-    }
-      
+      memcpy(q->tmp_input, input, (q->frame_size * q->decimate) * sizeof(cf_t));
+      if(q->decimate > 1) {
+        srslte_filt_decim_cc_execute(&(q->filter), q->tmp_input, q->filter.downsampled_input, q->filter.filter_output , (q->frame_size * q->decimate));
+        conv_output_len = srslte_conv_fft_cc_run_opt(&q->conv_fft, q->filter.filter_output,q->pss_signal_freq_full[q->N_id_2], q->conv_output);
+      } else {
+        conv_output_len = srslte_conv_fft_cc_run_opt(&q->conv_fft, q->tmp_input, q->pss_signal_freq_full[q->N_id_2], q->conv_output);
+      }
+
     #else
       conv_output_len = srslte_conv_cc(input, q->pss_signal_time[q->N_id_2], q->conv_output, q->frame_size, q->fft_size);
     #endif
@@ -450,97 +494,62 @@ int srslte_pss_synch_find_pss(srslte_pss_synch_t *q, cf_t *input, float *corr_pe
       for (int i=0;i<q->frame_size;i++) {
         q->conv_output[i] = srslte_vec_dot_prod_ccc(q->pss_signal_time[q->N_id_2], &input[i], q->fft_size);
       }
-      conv_output_len = q->frame_size; 
+      conv_output_len = q->frame_size;
     }
 
-   
-  #ifdef SRSLTE_PSS_ABS_SQUARE
-      srslte_vec_abs_square_cf(q->conv_output, q->conv_output_abs, conv_output_len-1);
-  #else
-      srslte_vec_abs_cf(q->conv_output, q->conv_output_abs, conv_output_len-1);
-  #endif
-    
+    // Compute modulus square
+    srslte_vec_abs_square_cf(q->conv_output, q->conv_output_abs, conv_output_len-1);
+
+    // If enabled, average the absolute value from previous calls
     if (q->ema_alpha < 1.0 && q->ema_alpha > 0.0) {
-      srslte_vec_sc_prod_fff(q->conv_output_abs, q->ema_alpha, q->conv_output_abs, conv_output_len-1);    
-      srslte_vec_sc_prod_fff(q->conv_output_avg, 1-q->ema_alpha, q->conv_output_avg, conv_output_len-1);    
+      srslte_vec_sc_prod_fff(q->conv_output_abs, q->ema_alpha, q->conv_output_abs, conv_output_len-1);
+      srslte_vec_sc_prod_fff(q->conv_output_avg, 1-q->ema_alpha, q->conv_output_avg, conv_output_len-1);
 
       srslte_vec_sum_fff(q->conv_output_abs, q->conv_output_avg, q->conv_output_avg, conv_output_len-1);
     } else {
       memcpy(q->conv_output_avg, q->conv_output_abs, sizeof(float)*(conv_output_len-1));
     }
+
     /* Find maximum of the absolute value of the correlation */
     corr_peak_pos = srslte_vec_max_fi(q->conv_output_avg, conv_output_len-1);
-    
-    // save absolute value 
+
+    // save absolute value
     q->peak_value = q->conv_output_avg[corr_peak_pos];
-    
-#ifdef SRSLTE_PSS_RETURN_PSR    
-    // Find second side lobe
-    
-    // Find end of peak lobe to the right
-    int pl_ub = corr_peak_pos+1;
-    while(q->conv_output_avg[pl_ub+1] <= q->conv_output_avg[pl_ub] && pl_ub < conv_output_len) {
-      pl_ub ++; 
-    }
-    // Find end of peak lobe to the left
-    int pl_lb; 
-    if (corr_peak_pos > 2) {
-      pl_lb = corr_peak_pos-1;
-        while(q->conv_output_avg[pl_lb-1] <= q->conv_output_avg[pl_lb] && pl_lb > 1) {
-        pl_lb --; 
-      }      
-    } else {
-      pl_lb = 0; 
-    }
 
-    int sl_distance_right = conv_output_len-1-pl_ub; 
-    if (sl_distance_right < 0) {
-      sl_distance_right = 0; 
-    }
-    int sl_distance_left = pl_lb; 
-    
-    int sl_right = pl_ub+srslte_vec_max_fi(&q->conv_output_avg[pl_ub], sl_distance_right);
-    int sl_left = srslte_vec_max_fi(q->conv_output_avg, sl_distance_left);    
-    float side_lobe_value = SRSLTE_MAX(q->conv_output_avg[sl_right], q->conv_output_avg[sl_left]);    
+#ifdef SRSLTE_PSS_RETURN_PSR
     if (corr_peak_value) {
-      *corr_peak_value = q->conv_output_avg[corr_peak_pos]/side_lobe_value;
-      
-      if (*corr_peak_value < 10)
-        DEBUG("peak_pos=%2d, pl_ub=%2d, pl_lb=%2d, sl_right: %2d, sl_left: %2d, PSR: %.2f/%.2f=%.2f\n", corr_peak_pos, pl_ub, pl_lb, 
-             sl_right,sl_left, q->conv_output_avg[corr_peak_pos], side_lobe_value,*corr_peak_value);
+      *corr_peak_value = compute_peak_sidelobe(q, corr_peak_pos, conv_output_len);
     }
 #else
     if (corr_peak_value) {
       *corr_peak_value = q->conv_output_avg[corr_peak_pos];
     }
 #endif
-    
-    if(q->decimate >1)
-    {
-        int decimation_correction = (q->filter.num_taps - 2);
-        corr_peak_pos = corr_peak_pos - decimation_correction;
-        corr_peak_pos = corr_peak_pos*q->decimate;
-    }
 
+    if(q->decimate >1) {
+      int decimation_correction = (q->filter.num_taps - 2);
+      corr_peak_pos = corr_peak_pos - decimation_correction;
+      corr_peak_pos = corr_peak_pos*q->decimate;
+    }
 
     if (q->frame_size >= q->fft_size) {
-      ret = (int) corr_peak_pos;                
+      ret = (int) corr_peak_pos;
     } else {
       ret = (int) corr_peak_pos + q->fft_size;
     }
-  } 
+  }
   return ret;
 }
 
-/* Computes frequency-domain channel estimation of the PSS symbol 
- * input signal is in the time-domain. 
- * ce is the returned frequency-domain channel estimates. 
+/* Computes frequency-domain channel estimation of the PSS symbol
+ * input signal is in the time-domain.
+ * ce is the returned frequency-domain channel estimates.
  */
-int srslte_pss_synch_chest(srslte_pss_synch_t *q, cf_t *input, cf_t ce[SRSLTE_PSS_LEN]) {
+int srslte_pss_synch_chest(srslte_pss_synch_t *q, const cf_t *input, cf_t ce[SRSLTE_PSS_LEN]) {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   cf_t input_fft[SRSLTE_SYMBOL_SZ_MAX];
 
-  if (q                 != NULL  && 
+  if (q                 != NULL  &&
       input             != NULL)
   {
 
@@ -548,16 +557,28 @@ int srslte_pss_synch_chest(srslte_pss_synch_t *q, cf_t *input, cf_t ce[SRSLTE_PS
       fprintf(stderr, "Error finding PSS peak, Must set N_id_2 first\n");
       return SRSLTE_ERROR;
     }
-    
+
     /* Transform to frequency-domain */
     srslte_dft_run_c(&q->dftp_input, input, input_fft);
-    
+
     /* Compute channel estimate taking the PSS sequence as reference */
     srslte_vec_prod_conj_ccc(&input_fft[(q->fft_size-SRSLTE_PSS_LEN)/2], q->pss_signal_freq[q->N_id_2], ce, SRSLTE_PSS_LEN);
-      
+
     ret = SRSLTE_SUCCESS;
   }
-  return ret; 
+  return ret;
+}
+
+// Frequency-domain filtering of the central 64 sub-carriers
+void srslte_pss_synch_filter(srslte_pss_synch_t *q, const cf_t *input, cf_t *output)
+{
+  srslte_dft_run_c(&q->dftp_input, input, q->tmp_fft);
+
+  memcpy(&q->tmp_fft2[q->fft_size/2-SRSLTE_PSS_LEN/2],
+         &q->tmp_fft[q->fft_size/2-SRSLTE_PSS_LEN/2],
+         sizeof(cf_t)*SRSLTE_PSS_LEN);
+
+  srslte_dft_run_c(&q->idftp_input, q->tmp_fft2, output);
 }
 
 /* Returns the CFO estimation given a PSS received sequence
@@ -565,14 +586,18 @@ int srslte_pss_synch_chest(srslte_pss_synch_t *q, cf_t *input, cf_t ce[SRSLTE_PS
  * Source: An Efficient CFO Estimation Algorithm for the Downlink of 3GPP-LTE
  *       Feng Wang and Yu Zhu
  */
-float srslte_pss_synch_cfo_compute(srslte_pss_synch_t* q, cf_t *pss_recv) {
-  cf_t y0, y1, yr;
+float srslte_pss_synch_cfo_compute(srslte_pss_synch_t* q, const cf_t *pss_recv) {
+  cf_t y0, y1;
 
-  y0 = srslte_vec_dot_prod_ccc(q->pss_signal_time[q->N_id_2], pss_recv, q->fft_size/2);
-  y1 = srslte_vec_dot_prod_ccc(&q->pss_signal_time[q->N_id_2][q->fft_size/2], &pss_recv[q->fft_size/2], q->fft_size/2);
-  
-  yr = conjf(y0) * y1;
+  const cf_t *pss_ptr = pss_recv;
+
+  if (q->filter_pss_enable) {
+    srslte_pss_synch_filter(q, pss_recv, q->tmp_fft);
+    pss_ptr = (const cf_t*) q->tmp_fft;
+  }
 
-  return atan2f(__imag__ yr, __real__ yr) / M_PI;
+  y0 = srslte_vec_dot_prod_ccc(q->pss_signal_time[q->N_id_2], pss_ptr, q->fft_size/2);
+  y1 = srslte_vec_dot_prod_ccc(&q->pss_signal_time[q->N_id_2][q->fft_size/2], &pss_ptr[q->fft_size/2], q->fft_size/2);
+  return carg(conjf(y0) * y1)/M_PI;
 }
 
diff --git a/lib/src/phy/sync/sync.c b/lib/src/phy/sync/sync.c
index ee2ba76d1..bc9d27c19 100644
--- a/lib/src/phy/sync/sync.c
+++ b/lib/src/phy/sync/sync.c
@@ -37,11 +37,10 @@
 #include "srslte/phy/utils/vector.h"
 #include "srslte/phy/sync/cfo.h"
 
-#define MEANPEAK_EMA_ALPHA      0.1
-#define CFO_EMA_ALPHA           0.1
-#define CP_EMA_ALPHA            0.1
+#define CFO_EMA_ALPHA   0.1
+#define CP_EMA_ALPHA    0.1
 
-#define DEFAULT_CFO_TOL         50.0 // Hz
+#define DEFAULT_CFO_TOL 0.0 // Hz
 
 static bool fft_size_isvalid(uint32_t fft_size) {
   if (fft_size >= SRSLTE_SYNC_FFT_SZ_MIN && fft_size <= SRSLTE_SYNC_FFT_SZ_MAX && (fft_size%64) == 0) {
@@ -51,13 +50,12 @@ static bool fft_size_isvalid(uint32_t fft_size) {
   }
 }
 
-
-
 int srslte_sync_init(srslte_sync_t *q, uint32_t frame_size, uint32_t max_offset, uint32_t fft_size)
 {
     return srslte_sync_init_decim(q, frame_size, max_offset, fft_size, 1);
 }
-int srslte_sync_init_decim(srslte_sync_t *q, uint32_t frame_size, uint32_t max_offset, uint32_t fft_size, int decimate) {
+int srslte_sync_init_decim(srslte_sync_t *q, uint32_t frame_size, uint32_t max_offset, uint32_t fft_size, int decimate)
+{
 
   int ret = SRSLTE_ERROR_INVALID_INPUTS; 
   
@@ -67,31 +65,34 @@ int srslte_sync_init_decim(srslte_sync_t *q, uint32_t frame_size, uint32_t max_o
   {
     ret = SRSLTE_ERROR; 
     bzero(q, sizeof(srslte_sync_t));
-    q->detect_cp = true;
-    q->sss_en = true;
-    q->mean_cfo = 0; 
-    q->mean_cfo2 = 0; 
-    q->N_id_2 = 1000; 
+
+    q->N_id_2 = 1000;
     q->N_id_1 = 1000;
-    q->cfo_i = 0; 
-    q->find_cfo_i = false; 
-    q->find_cfo_i_initiated = false; 
+
     q->cfo_ema_alpha = CFO_EMA_ALPHA;
+    q->sss_alg = SSS_FULL;
+
+    q->detect_cp       = true;
+    q->sss_en          = true;
+    q->cfo_pss_enable  = false;
+    q->cfo_cp_enable   = false;
+    q->cfo_i_initiated = false;
+    q->pss_filtering_enabled = false;
+    q->sss_filtering_enabled = false;
+
     q->fft_size = fft_size;
     q->frame_size = frame_size;
     q->max_offset = max_offset;
-    q->sss_alg = SSS_FULL;
     q->max_frame_size = frame_size;
-    q->mean_cfo_isunset = true;
-    q->mean_cfo2_isunset = true;
 
-    q->enable_cfo_corr = true; 
-    if (srslte_cfo_init(&q->cfocorr, q->frame_size)) {
+    srslte_sync_cfo_reset(q);
+
+    if (srslte_cfo_init(&q->cfo_corr_frame, q->frame_size)) {
       fprintf(stderr, "Error initiating CFO\n");
       goto clean_exit;
     }
 
-    if (srslte_cfo_init(&q->cfocorr2, q->frame_size)) {
+    if (srslte_cfo_init(&q->cfo_corr_symbol, q->fft_size)) {
       fprintf(stderr, "Error initiating CFO\n");
       goto clean_exit;
     }
@@ -147,18 +148,23 @@ clean_exit:
   return ret;
 }
 
-void srslte_sync_free(srslte_sync_t *q) {
+void srslte_sync_free(srslte_sync_t *q)
+{
   if (q) {
+
     srslte_pss_synch_free(&q->pss);     
     srslte_sss_synch_free(&q->sss);  
-    srslte_cfo_free(&q->cfocorr);
-    srslte_cfo_free(&q->cfocorr2);
+    srslte_cfo_free(&q->cfo_corr_frame);
+    srslte_cfo_free(&q->cfo_corr_symbol);
     srslte_cp_synch_free(&q->cp_synch);
-    for (int i=0;i<2;i++) {
-      if (q->cfo_i_corr[i]) {
-        free(q->cfo_i_corr[i]);
+
+    if (q->cfo_i_initiated) {
+      for (int i=0;i<2;i++) {
+        if (q->cfo_i_corr[i]) {
+          free(q->cfo_i_corr[i]);
+        }
+        srslte_pss_synch_free(&q->pss_i[i]);
       }
-      srslte_pss_synch_free(&q->pss_i[i]);
     }
     if (q->temp) {
       free(q->temp);
@@ -174,53 +180,51 @@ int srslte_sync_resize(srslte_sync_t *q, uint32_t frame_size, uint32_t max_offse
       frame_size        <= 307200       &&
       fft_size_isvalid(fft_size))
   {
-    ret = SRSLTE_ERROR;
-
     if (frame_size > q->max_frame_size) {
       fprintf(stderr, "Error in sync_resize(): frame_size must be lower than initialized\n");
       return SRSLTE_ERROR;
     }
-    q->detect_cp = true;
-    q->sss_en = true;
-    q->mean_cfo = 0;
-    q->mean_cfo2 = 0;
-    q->N_id_2 = 1000;
-    q->N_id_1 = 1000;
-    q->cfo_i = 0;
-    q->find_cfo_i = false;
-    q->find_cfo_i_initiated = false;
-    q->cfo_ema_alpha = CFO_EMA_ALPHA;
-    q->fft_size = fft_size;
+
+    q->fft_size   = fft_size;
     q->frame_size = frame_size;
     q->max_offset = max_offset;
-    q->sss_alg = SSS_FULL;
-
-    q->enable_cfo_corr = true;
 
-    if (srslte_pss_synch_resize(&q->pss, max_offset, fft_size, 0)) {
+    if (srslte_pss_synch_resize(&q->pss, q->max_offset, q->fft_size, 0)) {
       fprintf(stderr, "Error resizing PSS object\n");
       return SRSLTE_ERROR;
     }
-    if (srslte_sss_synch_resize(&q->sss, fft_size)) {
+    if (srslte_sss_synch_resize(&q->sss, q->fft_size)) {
       fprintf(stderr, "Error resizing SSS object\n");
       return SRSLTE_ERROR;
     }
 
-    if (srslte_cp_synch_resize(&q->cp_synch, fft_size)) {
+    if (srslte_cp_synch_resize(&q->cp_synch, q->fft_size)) {
       fprintf(stderr, "Error resizing CFO\n");
       return SRSLTE_ERROR;
     }
 
-    if (srslte_cfo_resize(&q->cfocorr, q->frame_size)) {
+    if (srslte_cfo_resize(&q->cfo_corr_frame, q->frame_size)) {
       fprintf(stderr, "Error resizing CFO\n");
       return SRSLTE_ERROR;
     }
 
-    if (srslte_cfo_resize(&q->cfocorr2, q->frame_size)) {
+    if (srslte_cfo_resize(&q->cfo_corr_symbol, q->fft_size)) {
       fprintf(stderr, "Error resizing CFO\n");
       return SRSLTE_ERROR;
     }
 
+    if (q->cfo_i_initiated) {
+      for (int i=0;i<2;i++) {
+        int offset=(i==0)?-1:1;
+        if (srslte_pss_synch_resize(&q->pss_i[i], q->max_offset, q->fft_size, offset)) {
+          fprintf(stderr, "Error initializing PSS object\n");
+        }
+        for (int t=0;t<q->frame_size;t++) {
+          q->cfo_i_corr[i][t] = cexpf(-2*_Complex_I*M_PI*offset*(float) t/q->fft_size);
+        }
+      }
+    }
+
     // Update CFO tolerance
     srslte_sync_set_cfo_tol(q, q->current_cfo_tol);
 
@@ -236,30 +240,14 @@ int srslte_sync_resize(srslte_sync_t *q, uint32_t frame_size, uint32_t max_offse
 
 void srslte_sync_set_cfo_tol(srslte_sync_t *q, float tol) {
   q->current_cfo_tol = tol;
-  srslte_cfo_set_tol(&q->cfocorr, tol/(15000.0*q->fft_size));
-  srslte_cfo_set_tol(&q->cfocorr2, tol/(15000.0*q->fft_size));
+  srslte_cfo_set_tol(&q->cfo_corr_frame,  tol/(15000.0*q->fft_size));
+  srslte_cfo_set_tol(&q->cfo_corr_symbol, tol/(15000.0*q->fft_size));
 }
 
 void srslte_sync_set_threshold(srslte_sync_t *q, float threshold) {
   q->threshold = threshold;
 }
 
-void srslte_sync_cfo_i_detec_en(srslte_sync_t *q, bool enabled) {
-  q->find_cfo_i = enabled;
-  if (enabled && !q->find_cfo_i_initiated) {
-    for (int i=0;i<2;i++) {
-      int offset=(i==0)?-1:1;
-      if (srslte_pss_synch_init_fft_offset(&q->pss_i[i], q->max_offset, q->fft_size, offset)) {
-        fprintf(stderr, "Error initializing PSS object\n");      
-      }
-      for (int t=0;t<q->frame_size;t++) {
-        q->cfo_i_corr[i][t] = cexpf(-2*_Complex_I*M_PI*offset*(float) t/q->fft_size);
-      }
-    }
-    q->find_cfo_i_initiated = true; 
-  }
-}
-
 void srslte_sync_sss_en(srslte_sync_t *q, bool enabled) {
   q->sss_en = enabled;
 }
@@ -291,30 +279,59 @@ uint32_t srslte_sync_get_sf_idx(srslte_sync_t *q) {
 }
 
 float srslte_sync_get_cfo(srslte_sync_t *q) {
-  return q->mean_cfo2 + q->cfo_i;
+  return q->cfo_cp_mean + q->cfo_pss_mean + q->cfo_i_value;
 }
 
-void srslte_sync_set_cfo(srslte_sync_t *q, float cfo) {
-  q->mean_cfo  = cfo;
-  q->mean_cfo2 = cfo;
-  q->mean_cfo2_isunset = false;
-  q->mean_cfo_isunset  = false;
+void srslte_sync_cfo_reset(srslte_sync_t *q)
+{
+  q->cfo_cp_mean    = 0;
+  q->cfo_cp_is_set  = false;
+  q->cfo_pss_mean   = 0;
+  q->cfo_pss_is_set = false;
 }
 
-void srslte_sync_set_cfo_i(srslte_sync_t *q, int cfo_i) {
-  q->cfo_i = cfo_i;
+void srslte_sync_copy_cfo(srslte_sync_t *q, srslte_sync_t *src_obj) {
+  q->cfo_cp_mean    = src_obj->cfo_cp_mean;
+  q->cfo_pss_mean   = src_obj->cfo_pss_mean;
+  q->cfo_i_value    = src_obj->cfo_i_value;
+  q->cfo_cp_is_set  = false;
+  q->cfo_pss_is_set = false;
 }
 
-void srslte_sync_set_cfo_enable(srslte_sync_t *q, bool enable) {
-  q->enable_cfo_corr = enable; 
+void srslte_sync_set_cfo_i_enable(srslte_sync_t *q, bool enable) {
+  q->cfo_i_enable = enable;
+  if (q->cfo_i_enable  && !q->cfo_i_initiated) {
+    for (int i=0;i<2;i++) {
+      int offset=(i==0)?-1:1;
+      if (srslte_pss_synch_init_fft_offset(&q->pss_i[i], q->max_offset, q->fft_size, offset)) {
+        fprintf(stderr, "Error initializing PSS object\n");
+      }
+      for (int t=0;t<q->frame_size;t++) {
+        q->cfo_i_corr[i][t] = cexpf(-2*_Complex_I*M_PI*offset*(float) t/q->fft_size);
+      }
+    }
+    q->cfo_i_initiated = true;
+  }
 }
 
-void srslte_sync_set_cfo_ema_alpha(srslte_sync_t *q, float alpha) {
-  q->cfo_ema_alpha = alpha;
+void srslte_sync_set_sss_filt_enable(srslte_sync_t *q, bool enable) {
+  q->sss_filtering_enabled = enable;
 }
 
-float srslte_sync_get_last_peak_value(srslte_sync_t *q) {
-  return q->peak_value;
+void srslte_sync_set_pss_filt_enable(srslte_sync_t *q, bool enable) {
+  q->pss_filtering_enabled = enable;
+}
+
+void srslte_sync_set_cfo_cp_enable(srslte_sync_t *q, bool enable) {
+  q->cfo_cp_enable = enable;
+}
+
+void srslte_sync_set_cfo_pss_enable(srslte_sync_t *q, bool enable) {
+  q->cfo_pss_enable = enable;
+}
+
+void srslte_sync_set_cfo_ema_alpha(srslte_sync_t *q, float alpha) {
+  q->cfo_ema_alpha = alpha;
 }
 
 float srslte_sync_get_peak_value(srslte_sync_t *q) {
@@ -325,18 +342,17 @@ void srslte_sync_cp_en(srslte_sync_t *q, bool enabled) {
   q->detect_cp = enabled;
 }
 
-bool srslte_sync_sss_is_en(srslte_sync_t *q) {
-  return q->sss_en;
-}
-
-void srslte_sync_set_em_alpha(srslte_sync_t *q, float alpha) {
+void srslte_sync_set_em_alpha(srslte_sync_t *q, float alpha)
+{
   srslte_pss_synch_set_ema_alpha(&q->pss, alpha);
 }
 
-srslte_cp_t srslte_sync_get_cp(srslte_sync_t *q) {
+srslte_cp_t srslte_sync_get_cp(srslte_sync_t *q)
+{
   return q->cp;
 }
-void srslte_sync_set_cp(srslte_sync_t *q, srslte_cp_t cp) {
+void srslte_sync_set_cp(srslte_sync_t *q, srslte_cp_t cp)
+{
   q->cp = cp;
   q->cp_len = SRSLTE_CP_ISNORM(q->cp)?SRSLTE_CP_LEN_NORM(1,q->fft_size):SRSLTE_CP_LEN_EXT(q->fft_size);
   if (q->frame_size < q->fft_size) {
@@ -346,7 +362,8 @@ void srslte_sync_set_cp(srslte_sync_t *q, srslte_cp_t cp) {
   }
 }
 
-void srslte_sync_set_sss_algorithm(srslte_sync_t *q, sss_alg_t alg) {
+void srslte_sync_set_sss_algorithm(srslte_sync_t *q, sss_alg_t alg)
+{
   q->sss_alg = alg; 
 }
 
@@ -354,7 +371,7 @@ void srslte_sync_set_sss_algorithm(srslte_sync_t *q, sss_alg_t alg) {
  * "SSS Detection Method for Initial Cell Search in 3GPP LTE FDD/TDD Dual Mode Receiver"
  * by Jung-In Kim et al. 
  */
-srslte_cp_t srslte_sync_detect_cp(srslte_sync_t *q, cf_t *input, uint32_t peak_pos) 
+srslte_cp_t srslte_sync_detect_cp(srslte_sync_t *q, const cf_t *input, uint32_t peak_pos)
 {
   float R_norm=0, R_ext=0, C_norm=0, C_ext=0; 
   float M_norm=0, M_ext=0; 
@@ -370,8 +387,8 @@ srslte_cp_t srslte_sync_detect_cp(srslte_sync_t *q, cf_t *input, uint32_t peak_p
   
   if (nof_symbols > 0) {
  
-    cf_t *input_cp_norm = &input[peak_pos-nof_symbols*(q->fft_size+cp_norm_len)]; 
-    cf_t *input_cp_ext = &input[peak_pos-nof_symbols*(q->fft_size+cp_ext_len)]; 
+    const cf_t *input_cp_norm = &input[peak_pos-nof_symbols*(q->fft_size+cp_norm_len)];
+    const cf_t *input_cp_ext = &input[peak_pos-nof_symbols*(q->fft_size+cp_ext_len)];
 
     for (int i=0;i<nof_symbols;i++) {
       R_norm  += crealf(srslte_vec_dot_prod_conj_ccc(&input_cp_norm[q->fft_size], input_cp_norm, cp_norm_len));    
@@ -414,28 +431,21 @@ srslte_cp_t srslte_sync_detect_cp(srslte_sync_t *q, cf_t *input, uint32_t peak_p
 /* Returns 1 if the SSS is found, 0 if not and -1 if there is not enough space 
  * to correlate
  */
-int sync_sss(srslte_sync_t *q, cf_t *input, uint32_t peak_pos, srslte_cp_t cp) {
-  int sss_idx, ret;
+int sync_sss_symbol(srslte_sync_t *q, const cf_t *input)
+{
+  int ret;
 
   srslte_sss_synch_set_N_id_2(&q->sss, q->N_id_2);
 
-  /* Make sure we have enough room to find SSS sequence */
-  sss_idx = (int) peak_pos-2*q->fft_size-SRSLTE_CP_LEN(q->fft_size, (SRSLTE_CP_ISNORM(q->cp)?SRSLTE_CP_NORM_LEN:SRSLTE_CP_EXT_LEN));
-  if (sss_idx < 0) {
-    DEBUG("Not enough room to decode SSS (sss_idx=%d, peak_pos=%d)\n", sss_idx, peak_pos);
-    return SRSLTE_ERROR;
-  }
-  DEBUG("Searching SSS around sss_idx: %d, peak_pos: %d\n", sss_idx, peak_pos);
-      
   switch(q->sss_alg) {
     case SSS_DIFF:
-      srslte_sss_synch_m0m1_diff(&q->sss, &input[sss_idx], &q->m0, &q->m0_value, &q->m1, &q->m1_value);
+      srslte_sss_synch_m0m1_diff(&q->sss, input, &q->m0, &q->m0_value, &q->m1, &q->m1_value);
       break;
     case SSS_PARTIAL_3:
-      srslte_sss_synch_m0m1_partial(&q->sss, &input[sss_idx], 3, NULL, &q->m0, &q->m0_value, &q->m1, &q->m1_value);
+      srslte_sss_synch_m0m1_partial(&q->sss, input, 3, NULL, &q->m0, &q->m0_value, &q->m1, &q->m1_value);
       break;
     case SSS_FULL:
-      srslte_sss_synch_m0m1_partial(&q->sss, &input[sss_idx], 1, NULL, &q->m0, &q->m0_value, &q->m1, &q->m1_value);
+      srslte_sss_synch_m0m1_partial(&q->sss, input, 1, NULL, &q->m0, &q->m0_value, &q->m1, &q->m1_value);
       break;
   }
 
@@ -452,20 +462,23 @@ int sync_sss(srslte_sync_t *q, cf_t *input, uint32_t peak_pos, srslte_cp_t cp) {
   }
 }
 
-srslte_pss_synch_t* srslte_sync_get_cur_pss_obj(srslte_sync_t *q) {
+srslte_pss_synch_t* srslte_sync_get_cur_pss_obj(srslte_sync_t *q)
+{
  srslte_pss_synch_t *pss_obj[3] = {&q->pss_i[0], &q->pss, &q->pss_i[1]};
- return pss_obj[q->cfo_i+1];
+ return pss_obj[q->cfo_i_value+1];
 }
 
-static float cfo_estimate(srslte_sync_t *q, cf_t *input) {
+static float cfo_cp_estimate(srslte_sync_t *q, const cf_t *input)
+{
   uint32_t cp_offset = 0; 
-  cp_offset = srslte_cp_synch(&q->cp_synch, input, q->max_offset, 2, SRSLTE_CP_LEN_NORM(1,q->fft_size));
+  cp_offset = srslte_cp_synch(&q->cp_synch, input, q->max_offset, 7, SRSLTE_CP_LEN_NORM(1,q->fft_size));
   cf_t cp_corr_max = srslte_cp_synch_corr_output(&q->cp_synch, cp_offset);
   float cfo = -carg(cp_corr_max) / M_PI / 2; 
   return cfo; 
 }
 
-static int cfo_i_estimate(srslte_sync_t *q, cf_t *input, int find_offset, int *peak_pos) {
+static int cfo_i_estimate(srslte_sync_t *q, const cf_t *input, int find_offset, int *peak_pos, int *cfo_i)
+{
   float peak_value; 
   float max_peak_value = -99;  
   int max_cfo_i = 0; 
@@ -473,6 +486,9 @@ static int cfo_i_estimate(srslte_sync_t *q, cf_t *input, int find_offset, int *p
   for (int cfo_i=0;cfo_i<3;cfo_i++) {
     srslte_pss_synch_set_N_id_2(pss_obj[cfo_i], q->N_id_2);
     int p = srslte_pss_synch_find_pss(pss_obj[cfo_i], &input[find_offset], &peak_value);
+    if (p < 0) {
+      return -1;
+    }
     if (peak_value > max_peak_value) {
       max_peak_value = peak_value;
       if (peak_pos) {
@@ -481,125 +497,172 @@ static int cfo_i_estimate(srslte_sync_t *q, cf_t *input, int find_offset, int *p
       q->peak_value = peak_value;
       max_cfo_i = cfo_i-1;
     }
-  }      
-  return max_cfo_i; 
-}
-
-float srslte_sync_cfo_estimate(srslte_sync_t *q, cf_t *input, int find_offset) {
-  float cfo_f = cfo_estimate(q, input); 
-    
-  int cfo_i = 0; 
-  if (q->find_cfo_i) {
-    cfo_i = cfo_i_estimate(q, input, find_offset, NULL);
   }
-  return (float) cfo_i + cfo_f; 
+  if (cfo_i) {
+    *cfo_i = max_cfo_i;
+  }
+  return 0;
 }
 
 /** Finds the PSS sequence previously defined by a call to srslte_sync_set_N_id_2()
- * around the position find_offset in the buffer input. 
+ * around the position find_offset in the buffer input.
+ *
  * Returns 1 if the correlation peak exceeds the threshold set by srslte_sync_set_threshold() 
  * or 0 otherwise. Returns a negative number on error (if N_id_2 has not been set) 
- * 
+ *
+ * The input signal is not modified. Any CFO correction is done in internal buffers
+ *
  * The maximum of the correlation peak is always stored in *peak_position
  */
-srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, cf_t *input, uint32_t find_offset, uint32_t *peak_position) 
+srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, const cf_t *input, uint32_t find_offset, uint32_t *peak_position)
 {
-  
-  srslte_sync_find_ret_t ret = SRSLTE_SYNC_ERROR; 
-  
+  srslte_sync_find_ret_t ret = SRSLTE_SYNC_ERROR;
+  int peak_pos = 0;
+
   if (!q) {
     return SRSLTE_ERROR_INVALID_INPUTS;
   }
   
-  if (input             != NULL     &&
+  if (input  != NULL                   &&
       srslte_N_id_2_isvalid(q->N_id_2) && 
       fft_size_isvalid(q->fft_size))
   {
-    int peak_pos = 0;
-    
-    ret = SRSLTE_SUCCESS; 
-    
+
     if (peak_position) {
       *peak_position = 0; 
     }
     
-    cf_t *input_cfo = input; 
+    const cf_t *input_ptr = input;
+
+    /* First CFO estimation stage is integer.
+     * Finds max PSS correlation for shifted +1/0/-1 integer versions.
+     * This should only used once N_id_2 is set
+     */
+    if (q->cfo_i_enable) {
+      if (cfo_i_estimate(q, input_ptr, find_offset, &peak_pos, &q->cfo_i_value) < 0) {
+        fprintf(stderr, "Error calling finding PSS sequence at : %d  \n", peak_pos);
+        return SRSLTE_ERROR;
+      }
+      // Correct it using precomputed signal and store in buffer (don't modify input signal)
+      if (q->cfo_i_value != 0) {
+        srslte_vec_prod_ccc((cf_t*) input_ptr, q->cfo_i_corr[q->cfo_i_value<0?0:1], q->temp, q->frame_size);
+        INFO("Compensating cfo_i=%d\n", q->cfo_i_value);
+        input_ptr = q->temp;
+      }
+    }
 
-    if (q->enable_cfo_corr) {
-      float cfo = cfo_estimate(q, input);
+    /* Second stage is coarse fractional CFO estimation using CP.
+     * In case of multi-cell, this can lead to incorrect estimations if CFO from different cells is different
+     */
+    if (q->cfo_cp_enable) {
+      float cfo_cp = cfo_cp_estimate(q, input_ptr);
 
-      if (q->mean_cfo_isunset) {
-        q->mean_cfo = cfo;
-        q->mean_cfo_isunset = false;
+      if (!q->cfo_cp_is_set) {
+        q->cfo_cp_mean   = cfo_cp;
+        q->cfo_cp_is_set = true;
       } else {
         /* compute exponential moving average CFO */
-        q->mean_cfo = SRSLTE_VEC_EMA(cfo, q->mean_cfo, q->cfo_ema_alpha);
+        q->cfo_cp_mean = SRSLTE_VEC_EMA(cfo_cp, q->cfo_cp_mean, q->cfo_ema_alpha);
       }
 
+      INFO("CP-CFO: estimated=%f, mean=%f\n", cfo_cp, q->cfo_cp_mean);
+
       /* Correct CFO with the averaged CFO estimation */
-      srslte_cfo_correct(&q->cfocorr2, input, q->temp, -q->mean_cfo / q->fft_size);
-      
-      input_cfo = q->temp; 
+      srslte_cfo_correct(&q->cfo_corr_frame, input_ptr, q->temp, -q->cfo_cp_mean / q->fft_size);
+      input_ptr = q->temp;
     }
-    
-    /* If integer CFO is enabled, find max PSS correlation for shifted +1/0/-1 integer versions */
-    if (q->find_cfo_i && q->enable_cfo_corr) {
-      q->cfo_i = cfo_i_estimate(q, input_cfo, find_offset, &peak_pos);   
-      if (q->cfo_i != 0) {
-        srslte_vec_prod_ccc(input_cfo, q->cfo_i_corr[q->cfo_i<0?0:1], input_cfo, q->frame_size);
-        INFO("Compensating cfo_i=%d\n", q->cfo_i);
-      }
-    } else {      
+
+    /* Find maximum of PSS correlation. If Integer CFO is enabled, correlation is already done
+     */
+    if (!q->cfo_i_enable) {
       srslte_pss_synch_set_N_id_2(&q->pss, q->N_id_2);
-      peak_pos = srslte_pss_synch_find_pss(&q->pss, &input_cfo[find_offset], &q->peak_value);
-      // this compensates for the constant time shift caused by the low pass filter
-      if(q->decimate && peak_pos < 0)
-      {
-              peak_pos  =  0 ;//peak_pos + q->decimate*(2);// replace 2 with q->filter_size -2;
-      }
+      peak_pos = srslte_pss_synch_find_pss(&q->pss, &input_ptr[find_offset], q->threshold>0?&q->peak_value:NULL);
       if (peak_pos < 0) {
         fprintf(stderr, "Error calling finding PSS sequence at : %d  \n", peak_pos);
-        return SRSLTE_ERROR; 
-      }      
+        return SRSLTE_ERROR;
+      }
     }
-    
+
+    INFO("PSS: id=%d, peak_pos=%d, peak_value=%f\n", q->N_id_2, peak_pos, q->peak_value);
+
+    // Save peak position
     if (peak_position) {
       *peak_position = (uint32_t) peak_pos;
     }
-    
-    // Try to detect SSS 
-    if (q->sss_en) {
-      // Set an invalid N_id_1 indicating SSS is yet to be detected
-      q->N_id_1 = 1000; 
-      
-      if (sync_sss(q, input_cfo, find_offset + peak_pos, q->cp) < 0) {
-        DEBUG("No space for SSS processing. Frame starts at %d\n", peak_pos);
-      }
+
+    // In case of decimation, this compensates for the constant time shift caused by the low pass filter
+    if(q->decimate && peak_pos < 0)  {
+      peak_pos = 0 ;//peak_pos + q->decimate*(2);// replace 2 with q->filter_size -2;
     }
-    
+
     /* If peak is over threshold, compute CFO and SSS */
-    if (q->peak_value >= q->threshold) {
-      
-      if (q->detect_cp) {
-        if (peak_pos + find_offset >= 2*(q->fft_size + SRSLTE_CP_LEN_EXT(q->fft_size))) {
-          srslte_sync_set_cp(q, srslte_sync_detect_cp(q, input_cfo, peak_pos + find_offset));
+    if (q->peak_value >= q->threshold || q->threshold == 0) {
+
+      if (q->cfo_pss_enable && peak_pos >= q->fft_size) {
+
+        // Filter central bands before PSS-based CFO estimation
+        const cf_t *pss_ptr = &input_ptr[find_offset + peak_pos - q->fft_size];
+        if (q->pss_filtering_enabled) {
+          srslte_pss_synch_filter(&q->pss, pss_ptr, q->pss_filt);
+          pss_ptr = q->pss_filt;
+        }
+
+        // PSS-based CFO estimation
+        float cfo_pss = srslte_pss_synch_cfo_compute(&q->pss, pss_ptr);
+        if (!q->cfo_pss_is_set) {
+          q->cfo_pss_mean   = cfo_pss;
+          q->cfo_pss_is_set = true;
         } else {
-          DEBUG("Not enough room to detect CP length. Peak position: %d\n", peak_pos);
+          q->cfo_pss_mean = SRSLTE_VEC_EMA(cfo_pss, q->cfo_pss_mean, q->cfo_ema_alpha);
         }
+
+        INFO("PSS-CFO: filter=%s, estimated=%f, mean=%f\n",
+             q->pss_filtering_enabled?"yes":"no", cfo_pss, q->cfo_pss_mean);
+
       }
-  
-      if (peak_pos + find_offset >= 2*(q->fft_size + SRSLTE_CP_LEN_EXT(q->fft_size))) {
-        float cfo2   = srslte_pss_synch_cfo_compute(&q->pss, &input[find_offset + peak_pos - q->fft_size]);
-        if (q->mean_cfo2_isunset) {
-          q->mean_cfo2 = cfo2;
-          q->mean_cfo2_isunset = true;
+
+      // If there is enough space for CP and PSS-based CFO estimation
+      if (peak_pos + find_offset >= 2 * (q->fft_size + SRSLTE_CP_LEN_EXT(q->fft_size))) {
+
+        // If SSS search is enabled, correlate SSS sequence
+        if (q->sss_en) {
+
+          // Set an invalid N_id_1 indicating SSS is yet to be detected
+          q->N_id_1 = 1000;
+
+          int sss_idx = find_offset + peak_pos - 2 * q->fft_size -
+                        SRSLTE_CP_LEN(q->fft_size, (SRSLTE_CP_ISNORM(q->cp) ? SRSLTE_CP_NORM_LEN : SRSLTE_CP_EXT_LEN));
+
+          const cf_t *sss_ptr = &input_ptr[sss_idx];
+
+          // Correct CFO if detected in PSS
+          if (q->cfo_pss_enable) {
+            srslte_cfo_correct(&q->cfo_corr_symbol, sss_ptr, q->sss_filt, -q->cfo_pss_mean / q->fft_size);
+            sss_ptr = q->sss_filt;
+          }
+
+          // Filter central bands before SSS estimation
+          if (q->sss_filtering_enabled) {
+            srslte_pss_synch_filter(&q->pss, sss_ptr, q->sss_filt);
+            sss_ptr = q->sss_filt;
+          }
+
+          if (sync_sss_symbol(q, sss_ptr) < 0) {
+            fprintf(stderr, "Error correlating SSS\n");
+            return -1;
+          }
+        }
+
+        // Detect CP length
+        if (q->detect_cp) {
+          srslte_sync_set_cp(q, srslte_sync_detect_cp(q, input_ptr, peak_pos + find_offset));
         } else {
-          q->mean_cfo2 = SRSLTE_VEC_EMA(cfo2, q->mean_cfo2, q->cfo_ema_alpha);
+          DEBUG("Not enough room to detect CP length. Peak position: %d\n", peak_pos);
         }
 
         ret = SRSLTE_SYNC_FOUND;
       } else {
-        ret = SRSLTE_SYNC_FOUND_NOSPACE; 
+        ret = SRSLTE_SYNC_FOUND_NOSPACE;
       }
     } else {
       ret = SRSLTE_SYNC_NOFOUND;
@@ -607,7 +670,7 @@ srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, cf_t *input, uint32_t
     
     DEBUG("SYNC ret=%d N_id_2=%d find_offset=%d frame_len=%d, pos=%d peak=%.2f threshold=%.2f sf_idx=%d, CFO=%.3f kHz\n",
          ret, q->N_id_2, find_offset, q->frame_size, peak_pos, q->peak_value, 
-         q->threshold, q->sf_idx, 15*(q->cfo_i+q->mean_cfo));
+         q->threshold, q->sf_idx, 15*(srslte_sync_get_cfo(q)));
 
   } else if (srslte_N_id_2_isvalid(q->N_id_2)) {
     fprintf(stderr, "Must call srslte_sync_set_N_id_2() first!\n");
diff --git a/lib/src/phy/sync/test/pss_usrp.c b/lib/src/phy/sync/test/pss_usrp.c
index 8e8377b83..7c22652f7 100644
--- a/lib/src/phy/sync/test/pss_usrp.c
+++ b/lib/src/phy/sync/test/pss_usrp.c
@@ -221,7 +221,9 @@ int main(int argc, char **argv) {
   uint32_t max_peak = 0; 
   uint32_t max_peak_ = 0; 
   uint32_t min_peak = fft_size; 
-  uint32_t min_peak_ = fft_size; 
+  uint32_t min_peak_ = fft_size;
+
+  pss.filter_pss_enable = true;
   
   while(frame_cnt < nof_frames || nof_frames == -1) {
     n = srslte_rf_recv(&rf, buffer, flen - peak_offset, 1);
@@ -243,7 +245,7 @@ int main(int argc, char **argv) {
         
       if (peak_idx >= fft_size) {
 
-        // Estimate CFO 
+        // Estimate CFO
         cfo = srslte_pss_synch_cfo_compute(&pss, &buffer[peak_idx-fft_size]);
         mean_cfo = SRSLTE_VEC_CMA(cfo, mean_cfo, frame_cnt);        
 
@@ -255,10 +257,14 @@ int main(int argc, char **argv) {
           fprintf(stderr, "Error computing channel estimation\n");
           exit(-1);
         }
-        
-        // Find SSS 
+
+        // Find SSS
         int sss_idx = peak_idx-2*fft_size-(SRSLTE_CP_ISNORM(cp)?SRSLTE_CP_LEN(fft_size, SRSLTE_CP_NORM_LEN):SRSLTE_CP_LEN(fft_size, SRSLTE_CP_EXT_LEN));             
         if (sss_idx >= 0 && sss_idx < flen-fft_size) {
+
+          // Filter SSS
+          srslte_pss_synch_filter(&pss, &buffer[sss_idx], &buffer[sss_idx]);
+
           INFO("Full N_id_1: %d\n", srslte_sss_synch_N_id_1(&sss, m0, m1));
           srslte_sss_synch_m0m1_partial(&sss, &buffer[sss_idx], 1, ce, &m0, &m0_value, &m1, &m1_value);
           if (srslte_sss_synch_N_id_1(&sss, m0, m1) != N_id_1) {
@@ -301,12 +307,12 @@ int main(int argc, char **argv) {
     }
 
     printf("[%5d]: Pos: %5d (%d-%d), PSR: %4.1f (~%4.1f) Pdet: %4.2f, "
-           "FA: %4.2f, CFO: %+4.1f kHz, SFO: %+.2f Hz SSSmiss: %4.2f/%4.2f/%4.2f CPNorm: %.0f%%\r", 
+           "FA: %4.2f, CFO: %+7.1f Hz, SFO: %+.2f Hz SSSmiss: %4.2f/%4.2f/%4.2f CPNorm: %.0f%%\r",
            frame_cnt, 
            peak_idx, min_peak_, max_peak_,
            peak_value, mean_peak,
            (float) nof_det/frame_cnt, 
-           (float) nof_nopeakdet/frame_cnt, mean_cfo*15, sfo,
+           (float) nof_nopeakdet/frame_cnt, mean_cfo*15000, sfo,
            (float) sss_error1/nof_det,(float) sss_error2/nof_det,(float) sss_error3/nof_det,
            (float) cp_is_norm/nof_det * 100);
     
diff --git a/lib/src/phy/sync/test/sync_test.c b/lib/src/phy/sync/test/sync_test.c
index 1e3951884..9016c1baa 100644
--- a/lib/src/phy/sync/test/sync_test.c
+++ b/lib/src/phy/sync/test/sync_test.c
@@ -124,8 +124,7 @@ int main(int argc, char **argv) {
   /* Set a very high threshold to make sure the correlation is ok */
   srslte_sync_set_threshold(&syncobj, 5.0);
   srslte_sync_set_sss_algorithm(&syncobj, SSS_PARTIAL_3);
-  srslte_sync_set_cfo_enable(&syncobj, false); 
-
+  
   if (cell_id == -1) {
     cid = 0;
     max_cid = 49;
diff --git a/lib/src/phy/ue/ue_cell_search.c b/lib/src/phy/ue/ue_cell_search.c
index dc35fb48c..133aad144 100644
--- a/lib/src/phy/ue/ue_cell_search.c
+++ b/lib/src/phy/ue/ue_cell_search.c
@@ -116,7 +116,7 @@ int srslte_ue_cellsearch_init_multi(srslte_ue_cellsearch_t * q, uint32_t max_fra
       goto clean_exit; 
     }
     if (srslte_ue_sync_set_cell(&q->ue_sync, cell)) {
-      fprintf(stderr, "Error initiating ue_sync\n");
+      fprintf(stderr, "Error setting cell in ue_sync\n");
       goto clean_exit;
     }
 
@@ -292,6 +292,8 @@ int srslte_ue_cellsearch_scan_N_id_2(srslte_ue_cellsearch_t * q,
 
     srslte_ue_sync_set_N_id_2(&q->ue_sync, N_id_2);
     srslte_ue_sync_reset(&q->ue_sync);
+    srslte_ue_sync_cfo_reset(&q->ue_sync);
+
     do {
       
       ret = srslte_ue_sync_zerocopy_multi(&q->ue_sync, q->sf_buffer);
diff --git a/lib/src/phy/ue/ue_sync.c b/lib/src/phy/ue/ue_sync.c
index e9c02ca8b..b5d89743f 100644
--- a/lib/src/phy/ue/ue_sync.c
+++ b/lib/src/phy/ue/ue_sync.c
@@ -47,6 +47,15 @@
 #define DEFAULT_SAMPLE_OFFSET_CORRECT_PERIOD  0
 #define DEFAULT_SFO_EMA_COEFF                 0.1
 
+#define DEFAULT_CFO_BW      0.2
+#define DEFAULT_CFO_PSS_MIN 500  // typical bias of PSS estimation.
+#define DEFAULT_CFO_REF_MIN 0    // typical bias of REF estimation
+#define DEFAULT_CFO_REF_MAX 500  // Maximum detection offset of REF based estimation
+
+#define DEFAULT_PSS_STABLE_TIMEOUT     100  // Time after which the PSS is considered to be stable and we accept REF-CFO
+
+#define DEFAULT_CFO_EMA_TRACK 0.1
+
 cf_t dummy_buffer0[15*2048/2];
 cf_t dummy_buffer1[15*2048/2];
 
@@ -70,9 +79,10 @@ int srslte_ue_sync_init_file_multi(srslte_ue_sync_t *q, uint32_t nof_prb, char *
     q->file_mode = true; 
     q->sf_len = SRSLTE_SF_LEN(srslte_symbol_sz(nof_prb));
     q->file_cfo = -offset_freq; 
-    q->correct_cfo = true; 
     q->fft_size = srslte_symbol_sz(nof_prb);
     q->nof_rx_antennas = nof_rx_ant;
+
+    q->cfo_correct_enable = true;
     
     if (srslte_cfo_init(&q->file_cfo_correct, 2*q->sf_len)) {
       fprintf(stderr, "Error initiating CFO\n");
@@ -96,6 +106,7 @@ int srslte_ue_sync_init_file_multi(srslte_ue_sync_t *q, uint32_t nof_prb, char *
       free(file_offset_buffer);
     }
 
+    srslte_ue_sync_cfo_reset(q);
     srslte_ue_sync_reset(q);
     
     ret = SRSLTE_SUCCESS;
@@ -107,6 +118,33 @@ clean_exit:
   return ret; 
 }
 
+void srslte_ue_sync_cfo_reset(srslte_ue_sync_t *q)
+{
+  q->cfo_is_copied     = false;
+  q->cfo_current_value = 0;
+  srslte_sync_cfo_reset(&q->strack);
+  srslte_sync_cfo_reset(&q->sfind);
+}
+
+void srslte_ue_sync_reset(srslte_ue_sync_t *q) {
+
+  if (!q->file_mode) {
+    srslte_sync_reset(&q->sfind);
+    srslte_sync_reset(&q->strack);
+  } else {
+    q->sf_idx = 9;
+  }
+  q->pss_stable_timeout = false;
+  q->state = SF_FIND;
+  q->frame_ok_cnt = 0;
+  q->frame_no_cnt = 0;
+  q->frame_total_cnt = 0;
+  q->mean_sample_offset = 0.0;
+  q->next_rf_sample_offset = 0;
+  q->frame_find_cnt = 0;
+}
+
+
 int srslte_ue_sync_start_agc(srslte_ue_sync_t *q, double (set_gain_callback)(void*, double), float init_gain_value) {
   uint32_t nframes; 
   if (q->nof_recv_sf == 1) {
@@ -118,6 +156,8 @@ int srslte_ue_sync_start_agc(srslte_ue_sync_t *q, double (set_gain_callback)(voi
   q->do_agc = n==0?true:false;
   if (q->do_agc) {
     srslte_agc_set_gain(&q->agc, init_gain_value);
+    srslte_agc_set_target(&q->agc, 0.3);
+    srslte_agc_set_bandwidth(&q->agc, 0.8);
   }
   return n; 
 }
@@ -177,13 +217,22 @@ int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q,
     q->fft_size = srslte_symbol_sz(max_prb);
     q->sf_len = SRSLTE_SF_LEN(q->fft_size);
     q->file_mode = false; 
-    q->correct_cfo = true; 
-    q->agc_period = 0; 
+    q->agc_period = 0;
     q->sample_offset_correct_period = DEFAULT_SAMPLE_OFFSET_CORRECT_PERIOD; 
     q->sfo_ema                      = DEFAULT_SFO_EMA_COEFF; 
 
     q->max_prb = max_prb;
 
+    q->cfo_ref_max = DEFAULT_CFO_REF_MAX;
+    q->cfo_ref_min = DEFAULT_CFO_REF_MIN;
+    q->cfo_pss_min = DEFAULT_CFO_PSS_MIN;
+    q->cfo_loop_bw_pss = DEFAULT_CFO_BW;
+    q->cfo_loop_bw_ref = DEFAULT_CFO_BW;
+    q->cfo_correct_enable = true;
+
+    q->pss_stable_cnt     = 0;
+    q->pss_stable_timeout = DEFAULT_PSS_STABLE_TIMEOUT;
+
     if (search_cell) {
       
       /* If the cell is unkown, we search PSS/SSS in 5 ms */
@@ -218,6 +267,29 @@ int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q,
       }
     }
 
+    // Configure FIND and TRACK sync objects behaviour (this configuration is always the same)
+    srslte_sync_set_cfo_i_enable(&q->sfind,     false);
+    srslte_sync_set_cfo_cp_enable(&q->sfind,    true);
+    srslte_sync_set_cfo_pss_enable(&q->sfind,   true);
+    srslte_sync_set_pss_filt_enable(&q->sfind,  true);
+    srslte_sync_set_sss_filt_enable(&q->sfind,  true);
+
+    // During track, we do CFO correction outside the sync object
+    srslte_sync_set_cfo_i_enable(&q->strack,    false);
+    srslte_sync_set_cfo_cp_enable(&q->strack,   false);
+    srslte_sync_set_cfo_pss_enable(&q->strack,  true);
+    srslte_sync_set_pss_filt_enable(&q->strack, true);
+    srslte_sync_set_sss_filt_enable(&q->strack, false);
+
+    // FIXME: CP detection not working very well. Not supporting Extended CP right now
+    srslte_sync_cp_en(&q->strack, false);
+    srslte_sync_cp_en(&q->sfind,  false);
+
+
+    srslte_sync_sss_en(&q->strack, true);
+    q->decode_sss_on_track = true;
+
+
     ret = SRSLTE_SUCCESS;
   }
 
@@ -253,8 +325,6 @@ int srslte_ue_sync_set_cell(srslte_ue_sync_t *q, srslte_cell_t cell)
   if (q                                 != NULL &&
       srslte_nofprb_isvalid(cell.nof_prb))
   {
-    ret = SRSLTE_ERROR;
-
     if (cell.nof_prb > q->max_prb) {
       fprintf(stderr, "Error in ue_sync_set_cell(): cell.nof_prb must be lower than initialized\n");
       return SRSLTE_ERROR;
@@ -269,15 +339,10 @@ int srslte_ue_sync_set_cell(srslte_ue_sync_t *q, srslte_cell_t cell)
 
       /* If the cell is unkown, we search PSS/SSS in 5 ms */
       q->nof_recv_sf = 5;
-
-      q->decode_sss_on_track = true;
-
     } else {
 
       /* If the cell is known, we work on a 1ms basis */
       q->nof_recv_sf = 1;
-
-      q->decode_sss_on_track = true;
     }
 
     q->frame_len = q->nof_recv_sf*q->sf_len;
@@ -287,59 +352,54 @@ int srslte_ue_sync_set_cell(srslte_ue_sync_t *q, srslte_cell_t cell)
     }
 
     if(srslte_sync_resize(&q->sfind, q->frame_len, q->frame_len, q->fft_size)) {
-      fprintf(stderr, "Error initiating sync find\n");
+      fprintf(stderr, "Error setting cell sync find\n");
       return SRSLTE_ERROR;
     }
     if (cell.id == 1000) {
       if(srslte_sync_resize(&q->strack, q->frame_len, TRACK_FRAME_SIZE, q->fft_size)) {
-        fprintf(stderr, "Error initiating sync track\n");
+        fprintf(stderr, "Error setting cell sync track\n");
         return SRSLTE_ERROR;
       }
     } else {
       if(srslte_sync_resize(&q->strack, q->frame_len, SRSLTE_CP_LEN_NORM(1,q->fft_size), q->fft_size)) {
-        fprintf(stderr, "Error initiating sync track\n");
+        fprintf(stderr, "Error setting cell sync track\n");
         return SRSLTE_ERROR;
       }
     }
 
     if (cell.id == 1000) {
-      /* If the cell id is unknown, enable CP detection on find */
-      // FIXME: CP detection not working very well. Not supporting Extended CP right now
-      srslte_sync_cp_en(&q->sfind, false);
-      srslte_sync_cp_en(&q->strack, false);
+      q->nof_avg_find_frames = FIND_NOF_AVG_FRAMES;
 
-      srslte_sync_set_cfo_ema_alpha(&q->sfind, 0.8);
+      srslte_sync_set_cfo_ema_alpha(&q->sfind,  0.8);
       srslte_sync_set_cfo_ema_alpha(&q->strack, 0.1);
 
-      srslte_sync_cfo_i_detec_en(&q->sfind, false);
+      srslte_sync_set_em_alpha(&q->sfind,   1);
 
-      q->nof_avg_find_frames = FIND_NOF_AVG_FRAMES;
-      srslte_sync_set_threshold(&q->sfind, 2.0);
+      srslte_sync_set_threshold(&q->sfind,  2.0);
       srslte_sync_set_threshold(&q->strack, 1.2);
 
+      srslte_sync_set_cfo_ema_alpha(&q->sfind,  0.1);
+      srslte_sync_set_cfo_ema_alpha(&q->strack, 0.1);
+
     } else {
-      srslte_sync_set_N_id_2(&q->sfind, cell.id%3);
-      srslte_sync_set_N_id_2(&q->strack, cell.id%3);
-      q->sfind.cp = cell.cp;
+      q->sfind.cp  = cell.cp;
       q->strack.cp = cell.cp;
-      srslte_sync_cp_en(&q->sfind, false);
-      srslte_sync_cp_en(&q->strack, false);
 
-      srslte_sync_cfo_i_detec_en(&q->sfind, false);
+      srslte_sync_set_N_id_2(&q->sfind,  cell.id%3);
+      srslte_sync_set_N_id_2(&q->strack, cell.id%3);
 
-      srslte_sync_set_cfo_ema_alpha(&q->sfind, 0.1);
-      srslte_sync_set_cfo_ema_alpha(&q->strack, 0.1);
+      srslte_sync_set_cfo_ema_alpha(&q->sfind,  0.1);
+      srslte_sync_set_cfo_ema_alpha(&q->strack, DEFAULT_CFO_EMA_TRACK);
 
       /* In find phase and if the cell is known, do not average pss correlation
        * because we only capture 1 subframe and do not know where the peak is.
        */
       q->nof_avg_find_frames = 1;
-      srslte_sync_set_em_alpha(&q->sfind, 1);
-      srslte_sync_set_threshold(&q->sfind, 3.0);
+      srslte_sync_set_em_alpha(&q->sfind,   1);
+      srslte_sync_set_threshold(&q->sfind,  3.0);
 
-      srslte_sync_set_em_alpha(&q->strack, 0.2);
+      srslte_sync_set_em_alpha(&q->strack,  0.2);
       srslte_sync_set_threshold(&q->strack, 1.2);
-
     }
 
     srslte_ue_sync_reset(q);
@@ -355,37 +415,60 @@ void srslte_ue_sync_get_last_timestamp(srslte_ue_sync_t *q, srslte_timestamp_t *
   memcpy(timestamp, &q->last_timestamp, sizeof(srslte_timestamp_t));
 }
 
-uint32_t srslte_ue_sync_peak_idx(srslte_ue_sync_t *q) {
-  return q->peak_idx;
+void srslte_ue_sync_set_cfo_loop_bw(srslte_ue_sync_t *q, float bw_pss, float bw_ref,
+                                    float pss_tol, float ref_tol, float ref_max,
+                                    uint32_t pss_stable_conv_time) {
+  q->cfo_loop_bw_pss = bw_pss;
+  q->cfo_loop_bw_ref = bw_ref;
+  q->cfo_pss_min = pss_tol;
+  q->cfo_ref_min = ref_tol;
+  q->cfo_ref_max = ref_max;
+  q->pss_stable_timeout = pss_stable_conv_time;
 }
 
 void srslte_ue_sync_set_cfo_ema(srslte_ue_sync_t *q, float ema) {
-  srslte_sync_set_cfo_ema_alpha(&q->sfind, ema);
   srslte_sync_set_cfo_ema_alpha(&q->strack, ema);
 }
 
-srslte_ue_sync_state_t srslte_ue_sync_get_state(srslte_ue_sync_t *q) {
-  return q->state;
+void srslte_ue_sync_set_cfo_ref(srslte_ue_sync_t *q, float ref_cfo)
+{
+  // Accept REF-based CFO adjustments only after PSS CFO is stable
+  if (q->pss_is_stable) {
+    if (fabsf(ref_cfo)*15000 > q->cfo_ref_min) {
+      if (fabsf(ref_cfo)*15000 > q->cfo_ref_max) {
+        ref_cfo = (ref_cfo>0?q->cfo_ref_max:-q->cfo_ref_max)/15000;
+      }
+      q->cfo_current_value += ref_cfo*q->cfo_loop_bw_ref;
+    }
+  }
 }
+
 uint32_t srslte_ue_sync_get_sfidx(srslte_ue_sync_t *q) {
   return q->sf_idx;    
 }
 
-void srslte_ue_sync_cfo_i_detec_en(srslte_ue_sync_t *q, bool enable) {
-  srslte_sync_cfo_i_detec_en(&q->strack, enable);
-  srslte_sync_cfo_i_detec_en(&q->sfind, enable);  
+void srslte_ue_sync_set_cfo_i_enable(srslte_ue_sync_t *q, bool enable) {
+  printf("Warning: Setting integer CFO detection/correction. This is experimental!\n");
+  srslte_sync_set_cfo_i_enable(&q->strack, enable);
+  srslte_sync_set_cfo_i_enable(&q->sfind,  enable);
 }
 
 float srslte_ue_sync_get_cfo(srslte_ue_sync_t *q) {
-  return 15000 * srslte_sync_get_cfo(&q->strack);
+  return 15000 * q->cfo_current_value;
 }
 
-void srslte_ue_sync_set_cfo(srslte_ue_sync_t *q, float cfo) {  
-  srslte_sync_set_cfo(&q->sfind, cfo/15000);
-  srslte_sync_set_cfo(&q->strack, cfo/15000);
+void srslte_ue_sync_copy_cfo(srslte_ue_sync_t *q, srslte_ue_sync_t *src_obj) {
+  // Copy find object internal CFO averages
+  srslte_sync_copy_cfo(&q->sfind,  &src_obj->sfind);
+  // Current CFO is tracking-phase CFO of previous object
+  q->cfo_current_value = src_obj->cfo_current_value;
+  q->cfo_is_copied = true;
 }
 
-void srslte_ue_sync_set_cfo_tol(srslte_ue_sync_t *q, float cfo_tol) {}
+void srslte_ue_sync_set_cfo_tol(srslte_ue_sync_t *q, float cfo_tol) {
+  srslte_sync_set_cfo_tol(&q->strack, cfo_tol);
+  srslte_sync_set_cfo_tol(&q->sfind, cfo_tol);
+}
 
 float srslte_ue_sync_get_sfo(srslte_ue_sync_t *q) {
   return q->mean_sfo/5e-3;
@@ -404,7 +487,8 @@ void srslte_ue_sync_set_sfo_ema(srslte_ue_sync_t *q, float ema_coefficient) {
 }
 
 void srslte_ue_sync_decode_sss_on_track(srslte_ue_sync_t *q, bool enabled) {
-  q->decode_sss_on_track = enabled; 
+  q->decode_sss_on_track = enabled;
+  srslte_sync_sss_en(&q->strack, q->decode_sss_on_track);
 }
 
 void srslte_ue_sync_set_N_id_2(srslte_ue_sync_t *q, uint32_t N_id_2) {
@@ -432,7 +516,7 @@ static int find_peak_ok(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE_MAX_PORTS
   q->frame_find_cnt++;  
   DEBUG("Found peak %d at %d, value %.3f, Cell_id: %d CP: %s\n", 
        q->frame_find_cnt, q->peak_idx, 
-       srslte_sync_get_last_peak_value(&q->sfind), q->cell.id, srslte_cp_string(q->cell.cp));       
+       srslte_sync_get_peak_value(&q->sfind), q->cell.id, srslte_cp_string(q->cell.cp));
 
   if (q->frame_find_cnt >= q->nof_avg_find_frames || q->peak_idx < 2*q->fft_size) {
     INFO("Realigning frame, reading %d samples\n", q->peak_idx+q->sf_len/2);
@@ -449,11 +533,13 @@ static int find_peak_ok(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE_MAX_PORTS
     q->mean_sample_offset = 0; 
     
     /* Goto Tracking state */
-    q->state = SF_TRACK;     
-    
-    /* Initialize track state CFO */
-    q->strack.mean_cfo = q->sfind.mean_cfo;
-    q->strack.cfo_i    = q->sfind.cfo_i; 
+    q->state = SF_TRACK;
+
+    /* Set CFO values. Since we correct before track, the initial track state is CFO=0 Hz */
+    if (!q->cfo_is_copied) {
+      q->cfo_current_value = srslte_sync_get_cfo(&q->sfind);
+    }
+    srslte_sync_cfo_reset(&q->strack);
   }
     
   return 0;
@@ -489,6 +575,23 @@ static int track_peak_ok(srslte_ue_sync_t *q, uint32_t track_idx) {
     q->mean_sample_offset = q->last_sample_offset; 
   }
 
+  /* Adjust current CFO estimation with PSS
+   * Since sync track has enabled only PSS-based correlation, get_cfo() returns that value only, already filtered.
+   */
+  INFO("TRACK: cfo_current=%f, cfo_strack=%f\n", 15000*q->cfo_current_value, 15000*srslte_sync_get_cfo(&q->strack));
+  if (15000*fabsf(srslte_sync_get_cfo(&q->strack)) > q->cfo_pss_min) {
+    q->cfo_current_value += srslte_sync_get_cfo(&q->strack)*q->cfo_loop_bw_pss;
+    q->pss_stable_cnt = 0;
+    q->pss_is_stable  = false;
+  } else {
+    if (!q->pss_is_stable) {
+      q->pss_stable_cnt++;
+      if (q->pss_stable_cnt >= q->pss_stable_timeout) {
+        q->pss_is_stable = true;
+      }
+    }
+  }
+
   // Compute cumulative moving average time offset */
   if (!frame_idx) {
     // Adjust RF sampling time based on the mean sampling offset
@@ -541,13 +644,7 @@ static int track_peak_no(srslte_ue_sync_t *q) {
     return 0; 
   } else {
     INFO("Tracking peak not found. Peak %.3f, %d lost\n", 
-         srslte_sync_get_last_peak_value(&q->strack), (int) q->frame_no_cnt);    
-    /*
-    printf("Saving files: pss_corr (%d), input (%d)\n", q->strack.pss.frame_size, SRSLTE_SF_LEN_PRB(q->cell.nof_prb));
-    srslte_vec_save_file("pss_corr", q->strack.pss.conv_output_avg, q->strack.pss.frame_size*sizeof(float));
-    srslte_vec_save_file("input", q->input_buffer, SRSLTE_SF_LEN_PRB(q->cell.nof_prb)*sizeof(cf_t));
-    exit(-1);
-    */
+         srslte_sync_get_peak_value(&q->strack), (int) q->frame_no_cnt);
     return 1;
   }
 
@@ -576,12 +673,10 @@ static int receive_samples(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE_MAX_PO
   return SRSLTE_SUCCESS; 
 }
 
-bool first_track = true; 
-
 int srslte_ue_sync_zerocopy(srslte_ue_sync_t *q, cf_t *input_buffer) {
-  cf_t *_input_buffer[SRSLTE_MAX_PORTS]; 
-  _input_buffer[0] = input_buffer; 
-  return srslte_ue_sync_zerocopy_multi(q, _input_buffer);  
+  cf_t *_input_buffer[SRSLTE_MAX_PORTS];
+  _input_buffer[0] = input_buffer;
+  return srslte_ue_sync_zerocopy_multi(q, _input_buffer);
 }
 
 /* Returns 1 if the subframe is synchronized in time, 0 otherwise */
@@ -608,7 +703,7 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
           return SRSLTE_ERROR;
         }
       }
-      if (q->correct_cfo) {
+      if (q->cfo_correct_enable) {
         for (int i = 0; i < q->nof_rx_antennas; i++) {
           srslte_cfo_correct(&q->file_cfo_correct,
                              input_buffer[i],
@@ -623,19 +718,20 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
       INFO("Reading %d samples. sf_idx = %d\n", q->sf_len, q->sf_idx);
       ret = 1;
     } else {
+
       if (receive_samples(q, input_buffer)) {
         fprintf(stderr, "Error receiving samples\n");
         return SRSLTE_ERROR;
       }
 
       switch (q->state) {
-        case SF_FIND:     
+        case SF_FIND:
           switch(srslte_sync_find(&q->sfind, input_buffer[0], 0, &q->peak_idx)) {
             case SRSLTE_SYNC_ERROR: 
               ret = SRSLTE_ERROR; 
               fprintf(stderr, "Error finding correlation peak (%d)\n", ret);
               return SRSLTE_ERROR;
-            case SRSLTE_SYNC_FOUND: 
+            case SRSLTE_SYNC_FOUND:
               ret = find_peak_ok(q, input_buffer);
               break;
             case SRSLTE_SYNC_FOUND_NOSPACE:
@@ -657,37 +753,39 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
         case SF_TRACK:
          
           ret = 1;
-          
-          srslte_sync_sss_en(&q->strack, q->decode_sss_on_track);
-          
+
+          // Increase subframe counter
           q->sf_idx = (q->sf_idx + q->nof_recv_sf) % 10;
 
-          /* Every SF idx 0 and 5, find peak around known position q->peak_idx */
-          if (q->sf_idx == 0 || q->sf_idx == 5) {
+          // Correct CFO before PSS/SSS tracking using the sync object corrector (initialized for 1 ms)
+          if (q->cfo_correct_enable) {
+            for (int i=0;i<q->nof_rx_antennas;i++) {
+              srslte_cfo_correct(&q->strack.cfo_corr_frame,
+                                 input_buffer[i],
+                                 input_buffer[i],
+                                 -q->cfo_current_value/q->fft_size);
+            }
+          }
 
+          /* Every SF idx 0 and 5, find peak around known position q->peak_idx */
+          if (q->sf_idx == 0 || q->sf_idx == 5)
+          {
+            // Process AGC every period
             if (q->do_agc && (q->agc_period == 0 || 
                              (q->agc_period && (q->frame_total_cnt%q->agc_period) == 0))) 
             {
               srslte_agc_process(&q->agc, input_buffer[0], q->sf_len);        
             }
 
-            #ifdef MEASURE_EXEC_TIME
-            struct timeval t[3];
-            gettimeofday(&t[1], NULL);
-            #endif
-            
-            track_idx = 0; 
-            
-            /* Track PSS/SSS around the expected PSS position 
+            /* Track PSS/SSS around the expected PSS position
              * In tracking phase, the subframe carrying the PSS is always the last one of the frame
              */
-            
+            track_idx = 0;
             switch(srslte_sync_find(&q->strack, input_buffer[0], 
                                     q->frame_len - q->sf_len/2 - q->fft_size - q->strack.max_offset/2, 
                                     &track_idx)) 
             {
               case SRSLTE_SYNC_ERROR:
-                ret = SRSLTE_ERROR; 
                 fprintf(stderr, "Error tracking correlation peak\n");
                 return SRSLTE_ERROR;
               case SRSLTE_SYNC_FOUND: 
@@ -697,60 +795,26 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
                 // It's very very unlikely that we fall here because this event should happen at FIND phase only 
                 ret = 0; 
                 q->state = SF_FIND; 
-                printf("Warning: No space for SSS/CP while in tracking phase\n"); 
+                INFO("Warning: No space for SSS/CP while in tracking phase\n");
                 break;
               case SRSLTE_SYNC_NOFOUND: 
                 ret = track_peak_no(q); 
                 break;
             }
             
-            #ifdef MEASURE_EXEC_TIME
-            gettimeofday(&t[2], NULL);
-            get_time_interval(t);
-            q->mean_exec_time = (float) SRSLTE_VEC_CMA((float) t[0].tv_usec, q->mean_exec_time, q->frame_total_cnt);
-            #endif
-
             if (ret == SRSLTE_ERROR) {
               fprintf(stderr, "Error processing tracking peak\n");
               q->state = SF_FIND; 
               return SRSLTE_SUCCESS;
-            } 
-                      
-            q->frame_total_cnt++;       
-          } 
-          if (q->correct_cfo) {
-            for (int i=0;i<q->nof_rx_antennas;i++) {
-              srslte_cfo_correct(&q->strack.cfocorr,
-                          input_buffer[i], 
-                          input_buffer[i], 
-                          -srslte_sync_get_cfo(&q->strack) / q->fft_size);                         
             }
-          }          
+
+            q->frame_total_cnt++;
+          }
         break;
       }
-      
     }
   }  
   return ret; 
 }
 
-void srslte_ue_sync_reset(srslte_ue_sync_t *q) {
-
-  if (!q->file_mode) {
-    srslte_sync_reset(&q->sfind);
-    srslte_sync_reset(&q->strack);
-  } else {
-    q->sf_idx = 9;
-  }
-  q->state = SF_FIND;
-  q->frame_ok_cnt = 0;
-  q->frame_no_cnt = 0;
-  q->frame_total_cnt = 0; 
-  q->mean_sample_offset = 0.0;
-  q->next_rf_sample_offset = 0;
-  q->frame_find_cnt = 0; 
-  #ifdef MEASURE_EXEC_TIME
-  q->mean_exec_time = 0;
-  #endif      
-}
 
diff --git a/lib/src/phy/ue/ue_ul.c b/lib/src/phy/ue/ue_ul.c
index 43772f0f9..6bf70741e 100644
--- a/lib/src/phy/ue/ue_ul.c
+++ b/lib/src/phy/ue/ue_ul.c
@@ -38,7 +38,7 @@
 
 #define MAX_SFLEN     SRSLTE_SF_LEN(srslte_symbol_sz(max_prb))
 
-#define DEFAULT_CFO_TOL   50.0 // Hz
+#define DEFAULT_CFO_TOL   0.0 // Hz
 
 int srslte_ue_ul_init(srslte_ue_ul_t *q,
                       cf_t *out_buffer,
diff --git a/lib/src/phy/utils/convolution.c b/lib/src/phy/utils/convolution.c
index f96bdd4a2..59807ad5b 100644
--- a/lib/src/phy/utils/convolution.c
+++ b/lib/src/phy/utils/convolution.c
@@ -115,7 +115,7 @@ void srslte_conv_fft_cc_free(srslte_conv_fft_cc_t *q) {
 
 }
 
-uint32_t srslte_conv_fft_cc_run_opt(srslte_conv_fft_cc_t *q, cf_t *input, cf_t *filter_freq, cf_t *output) 
+uint32_t srslte_conv_fft_cc_run_opt(srslte_conv_fft_cc_t *q, const cf_t *input, const cf_t *filter_freq, cf_t *output)
 {
     srslte_dft_run_c(&q->input_plan, input, q->input_fft);
     srslte_vec_prod_ccc(q->input_fft, filter_freq, q->output_fft, q->output_len);
@@ -125,7 +125,7 @@ uint32_t srslte_conv_fft_cc_run_opt(srslte_conv_fft_cc_t *q, cf_t *input, cf_t *
 
 }
 
-uint32_t srslte_conv_fft_cc_run(srslte_conv_fft_cc_t *q, cf_t *input, cf_t *filter, cf_t *output) {
+uint32_t srslte_conv_fft_cc_run(srslte_conv_fft_cc_t *q, const cf_t *input, const cf_t *filter, cf_t *output) {
   
   srslte_dft_run_c(&q->filter_plan, filter, q->filter_fft);
 
@@ -133,7 +133,7 @@ uint32_t srslte_conv_fft_cc_run(srslte_conv_fft_cc_t *q, cf_t *input, cf_t *filt
 
 }
 
-uint32_t srslte_conv_cc(cf_t *input, cf_t *filter, cf_t *output, uint32_t input_len, uint32_t filter_len) {
+uint32_t srslte_conv_cc(const cf_t *input, const cf_t *filter, cf_t *output, uint32_t input_len, uint32_t filter_len) {
   uint32_t i;
   uint32_t M = filter_len; 
   uint32_t N = input_len; 
diff --git a/lib/src/phy/utils/test/vector_test.c b/lib/src/phy/utils/test/vector_test.c
index 9058e8813..cb7eff480 100644
--- a/lib/src/phy/utils/test/vector_test.c
+++ b/lib/src/phy/utils/test/vector_test.c
@@ -47,8 +47,10 @@ bool verbose = false;
 #define MAX_FUNCTIONS (64)
 #define MAX_BLOCKS (16)
 
+
 #define RANDOM_F() ((float)rand())/((float)RAND_MAX)
 #define RANDOM_S() ((int16_t)(rand() && 0x800F))
+#define RANDOM_B() ((int8_t)(rand() && 0x8008))
 #define RANDOM_CF() (RANDOM_F() + _Complex_I*RANDOM_F())
 
 #define TEST_CALL(TEST_CODE)   gettimeofday(&start, NULL);\
@@ -87,6 +89,29 @@ float squared_error (cf_t a, cf_t b) {
   return diff_re*diff_re + diff_im*diff_im;
 }
 
+ TEST(srslte_vec_xor_bbb,
+     MALLOC(int8_t, x);
+         MALLOC(int8_t, y);
+         MALLOC(int8_t, z);
+
+         cf_t gold = 0.0f;
+         for (int i = 0; i < block_size; i++) {
+           x[i] = RANDOM_B();
+           y[i] = RANDOM_B();
+         }
+
+         TEST_CALL(srslte_vec_xor_bbb(x, y, z, block_size))
+
+         for (int i = 0; i < block_size; i++) {
+           gold = x[i] ^ y[i];
+           mse += cabsf(gold - z[i]);
+         }
+
+         free(x);
+         free(y);
+         free(z);
+)
+
 TEST(srslte_vec_acc_ff,
      MALLOC(float, x);
          float z;
@@ -416,7 +441,7 @@ TEST(srslte_vec_convert_fi,
     x[i] = (float) RANDOM_F();
   }
 
-  TEST_CALL(srslte_vec_convert_fi(x, z, scale, block_size))
+  TEST_CALL(srslte_vec_convert_fi(x, scale, z, block_size))
 
   for (int i = 0; i < block_size; i++) {
       gold = (short) ((x[i] * scale));
@@ -613,8 +638,8 @@ TEST(srslte_vec_div_fff,
 
          cf_t gold;
          for (int i = 0; i < block_size; i++) {
-           x[i] = RANDOM_F();
-           y[i] = RANDOM_F();
+           x[i] = RANDOM_F() + 0.0001;
+           y[i] = RANDOM_F()+ 0.0001;
          }
 
          TEST_CALL(srslte_vec_div_fff(x, y, z, block_size))
@@ -690,6 +715,11 @@ int main(int argc, char **argv) {
   for (uint32_t block_size = 1; block_size <= 1024*8; block_size *= 2) {
     func_count = 0;
 
+
+    passed[func_count][size_count] = test_srslte_vec_xor_bbb(func_names[func_count], &timmings[func_count][size_count], block_size);
+    func_count++;
+
+
     passed[func_count][size_count] = test_srslte_vec_acc_ff(func_names[func_count], &timmings[func_count][size_count], block_size);
     func_count++;
 
diff --git a/lib/src/phy/utils/vector.c b/lib/src/phy/utils/vector.c
index 3bb7fb08f..35457fcb5 100644
--- a/lib/src/phy/utils/vector.c
+++ b/lib/src/phy/utils/vector.c
@@ -37,75 +37,73 @@
 
 
 
-// Used in PRACH detector, AGC and chest_dl for noise averaging
-float srslte_vec_acc_ff(float *x, uint32_t len) {
-  return srslte_vec_acc_ff_simd(x, len);
+void srslte_vec_xor_bbb(int8_t *x,int8_t *y,int8_t *z, const uint32_t len) {
+  srslte_vec_xor_bbb_simd(x, y, z, len);
 }
 
-void srslte_vec_ema_filter(cf_t *new_data, cf_t *average, cf_t *output, float coeff, uint32_t len) {
-  srslte_vec_sc_prod_cfc(new_data, coeff, new_data, len);
-  srslte_vec_sc_prod_cfc(average, 1-coeff, output, len);
-  srslte_vec_sum_ccc(output, new_data, output, len);
+// Used in PRACH detector, AGC and chest_dl for noise averaging
+float srslte_vec_acc_ff(const float *x, const uint32_t len) {
+  return srslte_vec_acc_ff_simd(x, len);
 }
 
-cf_t srslte_vec_acc_cc(cf_t *x, uint32_t len) {
+cf_t srslte_vec_acc_cc(const cf_t *x, const uint32_t len) {
   return srslte_vec_acc_cc_simd(x, len);
 }
 
-void srslte_vec_sub_fff(float *x, float *y, float *z, uint32_t len) {
+void srslte_vec_sub_fff(const float *x, const float *y, float *z, const uint32_t len) {
   srslte_vec_sub_fff_simd(x, y, z, len);
 }
 
-void srslte_vec_sub_sss(int16_t *x, int16_t *y, int16_t *z, uint32_t len) {
+void srslte_vec_sub_sss(const int16_t *x, const int16_t *y, int16_t *z, const uint32_t len) {
   srslte_vec_sub_sss_simd(x, y, z, len);
 }
 
 // Noise estimation in chest_dl, interpolation 
-void srslte_vec_sub_ccc(cf_t *x, cf_t *y, cf_t *z, uint32_t len) {
-  return srslte_vec_sub_fff((float*) x,(float*) y,(float*) z, 2*len);
+void srslte_vec_sub_ccc(const cf_t *x, const cf_t *y, cf_t *z, const uint32_t len) {
+  return srslte_vec_sub_fff((const float*) x,(const float*) y,(float*) z, 2*len);
 }
 
 // Used in PSS/SSS and sum_ccc
-void srslte_vec_sum_fff(float *x, float *y, float *z, uint32_t len) {
+void srslte_vec_sum_fff(const float *x, const float *y, float *z, const uint32_t len) {
   srslte_vec_add_fff_simd(x, y, z, len);
 }
 
-void srslte_vec_sum_sss(int16_t *x, int16_t *y, int16_t *z, uint32_t len) {
+void srslte_vec_sum_sss(const int16_t *x, const int16_t *y, int16_t *z, const uint32_t len) {
   srslte_vec_sum_sss_simd(x, y, z, len);
 }
 
-void srslte_vec_sum_ccc(cf_t *x, cf_t *y, cf_t *z, uint32_t len) {
+void srslte_vec_sum_ccc(const cf_t *x, const cf_t *y, cf_t *z, const uint32_t len) {
   srslte_vec_sum_fff((float*) x,(float*) y,(float*) z,2*len);
 }
 
 // PSS, PBCH, DEMOD, FFTW, etc.
-void srslte_vec_sc_prod_fff(float *x, float h, float *z, uint32_t len) {
+void srslte_vec_sc_prod_fff(const float *x, const float h, float *z, const uint32_t len) {
   srslte_vec_sc_prod_fff_simd(x, h, z, len);
 }
 
 // Used throughout 
-void srslte_vec_sc_prod_cfc(cf_t *x, float h, cf_t *z, uint32_t len) { 
+void srslte_vec_sc_prod_cfc(const const cf_t *x, const float h, cf_t *z, const uint32_t len) {
   srslte_vec_sc_prod_cfc_simd(x,h,z,len);
 }
 
 // Chest UL 
-void srslte_vec_sc_prod_ccc(cf_t *x, cf_t h, cf_t *z, uint32_t len) {
+void srslte_vec_sc_prod_ccc(const cf_t *x, const cf_t h, cf_t *z, const uint32_t len) {
   srslte_vec_sc_prod_ccc_simd(x,h,z,len);
 }
 
 // Used in turbo decoder 
-void srslte_vec_convert_if(int16_t *x, float *z, float scale, uint32_t len) {
+void srslte_vec_convert_if(const int16_t *x, const float scale, float *z, const uint32_t len) {
   int i;
   for (i=0;i<len;i++) {
     z[i] = ((float) x[i])/scale;
   }
 }
 
-void srslte_vec_convert_fi(float *x, int16_t *z, float scale, uint32_t len) {
+void srslte_vec_convert_fi(const float *x, const float scale, int16_t *z, const uint32_t len) {
   srslte_vec_convert_fi_simd(x, z, scale, len);
 }
 
-void srslte_vec_lut_sss(short *x, unsigned short *lut, short *y, uint32_t len) {
+void srslte_vec_lut_sss(const short *x, const unsigned short *lut, short *y, const uint32_t len) {
   srslte_vec_lut_sss_simd(x, lut, y, len);
 }
 
@@ -134,7 +132,7 @@ void *srslte_vec_realloc(void *ptr, uint32_t old_size, uint32_t new_size) {
 }
 
 
-void srslte_vec_fprint_c(FILE *stream, cf_t *x, uint32_t len) {
+void srslte_vec_fprint_c(FILE *stream, cf_t *x, const uint32_t len) {
   int i;
   fprintf(stream, "[");
   for (i=0;i<len;i++) {
@@ -143,7 +141,7 @@ void srslte_vec_fprint_c(FILE *stream, cf_t *x, uint32_t len) {
   fprintf(stream, "];\n");
 }
 
-void srslte_vec_fprint_f(FILE *stream, float *x, uint32_t len) {
+void srslte_vec_fprint_f(FILE *stream, float *x, const uint32_t len) {
   int i;
   fprintf(stream, "[");
   for (i=0;i<len;i++) {
@@ -153,7 +151,7 @@ void srslte_vec_fprint_f(FILE *stream, float *x, uint32_t len) {
 }
 
 
-void srslte_vec_fprint_b(FILE *stream, uint8_t *x, uint32_t len) {
+void srslte_vec_fprint_b(FILE *stream, uint8_t *x, const uint32_t len) {
   int i;
   fprintf(stream, "[");
   for (i=0;i<len;i++) {
@@ -162,7 +160,7 @@ void srslte_vec_fprint_b(FILE *stream, uint8_t *x, uint32_t len) {
   fprintf(stream, "];\n");
 }
 
-void srslte_vec_fprint_byte(FILE *stream, uint8_t *x, uint32_t len) {
+void srslte_vec_fprint_byte(FILE *stream, uint8_t *x, const uint32_t len) {
   int i;
   fprintf(stream, "[");
   for (i=0;i<len;i++) {
@@ -171,7 +169,7 @@ void srslte_vec_fprint_byte(FILE *stream, uint8_t *x, uint32_t len) {
   fprintf(stream, "];\n");
 }
 
-void srslte_vec_fprint_i(FILE *stream, int *x, uint32_t len) {
+void srslte_vec_fprint_i(FILE *stream, int *x, const uint32_t len) {
   int i;
   fprintf(stream, "[");
   for (i=0;i<len;i++) {
@@ -180,7 +178,7 @@ void srslte_vec_fprint_i(FILE *stream, int *x, uint32_t len) {
   fprintf(stream, "];\n");
 }
 
-void srslte_vec_fprint_s(FILE *stream, short *x, uint32_t len) {
+void srslte_vec_fprint_s(FILE *stream, short *x, const uint32_t len) {
   int i;
   fprintf(stream, "[");
   for (i=0;i<len;i++) {
@@ -189,7 +187,7 @@ void srslte_vec_fprint_s(FILE *stream, short *x, uint32_t len) {
   fprintf(stream, "];\n");
 }
 
-void srslte_vec_fprint_hex(FILE *stream, uint8_t *x, uint32_t len) {
+void srslte_vec_fprint_hex(FILE *stream, uint8_t *x, const uint32_t len) {
   uint32_t i, nbytes; 
   uint8_t byte;
   nbytes = len/8;
@@ -205,7 +203,7 @@ void srslte_vec_fprint_hex(FILE *stream, uint8_t *x, uint32_t len) {
   fprintf(stream, "];\n");
 }
 
-void srslte_vec_sprint_hex(char *str, uint8_t *x, uint32_t len) {
+void srslte_vec_sprint_hex(char *str, uint8_t *x, const uint32_t len) {
   uint32_t i, nbytes; 
   uint8_t byte;
   nbytes = len/8;
@@ -222,7 +220,7 @@ void srslte_vec_sprint_hex(char *str, uint8_t *x, uint32_t len) {
   n+=sprintf(&str[n], "]");
 }
 
-void srslte_vec_save_file(char *filename, void *buffer, uint32_t len) {
+void srslte_vec_save_file(char *filename, const void *buffer, const uint32_t len) {
   FILE *f; 
   f = fopen(filename, "w");
   if (f) {
@@ -233,7 +231,7 @@ void srslte_vec_save_file(char *filename, void *buffer, uint32_t len) {
   }  
 }
 
-void srslte_vec_load_file(char *filename, void *buffer, uint32_t len) {
+void srslte_vec_load_file(char *filename, void *buffer, const uint32_t len) {
   FILE *f; 
   f = fopen(filename, "r");
   if (f) {
@@ -245,7 +243,7 @@ void srslte_vec_load_file(char *filename, void *buffer, uint32_t len) {
 }
 
 // Used in PSS
-void srslte_vec_conj_cc(cf_t *x, cf_t *y, uint32_t len) {
+void srslte_vec_conj_cc(const cf_t *x, cf_t *y, const uint32_t len) {
   /* This function is used in initialisation only, then no optimisation is required */
   int i;
   for (i=0;i<len;i++) {
@@ -254,57 +252,58 @@ void srslte_vec_conj_cc(cf_t *x, cf_t *y, uint32_t len) {
 }
 
 // Used in scrambling complex 
-void srslte_vec_prod_cfc(cf_t *x, float *y, cf_t *z, uint32_t len) {
+void srslte_vec_prod_cfc(const cf_t *x, const float *y, cf_t *z, const uint32_t len) {
   srslte_vec_prod_cfc_simd(x, y, z, len);
 }
 
 // Used in scrambling float
-void srslte_vec_prod_fff(float *x, float *y, float *z, uint32_t len) {
+void srslte_vec_prod_fff(const float *x, const float *y, float *z, const uint32_t len) {
   srslte_vec_prod_fff_simd(x, y, z, len);
 }
 
 // Scrambling Short
-void srslte_vec_prod_sss(int16_t *x, int16_t *y, int16_t *z, uint32_t len) {
+void srslte_vec_prod_sss(const int16_t *x, const int16_t *y, int16_t *z, const uint32_t len) {
   srslte_vec_prod_sss_simd(x,y,z,len);
 }
 
 // CFO and OFDM processing
-void srslte_vec_prod_ccc(cf_t *x,cf_t *y, cf_t *z, uint32_t len) {
+void srslte_vec_prod_ccc(const cf_t *x, const cf_t *y, cf_t *z, const uint32_t len) {
   srslte_vec_prod_ccc_simd(x,y,z,len);
 }
 
-void srslte_vec_prod_ccc_split(float *x_re, float *x_im, float *y_re, float *y_im, float *z_re, float *z_im, uint32_t len) {
+void srslte_vec_prod_ccc_split(const float *x_re, const float *x_im, const float *y_re, const float *y_im,
+                               float *z_re, float *z_im, const uint32_t len) {
   srslte_vec_prod_ccc_split_simd(x_re, x_im, y_re , y_im, z_re,z_im, len);
 }
 
 // PRACH, CHEST UL, etc. 
-void srslte_vec_prod_conj_ccc(cf_t *x,cf_t *y, cf_t *z, uint32_t len) {
+void srslte_vec_prod_conj_ccc(const const cf_t *x, const cf_t *y, cf_t *z, const uint32_t len) {
   srslte_vec_prod_conj_ccc_simd(x,y,z,len);
 }
 
 //#define DIV_USE_VEC
 
 // Used in SSS 
-void srslte_vec_div_ccc(cf_t *x, cf_t *y, cf_t *z, uint32_t len) {
+void srslte_vec_div_ccc(const cf_t *x, const cf_t *y, cf_t *z, const uint32_t len) {
   srslte_vec_div_ccc_simd(x, y, z, len);
 }
 
 /* Complex division by float z=x/y */
-void srslte_vec_div_cfc(cf_t *x, float *y, cf_t *z, uint32_t len) {
+void srslte_vec_div_cfc(const cf_t *x, const float *y, cf_t *z, const uint32_t len) {
   srslte_vec_div_cfc_simd(x, y, z, len);
 }
 
-void srslte_vec_div_fff(float *x, float *y, float *z, uint32_t len) {
+void srslte_vec_div_fff(const float *x, const float *y, float *z, const uint32_t len) {
   srslte_vec_div_fff_simd(x, y, z, len);
 }
 
 // PSS. convolution 
-cf_t srslte_vec_dot_prod_ccc(cf_t *x, cf_t *y, uint32_t len) {
+cf_t srslte_vec_dot_prod_ccc(const const cf_t *x, const cf_t *y, const uint32_t len) {
   return srslte_vec_dot_prod_ccc_simd(x, y, len);
 }
 
 // Convolution filter and in SSS search 
-cf_t srslte_vec_dot_prod_cfc(cf_t *x, float *y, uint32_t len) {
+cf_t srslte_vec_dot_prod_cfc(const const cf_t *x, const float *y, const uint32_t len) {
   uint32_t i;
   cf_t res = 0;
   for (i=0;i<len;i++) {
@@ -314,12 +313,12 @@ cf_t srslte_vec_dot_prod_cfc(cf_t *x, float *y, uint32_t len) {
 }
 
 // SYNC 
-cf_t srslte_vec_dot_prod_conj_ccc(cf_t *x, cf_t *y, uint32_t len) {
+cf_t srslte_vec_dot_prod_conj_ccc(const cf_t *x, const cf_t *y, const uint32_t len) {
   return srslte_vec_dot_prod_conj_ccc_simd(x, y, len);
 }
 
 // PHICH 
-float srslte_vec_dot_prod_fff(float *x, float *y, uint32_t len) {
+float srslte_vec_dot_prod_fff(const float *x, const float *y, const uint32_t len) {
   uint32_t i;
   float res = 0;
   for (i=0;i<len;i++) {
@@ -328,16 +327,16 @@ float srslte_vec_dot_prod_fff(float *x, float *y, uint32_t len) {
   return res;
 }
 
-int32_t srslte_vec_dot_prod_sss(int16_t *x, int16_t *y, uint32_t len) {
+int32_t srslte_vec_dot_prod_sss(const int16_t *x, const int16_t *y, const uint32_t len) {
   return srslte_vec_dot_prod_sss_simd(x, y, len);
 }
 
-float srslte_vec_avg_power_cf(cf_t *x, uint32_t len) {
+float srslte_vec_avg_power_cf(const cf_t *x, const uint32_t len) {
   return crealf(srslte_vec_dot_prod_conj_ccc(x,x,len)) / len;
 }
 
 // Correlation assumes zero-mean x and y
-float srslte_vec_corr_ccc(cf_t *x, cf_t *y, uint32_t len) {
+float srslte_vec_corr_ccc(const cf_t *x, cf_t *y, const uint32_t len) {
 //  return crealf(srslte_vec_dot_prod_conj_ccc(x,y,len)) / len;
   float s_x = crealf(srslte_vec_dot_prod_conj_ccc(x, x, len))/len;
   float s_y = crealf(srslte_vec_dot_prod_conj_ccc(y, y, len))/len;
@@ -346,25 +345,25 @@ float srslte_vec_corr_ccc(cf_t *x, cf_t *y, uint32_t len) {
 }
 
 // PSS (disabled and using abs_square )
-void srslte_vec_abs_cf(cf_t *x, float *abs, uint32_t len) {
+void srslte_vec_abs_cf(const cf_t *x, float *abs, const uint32_t len) {
   srslte_vec_abs_cf_simd(x, abs, len);
 }
 
 // PRACH 
-void srslte_vec_abs_square_cf(cf_t *x, float *abs_square, uint32_t len) {
+void srslte_vec_abs_square_cf(const cf_t *x, float *abs_square, const uint32_t len) {
   srslte_vec_abs_square_cf_simd(x,abs_square,len);
 }
 
-uint32_t srslte_vec_max_fi(float *x, uint32_t len) {
+uint32_t srslte_vec_max_fi(const float *x, const uint32_t len) {
   return srslte_vec_max_fi_simd(x, len);
 }
 
 // CP autocorr
-uint32_t srslte_vec_max_abs_ci(cf_t *x, uint32_t len) {
+uint32_t srslte_vec_max_abs_ci(const cf_t *x, const uint32_t len) {
   return srslte_vec_max_ci_simd(x, len);
 }
 
-void srslte_vec_quant_fuc(float *in, uint8_t *out, float gain, float offset, float clip, uint32_t len) {
+void srslte_vec_quant_fuc(const float *in, uint8_t *out, const float gain, const float offset, const float clip, const uint32_t len) {
   int i;
   int tmp;
   
@@ -378,7 +377,7 @@ void srslte_vec_quant_fuc(float *in, uint8_t *out, float gain, float offset, flo
   }
 }
 
-void srslte_vec_quant_suc(int16_t *in, uint8_t *out, float gain, int16_t offset, int16_t clip, uint32_t len) {
+void srslte_vec_quant_suc(const int16_t *in, uint8_t *out, const float gain, const int16_t offset, const int16_t clip, const uint32_t len) {
   int i;
   int16_t tmp;
   
@@ -392,6 +391,6 @@ void srslte_vec_quant_suc(int16_t *in, uint8_t *out, float gain, int16_t offset,
   }
 }
 
-void srs_vec_cf_cpy(cf_t *dst, cf_t *src, int len) {
+void srs_vec_cf_cpy(const cf_t *dst, cf_t *src, int len) {
   srslte_vec_cp_simd(dst, src, len);
 }
diff --git a/lib/src/phy/utils/vector_simd.c b/lib/src/phy/utils/vector_simd.c
index 23c58cfce..78286a81a 100644
--- a/lib/src/phy/utils/vector_simd.c
+++ b/lib/src/phy/utils/vector_simd.c
@@ -37,7 +37,36 @@
 #include "srslte/phy/utils/simd.h"
 
 
-int srslte_vec_dot_prod_sss_simd(int16_t *x, int16_t *y, int len) {
+void srslte_vec_xor_bbb_simd(const int8_t *x, const int8_t *y, int8_t *z, const int len) {
+  int i = 0;
+#if SRSLTE_SIMD_B_SIZE
+  if (SRSLTE_IS_ALIGNED(x) && SRSLTE_IS_ALIGNED(y) && SRSLTE_IS_ALIGNED(z)) {
+    for (; i < len - SRSLTE_SIMD_B_SIZE + 1; i += SRSLTE_SIMD_B_SIZE) {
+      simd_b_t a = srslte_simd_b_load(&x[i]);
+      simd_b_t b = srslte_simd_b_load(&y[i]);
+
+      simd_b_t r = srslte_simd_b_xor(a, b);
+
+      srslte_simd_b_store(&z[i], r);
+    }
+  } else {
+    for (; i < len - SRSLTE_SIMD_B_SIZE + 1; i += SRSLTE_SIMD_B_SIZE) {
+      simd_b_t a = srslte_simd_b_loadu(&x[i]);
+      simd_b_t b = srslte_simd_b_loadu(&y[i]);
+
+      simd_s_t r = srslte_simd_b_xor(a, b);
+
+      srslte_simd_b_storeu(&z[i], r);
+    }
+  }
+#endif /* SRSLTE_SIMD_B_SIZE */
+  
+    for(; i < len; i++){
+    z[i] = x[i] ^ y[i];
+  }
+}
+
+int srslte_vec_dot_prod_sss_simd(const int16_t *x, const int16_t *y, const int len) {
   int i = 0;
   int result = 0;
 #if SRSLTE_SIMD_S_SIZE
@@ -75,7 +104,7 @@ int srslte_vec_dot_prod_sss_simd(int16_t *x, int16_t *y, int len) {
   return result; 
 }
 
-void srslte_vec_sum_sss_simd(int16_t *x, int16_t *y, int16_t *z, int len) {
+void srslte_vec_sum_sss_simd(const int16_t *x, const int16_t *y, int16_t *z, const int len) {
   int i = 0;
 #if SRSLTE_SIMD_S_SIZE
   if (SRSLTE_IS_ALIGNED(x) && SRSLTE_IS_ALIGNED(y) && SRSLTE_IS_ALIGNED(z)) {
@@ -104,7 +133,7 @@ void srslte_vec_sum_sss_simd(int16_t *x, int16_t *y, int16_t *z, int len) {
   }
 }
 
-void srslte_vec_sub_sss_simd(int16_t *x, int16_t *y, int16_t *z, int len) {
+void srslte_vec_sub_sss_simd(const int16_t *x, const int16_t *y, int16_t *z, const int len) {
   int i = 0;
 #if SRSLTE_SIMD_S_SIZE
   if (SRSLTE_IS_ALIGNED(x) && SRSLTE_IS_ALIGNED(y) && SRSLTE_IS_ALIGNED(z)) {
@@ -133,7 +162,7 @@ void srslte_vec_sub_sss_simd(int16_t *x, int16_t *y, int16_t *z, int len) {
   }
 }
 
-void srslte_vec_prod_sss_simd(int16_t *x, int16_t *y, int16_t *z, int len) {
+void srslte_vec_prod_sss_simd(const int16_t *x, const int16_t *y, int16_t *z, const int len) {
   int i = 0;
 #if SRSLTE_SIMD_S_SIZE
   if (SRSLTE_IS_ALIGNED(x) && SRSLTE_IS_ALIGNED(y) && SRSLTE_IS_ALIGNED(z)) {
@@ -163,7 +192,7 @@ void srslte_vec_prod_sss_simd(int16_t *x, int16_t *y, int16_t *z, int len) {
 }
 
 /* No improvement with AVX */
-void srslte_vec_lut_sss_simd(short *x, unsigned short *lut, short *y, int len) {
+void srslte_vec_lut_sss_simd(const short *x, const unsigned short *lut, short *y, const int len) {
   int i = 0;
 #ifdef LV_HAVE_SSE
 #if CMAKE_BUILD_TYPE!=Debug
@@ -199,7 +228,7 @@ void srslte_vec_lut_sss_simd(short *x, unsigned short *lut, short *y, int len) {
   }
 }
 
-void srslte_vec_convert_fi_simd(float *x, int16_t *z, float scale, int len) {
+void srslte_vec_convert_fi_simd(const float *x, int16_t *z, const float scale, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_F_SIZE && SRSLTE_SIMD_S_SIZE
@@ -236,7 +265,7 @@ void srslte_vec_convert_fi_simd(float *x, int16_t *z, float scale, int len) {
   }
 }
 
-float srslte_vec_acc_ff_simd(float *x, int len) {
+float srslte_vec_acc_ff_simd(const float *x, const int len) {
   int i = 0;
   float acc_sum = 0.0f;
 
@@ -271,7 +300,7 @@ float srslte_vec_acc_ff_simd(float *x, int len) {
   return acc_sum;
 }
 
-cf_t srslte_vec_acc_cc_simd(cf_t *x, int len) {
+cf_t srslte_vec_acc_cc_simd(const cf_t *x, const int len) {
   int i = 0;
   cf_t acc_sum = 0.0f;
 
@@ -305,7 +334,7 @@ cf_t srslte_vec_acc_cc_simd(cf_t *x, int len) {
   return acc_sum;
 }
 
-void srslte_vec_add_fff_simd(float *x, float *y, float *z, int len) {
+void srslte_vec_add_fff_simd(const float *x, const float *y, float *z, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_F_SIZE
@@ -335,7 +364,7 @@ void srslte_vec_add_fff_simd(float *x, float *y, float *z, int len) {
   }
 }
 
-void srslte_vec_sub_fff_simd(float *x, float *y, float *z, int len) {
+void srslte_vec_sub_fff_simd(const float *x, const float *y, float *z, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_F_SIZE
@@ -365,7 +394,7 @@ void srslte_vec_sub_fff_simd(float *x, float *y, float *z, int len) {
   }
 }
 
-cf_t srslte_vec_dot_prod_ccc_simd(cf_t *x, cf_t *y, int len) {
+cf_t srslte_vec_dot_prod_ccc_simd(const cf_t *x, const cf_t *y, const int len) {
   int i = 0;
   cf_t result = 0;
 
@@ -404,7 +433,7 @@ cf_t srslte_vec_dot_prod_ccc_simd(cf_t *x, cf_t *y, int len) {
   return result;
 }
 
-c16_t srslte_vec_dot_prod_ccc_c16i_simd(c16_t *x, c16_t *y, int len) {
+c16_t srslte_vec_dot_prod_ccc_c16i_simd(const c16_t *x, const c16_t *y, const int len) {
   int i = 0;
   c16_t result = 0;
 
@@ -432,7 +461,7 @@ c16_t srslte_vec_dot_prod_ccc_c16i_simd(c16_t *x, c16_t *y, int len) {
   return result;
 }
 
-cf_t srslte_vec_dot_prod_conj_ccc_simd(cf_t *x, cf_t *y, int len)
+cf_t srslte_vec_dot_prod_conj_ccc_simd(const cf_t *x, const cf_t *y, const int len)
 {
   int i = 0;
   cf_t result = 0;
@@ -470,7 +499,7 @@ cf_t srslte_vec_dot_prod_conj_ccc_simd(cf_t *x, cf_t *y, int len)
   return result;
 }
 
-void srslte_vec_prod_cfc_simd(cf_t *x, float *y, cf_t *z, int len) {
+void srslte_vec_prod_cfc_simd(const cf_t *x, const float *y, cf_t *z, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_CF_SIZE
@@ -498,7 +527,7 @@ void srslte_vec_prod_cfc_simd(cf_t *x, float *y, cf_t *z, int len) {
   }
 }
 
-void srslte_vec_prod_fff_simd(float *x, float *y, float *z, int len) {
+void srslte_vec_prod_fff_simd(const float *x, const float *y, float *z, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_F_SIZE
@@ -528,7 +557,7 @@ void srslte_vec_prod_fff_simd(float *x, float *y, float *z, int len) {
   }
 }
 
-void srslte_vec_prod_ccc_simd(cf_t *x,cf_t *y, cf_t *z, int len) {
+void srslte_vec_prod_ccc_simd(const cf_t *x, const cf_t *y, cf_t *z, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_CF_SIZE
@@ -558,7 +587,8 @@ void srslte_vec_prod_ccc_simd(cf_t *x,cf_t *y, cf_t *z, int len) {
   }
 }
 
-void srslte_vec_prod_ccc_split_simd(float *a_re, float *a_im, float *b_re, float *b_im, float *r_re, float *r_im, int len) {
+void srslte_vec_prod_ccc_split_simd(const float *a_re, const float *a_im, const float *b_re, const float *b_im,
+                                    float *r_re, float *r_im, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_F_SIZE
@@ -590,8 +620,8 @@ void srslte_vec_prod_ccc_split_simd(float *a_re, float *a_im, float *b_re, float
   }
 }
 
-void srslte_vec_prod_ccc_c16_simd(int16_t *a_re, int16_t *a_im, int16_t *b_re, int16_t *b_im, int16_t *r_re,
-                                  int16_t *r_im, int len) {
+void srslte_vec_prod_ccc_c16_simd(const int16_t *a_re, const int16_t *a_im, const int16_t *b_re, const int16_t *b_im,
+                                  int16_t *r_re, int16_t *r_im, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_C16_SIZE
@@ -623,7 +653,7 @@ void srslte_vec_prod_ccc_c16_simd(int16_t *a_re, int16_t *a_im, int16_t *b_re, i
   }
 }
 
-void srslte_vec_prod_conj_ccc_simd(cf_t *x,cf_t *y, cf_t *z, int len) {
+void srslte_vec_prod_conj_ccc_simd(const cf_t *x, const cf_t *y, cf_t *z, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_CF_SIZE
@@ -653,7 +683,7 @@ void srslte_vec_prod_conj_ccc_simd(cf_t *x,cf_t *y, cf_t *z, int len) {
   }
 }
 
-void srslte_vec_div_ccc_simd(cf_t *x,cf_t *y, cf_t *z, int len) {
+void srslte_vec_div_ccc_simd(const cf_t *x, const cf_t *y, cf_t *z, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_CF_SIZE
@@ -686,7 +716,7 @@ void srslte_vec_div_ccc_simd(cf_t *x,cf_t *y, cf_t *z, int len) {
 }
 
 
-void srslte_vec_div_cfc_simd(cf_t *x,float *y, cf_t *z, int len) {
+void srslte_vec_div_cfc_simd(const cf_t *x, const float *y, cf_t *z, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_CF_SIZE && SRSLTE_SIMD_CF_SIZE == SRSLTE_SIMD_F_SIZE
@@ -718,7 +748,7 @@ void srslte_vec_div_cfc_simd(cf_t *x,float *y, cf_t *z, int len) {
   }
 }
 
-void srslte_vec_div_fff_simd(float *x, float *y, float *z, int len) {
+void srslte_vec_div_fff_simd(const float *x, const float *y, float *z, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_F_SIZE
@@ -752,7 +782,7 @@ void srslte_vec_div_fff_simd(float *x, float *y, float *z, int len) {
 
 
 
-int  srslte_vec_sc_prod_ccc_simd2(cf_t *x, cf_t h, cf_t *z, int len)
+int  srslte_vec_sc_prod_ccc_simd2(const cf_t *x, const cf_t h, cf_t *z, const int len)
 {     
    int i = 0;
    const unsigned int loops = len / 4;
@@ -772,7 +802,7 @@ int  srslte_vec_sc_prod_ccc_simd2(cf_t *x, cf_t h, cf_t *z, int len)
 return i;
 }
 
-void srslte_vec_sc_prod_ccc_simd(cf_t *x, cf_t h, cf_t *z, int len) {
+void srslte_vec_sc_prod_ccc_simd(const cf_t *x, const cf_t h, cf_t *z, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_F_SIZE
@@ -815,7 +845,7 @@ void srslte_vec_sc_prod_ccc_simd(cf_t *x, cf_t h, cf_t *z, int len) {
   
 }
 
-void srslte_vec_sc_prod_fff_simd(float *x, float h, float *z, int len) {
+void srslte_vec_sc_prod_fff_simd(const float *x, const float h, float *z, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_F_SIZE
@@ -844,7 +874,7 @@ void srslte_vec_sc_prod_fff_simd(float *x, float h, float *z, int len) {
   }
 }
 
-void srslte_vec_abs_cf_simd(cf_t *x, float *z, int len) {
+void srslte_vec_abs_cf_simd(const cf_t *x, float *z, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_F_SIZE
@@ -881,7 +911,7 @@ void srslte_vec_abs_cf_simd(cf_t *x, float *z, int len) {
   }
 }
 
-void srslte_vec_abs_square_cf_simd(cf_t *x, float *z, int len) {
+void srslte_vec_abs_square_cf_simd(const cf_t *x, float *z, const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_F_SIZE
@@ -918,7 +948,7 @@ void srslte_vec_abs_square_cf_simd(cf_t *x, float *z, int len) {
 }
 
 
-void srslte_vec_sc_prod_cfc_simd(const cf_t *x, const float h, cf_t *z, const int len) {
+void srslte_vec_sc_prod_cfc_simd(const cf_t *x, const float h, cf_t *z, const const int len) {
   int i = 0;
 
 #if SRSLTE_SIMD_F_SIZE
@@ -948,7 +978,7 @@ void srslte_vec_sc_prod_cfc_simd(const cf_t *x, const float h, cf_t *z, const in
   }
 }
 
-void srslte_vec_cp_simd(cf_t *src, cf_t *dst, int len) {
+void srslte_vec_cp_simd(const cf_t *src, cf_t *dst, const int len) {
   uint32_t i = 0;
 
 #if SRSLTE_SIMD_F_SIZE
@@ -972,7 +1002,7 @@ void srslte_vec_cp_simd(cf_t *src, cf_t *dst, int len) {
   }
 }
 
-uint32_t srslte_vec_max_fi_simd(float *x, int len) {
+uint32_t srslte_vec_max_fi_simd(const float *x, const int len) {
   int i = 0;
 
   float max_value = -INFINITY;
@@ -1028,7 +1058,7 @@ uint32_t srslte_vec_max_fi_simd(float *x, int len) {
   return max_index;
 }
 
-uint32_t srslte_vec_max_ci_simd(cf_t *x, int len) {
+uint32_t srslte_vec_max_ci_simd(const cf_t *x, const int len) {
   int i = 0;
 
   float max_value = -INFINITY;
diff --git a/srsue/hdr/phy/phch_common.h b/srsue/hdr/phy/phch_common.h
index 0fe8529da..963f146dc 100644
--- a/srsue/hdr/phy/phch_common.h
+++ b/srsue/hdr/phy/phch_common.h
@@ -42,131 +42,138 @@
 
 namespace srsue {
 
+
+class chest_feedback_itf
+{
+public:
+  virtual void set_cfo(float cfo) = 0;
+};
+
 /* Subclass that manages variables common to all workers */
-  class phch_common {
-  public:
-    
-    /* Common variables used by all phy workers */
-    phy_interface_rrc::phy_cfg_t *config; 
-    phy_args_t                   *args;
-    rrc_interface_phy *rrc;
-    mac_interface_phy *mac;
-    srslte_ue_ul_t     ue_ul; 
-    
-    /* Power control variables */
-    float pathloss;
-    float cur_pathloss;
-    float p0_preamble;     
-    float cur_radio_power; 
-    float cur_pusch_power;
-    float avg_rsrp_db;
-    float avg_rsrq_db; 
-    float rx_gain_offset;
-    float avg_snr_db; 
-    float avg_noise; 
-    float avg_rsrp;
-
-    // Save last TBS for mcs>28 cases
-    int last_dl_tbs[2*HARQ_DELAY_MS][SRSLTE_MAX_CODEWORDS];
-    uint32_t last_dl_tti[2*HARQ_DELAY_MS];
-
-    int last_ul_tbs[2*HARQ_DELAY_MS];
-    uint32_t last_ul_tti[2*HARQ_DELAY_MS];
-    srslte_mod_t last_ul_mod[2*HARQ_DELAY_MS];
-
-    phch_common(uint32_t max_mutex = 3);
-    void init(phy_interface_rrc::phy_cfg_t *config, 
-              phy_args_t  *args, 
-              srslte::log *_log, 
-              srslte::radio *_radio,
-              rrc_interface_phy *rrc,
-              mac_interface_phy *_mac);
-    
-    /* For RNTI searches, -1 means now or forever */    
-    void               set_ul_rnti(srslte_rnti_type_t type, uint16_t rnti_value, int tti_start = -1, int tti_end = -1);
-    uint16_t           get_ul_rnti(uint32_t tti);
-    srslte_rnti_type_t get_ul_rnti_type();
-
-    void               set_dl_rnti(srslte_rnti_type_t type, uint16_t rnti_value, int tti_start = -1, int tti_end = -1);
-    uint16_t           get_dl_rnti(uint32_t tti);
-    srslte_rnti_type_t get_dl_rnti_type();
-    
-    void set_rar_grant(uint32_t tti, uint8_t grant_payload[SRSLTE_RAR_GRANT_LEN]);
-    bool get_pending_rar(uint32_t tti, srslte_dci_rar_grant_t *rar_grant = NULL);
-    
-    void reset_pending_ack(uint32_t tti);
-    void set_pending_ack(uint32_t tti, uint32_t I_lowest, uint32_t n_dmrs);   
-    bool get_pending_ack(uint32_t tti);    
-    bool get_pending_ack(uint32_t tti, uint32_t *I_lowest, uint32_t *n_dmrs);
-        
-    void worker_end(uint32_t tti, bool tx_enable, cf_t *buffer, uint32_t nof_samples, srslte_timestamp_t tx_time);
-    
-    void set_nof_mutex(uint32_t nof_mutex);
-    
-    bool sr_enabled; 
-    int  sr_last_tx_tti; 
-   
-    srslte::radio*    get_radio();
-
-    void set_cell(const srslte_cell_t &c);
-    uint32_t get_nof_prb();
-    void set_dl_metrics(const dl_metrics_t &m);
-    void get_dl_metrics(dl_metrics_t &m);
-    void set_ul_metrics(const ul_metrics_t &m);
-    void get_ul_metrics(ul_metrics_t &m);
-    void set_sync_metrics(const sync_metrics_t &m);
-    void get_sync_metrics(sync_metrics_t &m);
-
-    void reset_ul();
-    
-  private: 
-    
-    std::vector<pthread_mutex_t>    tx_mutex; 
-    
-    bool               is_first_of_burst;
-    srslte::radio      *radio_h;
-    float              cfo;
-    srslte::log       *log_h;
-
-    
-    bool               ul_rnti_active(uint32_t tti);
-    bool               dl_rnti_active(uint32_t tti);
-    uint16_t           ul_rnti, dl_rnti;  
-    srslte_rnti_type_t ul_rnti_type, dl_rnti_type; 
-    int                ul_rnti_start, ul_rnti_end, dl_rnti_start, dl_rnti_end; 
-    
-    float              time_adv_sec; 
-    
-    srslte_dci_rar_grant_t rar_grant; 
-    bool                   rar_grant_pending; 
-    uint32_t               rar_grant_tti; 
-    
-    typedef struct {
-      bool enabled; 
-      uint32_t I_lowest; 
-      uint32_t n_dmrs;
-    } pending_ack_t;
-    pending_ack_t pending_ack[TTIMOD_SZ];
-
-    bool            is_first_tx;
-
-    uint32_t        nof_workers;
-    uint32_t        nof_mutex;
-    uint32_t        max_mutex;
-
-    srslte_cell_t   cell;
-
-    dl_metrics_t    dl_metrics;
-    uint32_t        dl_metrics_count;
-    bool            dl_metrics_read;
-    ul_metrics_t    ul_metrics;
-    uint32_t        ul_metrics_count;
-    bool            ul_metrics_read;
-    sync_metrics_t  sync_metrics;
-    uint32_t        sync_metrics_count;
-    bool            sync_metrics_read;
-  };
-  
+class phch_common {
+public:
+
+  /* Common variables used by all phy workers */
+  phy_interface_rrc::phy_cfg_t *config;
+  phy_args_t                   *args;
+  rrc_interface_phy *rrc;
+  mac_interface_phy *mac;
+  srslte_ue_ul_t     ue_ul;
+
+  /* Power control variables */
+  float pathloss;
+  float cur_pathloss;
+  float p0_preamble;
+  float cur_radio_power;
+  float cur_pusch_power;
+  float avg_rsrp_db;
+  float avg_rsrq_db;
+  float rx_gain_offset;
+  float avg_snr_db;
+  float avg_noise;
+  float avg_rsrp;
+
+  // Save last TBS for mcs>28 cases
+  int last_dl_tbs[2*HARQ_DELAY_MS][SRSLTE_MAX_CODEWORDS];
+  uint32_t last_dl_tti[2*HARQ_DELAY_MS];
+
+  int last_ul_tbs[2*HARQ_DELAY_MS];
+  uint32_t last_ul_tti[2*HARQ_DELAY_MS];
+  srslte_mod_t last_ul_mod[2*HARQ_DELAY_MS];
+
+  phch_common(uint32_t max_mutex = 3);
+  void init(phy_interface_rrc::phy_cfg_t *config,
+            phy_args_t  *args,
+            srslte::log *_log,
+            srslte::radio *_radio,
+            rrc_interface_phy *rrc,
+            mac_interface_phy *_mac);
+
+  /* For RNTI searches, -1 means now or forever */
+  void               set_ul_rnti(srslte_rnti_type_t type, uint16_t rnti_value, int tti_start = -1, int tti_end = -1);
+  uint16_t           get_ul_rnti(uint32_t tti);
+  srslte_rnti_type_t get_ul_rnti_type();
+
+  void               set_dl_rnti(srslte_rnti_type_t type, uint16_t rnti_value, int tti_start = -1, int tti_end = -1);
+  uint16_t           get_dl_rnti(uint32_t tti);
+  srslte_rnti_type_t get_dl_rnti_type();
+
+  void set_rar_grant(uint32_t tti, uint8_t grant_payload[SRSLTE_RAR_GRANT_LEN]);
+  bool get_pending_rar(uint32_t tti, srslte_dci_rar_grant_t *rar_grant = NULL);
+
+  void reset_pending_ack(uint32_t tti);
+  void set_pending_ack(uint32_t tti, uint32_t I_lowest, uint32_t n_dmrs);
+  bool get_pending_ack(uint32_t tti);
+  bool get_pending_ack(uint32_t tti, uint32_t *I_lowest, uint32_t *n_dmrs);
+
+  void worker_end(uint32_t tti, bool tx_enable, cf_t *buffer, uint32_t nof_samples, srslte_timestamp_t tx_time);
+
+  void set_nof_mutex(uint32_t nof_mutex);
+
+  bool sr_enabled;
+  int  sr_last_tx_tti;
+
+  srslte::radio*    get_radio();
+
+  void set_cell(const srslte_cell_t &c);
+  uint32_t get_nof_prb();
+  void set_dl_metrics(const dl_metrics_t &m);
+  void get_dl_metrics(dl_metrics_t &m);
+  void set_ul_metrics(const ul_metrics_t &m);
+  void get_ul_metrics(ul_metrics_t &m);
+  void set_sync_metrics(const sync_metrics_t &m);
+  void get_sync_metrics(sync_metrics_t &m);
+
+  void reset_ul();
+
+private:
+
+  std::vector<pthread_mutex_t>    tx_mutex;
+
+  bool               is_first_of_burst;
+  srslte::radio      *radio_h;
+  float              cfo;
+  srslte::log       *log_h;
+
+
+  bool               ul_rnti_active(uint32_t tti);
+  bool               dl_rnti_active(uint32_t tti);
+  uint16_t           ul_rnti, dl_rnti;
+  srslte_rnti_type_t ul_rnti_type, dl_rnti_type;
+  int                ul_rnti_start, ul_rnti_end, dl_rnti_start, dl_rnti_end;
+
+  float              time_adv_sec;
+
+  srslte_dci_rar_grant_t rar_grant;
+  bool                   rar_grant_pending;
+  uint32_t               rar_grant_tti;
+
+  typedef struct {
+    bool enabled;
+    uint32_t I_lowest;
+    uint32_t n_dmrs;
+  } pending_ack_t;
+  pending_ack_t pending_ack[TTIMOD_SZ];
+
+  bool            is_first_tx;
+
+  uint32_t        nof_workers;
+  uint32_t        nof_mutex;
+  uint32_t        max_mutex;
+
+  srslte_cell_t   cell;
+
+  dl_metrics_t    dl_metrics;
+  uint32_t        dl_metrics_count;
+  bool            dl_metrics_read;
+  ul_metrics_t    ul_metrics;
+  uint32_t        ul_metrics_count;
+  bool            ul_metrics_read;
+  sync_metrics_t  sync_metrics;
+  uint32_t        sync_metrics_count;
+  bool            sync_metrics_read;
+};
+
 } // namespace srsue
 
 #endif // UEPHYWORKERCOMMON_H
diff --git a/srsue/hdr/phy/phch_recv.h b/srsue/hdr/phy/phch_recv.h
index 01a296094..a5f290887 100644
--- a/srsue/hdr/phy/phch_recv.h
+++ b/srsue/hdr/phy/phch_recv.h
@@ -41,7 +41,7 @@ namespace srsue {
     
 typedef _Complex float cf_t; 
 
-class phch_recv : public thread
+class phch_recv : public thread, public chest_feedback_itf
 {
 public:
   phch_recv();
@@ -65,6 +65,9 @@ public:
   
   bool    status_is_sync();
 
+  // from chest_feedback_itf
+  void    set_cfo(float cfo);
+
   void    set_time_adv_sec(float time_adv_sec);
   void    get_current_cell(srslte_cell_t *cell);
   
diff --git a/srsue/hdr/phy/phch_worker.h b/srsue/hdr/phy/phch_worker.h
index b54165229..9540b278d 100644
--- a/srsue/hdr/phy/phch_worker.h
+++ b/srsue/hdr/phy/phch_worker.h
@@ -45,7 +45,7 @@ public:
   ~phch_worker();
   void  reset(); 
   void  set_common(phch_common *phy);
-  bool  init(uint32_t max_prb, srslte::log *log);
+  bool  init(uint32_t max_prb, srslte::log *log, chest_feedback_itf *chest_loop);
 
   bool  set_cell(srslte_cell_t cell);
 
@@ -66,6 +66,8 @@ public:
   int read_ce_abs(float *ce_abs);
   int read_pdsch_d(cf_t *pdsch_d);
   void start_plot();
+
+  float get_ref_cfo();
   
 private: 
   /* Inherited from thread_pool::worker. Function called every subframe to run the DL/UL processing */
@@ -115,6 +117,7 @@ private:
   /* Common objects */  
   phch_common    *phy;
   srslte::log    *log_h;
+  chest_feedback_itf *chest_loop;
   srslte_cell_t  cell;
   bool           mem_initiated;
   bool           cell_initiated;
diff --git a/srsue/hdr/ue_base.h b/srsue/hdr/ue_base.h
index 13e2e4660..e23adf3d2 100644
--- a/srsue/hdr/ue_base.h
+++ b/srsue/hdr/ue_base.h
@@ -163,6 +163,10 @@ public:
   srslte::log_filter rf_log;
   rf_metrics_t     rf_metrics;
   srslte::LOG_LEVEL_ENUM level(std::string l);
+
+  std::string get_build_mode();
+  std::string get_build_info();
+  std::string get_build_string();
 };
 
 } // namespace srsue
diff --git a/srsue/hdr/upper/nas.h b/srsue/hdr/upper/nas.h
index e743165fb..2e795098e 100644
--- a/srsue/hdr/upper/nas.h
+++ b/srsue/hdr/upper/nas.h
@@ -57,6 +57,9 @@ static const char emm_state_text[EMM_STATE_N_ITEMS][100] = {"NULL",
                                                             "DEREGISTERED INITIATED",
                                                             "TRACKING AREA UPDATE INITIATED"};
 
+static const bool eia_caps[8] = {false, true,  true,  false, false, false, false, false};
+static const bool eea_caps[8] = {true,  false, false, false, false, false, false, false};
+
 typedef enum {
   PLMN_NOT_SELECTED = 0,
   PLMN_SELECTED
@@ -80,16 +83,12 @@ public:
 
   // RRC interface
   void notify_connection_setup();
-
   void write_pdu(uint32_t lcid, byte_buffer_t *pdu);
-
   uint32_t get_ul_count();
-
   bool is_attached();
   bool is_attaching();
-
   bool get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi);
-
+  bool get_k_asme(uint8_t *k_asme_, uint32_t n);
   void plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code);
   void plmn_search_end();
 
@@ -115,72 +114,100 @@ private:
 
   std::vector<LIBLTE_RRC_PLMN_IDENTITY_STRUCT > known_plmns;
 
-  // Save short MAC
+  LIBLTE_MME_EMM_INFORMATION_MSG_STRUCT emm_info;
 
-  // Identifiers
-  LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT guti;
-  bool is_guti_set;
+  // Security context
+  struct nas_sec_ctxt{
+    uint8_t  ksi;
+    uint8_t  k_asme[32];
+    uint32_t tx_count;
+    uint32_t rx_count;
+    srslte::CIPHERING_ALGORITHM_ID_ENUM  cipher_algo;
+    srslte::INTEGRITY_ALGORITHM_ID_ENUM  integ_algo;
+    LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT guti;
+  };
+
+  bool have_guti;
+  bool have_ctxt;
+  nas_sec_ctxt ctxt;
 
   uint32_t ip_addr;
   uint8_t eps_bearer_id;
 
   uint8_t transaction_id;
 
-  // NAS counters - incremented for each security-protected message recvd/sent
-  uint32_t count_ul;
-  uint32_t count_dl;
-
   // Security
-  uint8_t ksi;
   uint8_t k_nas_enc[32];
   uint8_t k_nas_int[32];
 
-  srslte::CIPHERING_ALGORITHM_ID_ENUM cipher_algo;
-  srslte::INTEGRITY_ALGORITHM_ID_ENUM integ_algo;
-
-  void integrity_generate(uint8_t *key_128,
+  void integrity_generate(uint8_t integ_algo,
+                          uint8_t *key_128,
                           uint32_t count,
                           uint8_t rb_id,
                           uint8_t direction,
                           uint8_t *msg,
                           uint32_t msg_len,
                           uint8_t *mac);
-
   void integrity_check();
-
   void cipher_encrypt();
-
   void cipher_decrypt();
+  bool check_cap_replay(LIBLTE_MME_UE_SECURITY_CAPABILITIES_STRUCT *caps);
 
   // Parsers
   void parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu);
-
   void parse_attach_reject(uint32_t lcid, byte_buffer_t *pdu);
-
   void parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu);
-
   void parse_authentication_reject(uint32_t lcid, byte_buffer_t *pdu);
-
   void parse_identity_request(uint32_t lcid, byte_buffer_t *pdu);
-
   void parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu);
-
   void parse_service_reject(uint32_t lcid, byte_buffer_t *pdu);
-
   void parse_esm_information_request(uint32_t lcid, byte_buffer_t *pdu);
-
   void parse_emm_information(uint32_t lcid, byte_buffer_t *pdu);
 
   // Senders
   void send_attach_request();
-
   void send_identity_response();
-
   void send_service_request();
-
   void send_esm_information_response();
-
   void gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg);
+  void send_security_mode_reject(uint8_t cause);
+
+  // security context persistence file
+  bool read_ctxt_file(nas_sec_ctxt *ctxt);
+  bool write_ctxt_file(nas_sec_ctxt ctxt);
+
+  // ctxt file helpers
+  std::string hex_to_string(uint8_t *hex, int size);
+  bool        string_to_hex(std::string hex_str, uint8_t *hex, uint32_t len);
+  std::string emm_info_str(LIBLTE_MME_EMM_INFORMATION_MSG_STRUCT *info);
+
+  template <class T>
+  bool readvar(std::istream &file, const char *key, T *var)
+  {
+    std::string line;
+    size_t len = strlen(key);
+    std::getline(file, line);
+    if(line.substr(0,len).compare(key)) {
+      return false;
+    }
+    *var = (T)atoi(line.substr(len).c_str());
+    return true;
+  }
+
+  bool readvar(std::istream &file, const char *key, uint8_t *var, int varlen)
+  {
+    std::string line;
+    size_t len = strlen(key);
+    std::getline(file, line);
+    if(line.substr(0,len).compare(key)) {
+      return false;
+    }
+    std::string tmp = line.substr(len);
+    if(!string_to_hex(tmp, var, varlen)) {
+      return false;
+    }
+    return true;
+  }
 };
 
 } // namespace srsue
diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index 37ab941cd..d561b0f1c 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -40,8 +40,6 @@
 #include <map>
 
 typedef struct {
-  bool                          stmsi_attach;
-  LIBLTE_RRC_S_TMSI_STRUCT      stmsi_value;
   uint32_t                      ue_category;
   uint32_t                      feature_group;
   uint8_t                       supported_bands[LIBLTE_RRC_BAND_N_ITEMS];
@@ -198,21 +196,16 @@ private:
 
   // MAC interface
   void release_pucch_srs();
-
   void ra_problem();
 
   // GW interface
   bool is_connected();
-
   bool have_drb();
 
   // PDCP interface
   void write_pdu(uint32_t lcid, byte_buffer_t *pdu);
-
   void write_pdu_bcch_bch(byte_buffer_t *pdu);
-
   void write_pdu_bcch_dlsch(byte_buffer_t *pdu);
-
   void write_pdu_pcch(byte_buffer_t *pdu);
 
   // Radio bearers
@@ -280,11 +273,6 @@ private:
   void          set_mac_default();
   void          set_rrc_default(); 
   void          set_bearers();
-
-  // s-tmsi persistent file
-  bool read_stimsi_file(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi);
-  bool write_stimsi_file(LIBLTE_RRC_S_TMSI_STRUCT s_tmsi);
-
 };
 
 } // namespace srsue
diff --git a/srsue/hdr/upper/usim.h b/srsue/hdr/upper/usim.h
index fea15ba68..73acd5594 100644
--- a/srsue/hdr/upper/usim.h
+++ b/srsue/hdr/upper/usim.h
@@ -1,4 +1,4 @@
-/**
+/**
  *
  * \section COPYRIGHT
  *
@@ -59,24 +59,30 @@ public:
   void stop();
 
   // NAS interface
-  void get_imsi_vec(uint8_t* imsi_, uint32_t n);
-  void get_imei_vec(uint8_t* imei_, uint32_t n);
-  int  get_home_plmn_id(LIBLTE_RRC_PLMN_IDENTITY_STRUCT *home_plmn_id);
+  std::string get_imsi_str();
+  std::string get_imei_str();
+
+  bool get_imsi_vec(uint8_t* imsi_, uint32_t n);
+  bool get_imei_vec(uint8_t* imei_, uint32_t n);
+  bool get_home_plmn_id(LIBLTE_RRC_PLMN_IDENTITY_STRUCT *home_plmn_id);
 
   void generate_authentication_response(uint8_t  *rand,
                                         uint8_t  *autn_enb,
                                         uint16_t  mcc,
                                         uint16_t  mnc,
                                         bool     *net_valid,
-                                        uint8_t  *res);
+                                        uint8_t  *res,
+                                        uint8_t  *k_asme);
 
-  void generate_nas_keys(uint8_t *k_nas_enc,
+  void generate_nas_keys(uint8_t *k_asme,
+                         uint8_t *k_nas_enc,
                          uint8_t *k_nas_int,
                          srslte::CIPHERING_ALGORITHM_ID_ENUM cipher_algo,
                          srslte::INTEGRITY_ALGORITHM_ID_ENUM integ_algo);
 
   // RRC interface
-  void generate_as_keys(uint32_t count_ul,
+  void generate_as_keys(uint8_t *k_asme,
+                        uint32_t count_ul,
                         uint8_t *k_rrc_enc,
                         uint8_t *k_rrc_int,
                         uint8_t *k_up_enc,
@@ -91,13 +97,15 @@ private:
                               uint16_t  mcc,
                               uint16_t  mnc,
                               bool     *net_valid,
-                              uint8_t  *res);
+                              uint8_t  *res,
+                              uint8_t  *k_asme);
   void gen_auth_res_xor(      uint8_t  *rand,
                               uint8_t  *autn_enb,
                               uint16_t  mcc,
                               uint16_t  mnc,
                               bool     *net_valid,
-                              uint8_t  *res);
+                              uint8_t  *res,
+                              uint8_t  *k_asme);
   void str_to_hex(std::string str, uint8_t *hex);
 
   srslte::log *usim_log;
@@ -110,6 +118,9 @@ private:
   uint64_t    imei;
   uint8_t     k[16];
 
+  std::string imsi_str;
+  std::string imei_str;
+
   // Security variables
   uint8_t     rand[16];
   uint8_t     ck[16];
@@ -117,7 +128,6 @@ private:
   uint8_t     ak[6];
   uint8_t     mac[8];
   uint8_t     autn[16];
-  uint8_t     k_asme[32];
   uint8_t     k_enb[32];
 
   bool initiated;
diff --git a/srsue/src/main.cc b/srsue/src/main.cc
index f1edaaf75..201af1b14 100644
--- a/srsue/src/main.cc
+++ b/srsue/src/main.cc
@@ -77,17 +77,12 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
      "Transmission time advance")
     ("rf.burst_preamble_us", bpo::value<string>(&args->rf.burst_preamble)->default_value("auto"), "Transmission time advance")
 
-    ("rrc.stmsi_attach", bpo::value<bool>(&args->rrc.stmsi_attach)->default_value(false),"If enabled, always tries first an S-TMSI attach using the\n"
-                                                                                           "S-TMSI value stored in .stimsi file generated in previous run")
-    ("rrc.mmec_value",    bpo::value<uint8_t>(&args->rrc.stmsi_value.mmec)->default_value(0),    "If defined (non-zero), overwrites the value stored in .stimsi file")
-    ("rrc.mtmsi_value",  bpo::value<uint32_t>(&args->rrc.stmsi_value.m_tmsi)->default_value(0), "If defined (non-zero), overwrites the value stored in .stimsi file")
-
     ("rrc.feature_group", bpo::value<uint32_t>(&args->rrc.feature_group)->default_value(0xe6041c00), "Hex value of the featureGroupIndicators field in the"
                                                                                            "UECapabilityInformation message. Default 0xe6041c00")
     ("rrc.ue_category",   bpo::value<string>(&args->ue_category_str)->default_value("4"),  "UE Category (1 to 5)")
 
 
-       ("pcap.enable", bpo::value<bool>(&args->pcap.enable)->default_value(false),
+    ("pcap.enable", bpo::value<bool>(&args->pcap.enable)->default_value(false),
      "Enable MAC packet captures for wireshark")
     ("pcap.filename", bpo::value<string>(&args->pcap.filename)->default_value("ue.pcap"), "MAC layer capture filename")
 
@@ -204,13 +199,46 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
      "Enables integer CFO estimation and correction.")
 
     ("expert.cfo_correct_tol_hz",
-     bpo::value<float>(&args->expert.phy.cfo_correct_tol_hz)->default_value(50.0),
-     "Tolerance (in Hz) for digial CFO compensation.")
+     bpo::value<float>(&args->expert.phy.cfo_correct_tol_hz)->default_value(0.0),
+     "Tolerance (in Hz) for digital CFO compensation (needs to be low if average_subframe_enabled=true.")
+
+    ("expert.cfo_pss_ema",
+     bpo::value<float>(&args->expert.phy.cfo_pss_ema)->default_value(0.1),
+     "CFO Exponential Moving Average coefficient for PSS estimation during TRACK.")
+
+    ("expert.cfo_ref_ema",
+     bpo::value<float>(&args->expert.phy.cfo_ref_ema)->default_value(0.01),
+     "CFO Exponential Moving Average coefficient for RS estimation after PSS acquisition")
+
+    ("expert.cfo_ref_mask",
+     bpo::value<uint32_t>(&args->expert.phy.cfo_ref_mask)->default_value(1023),
+     "Bitmask for subframes on which to run RS estimation (set to 0 to disable, default all sf)")
+
+    ("expert.cfo_loop_bw_pss",
+     bpo::value<float>(&args->expert.phy.cfo_loop_bw_pss)->default_value(0.05),
+     "CFO feedback loop bandwidth for samples from PSS")
+
+    ("expert.cfo_loop_bw_ref",
+     bpo::value<float>(&args->expert.phy.cfo_loop_bw_ref)->default_value(0.01),
+     "CFO feedback loop bandwidth for samples from RS")
+
+    ("expert.cfo_loop_pss_tol",
+     bpo::value<float>(&args->expert.phy.cfo_loop_pss_tol)->default_value(300),
+     "Tolerance (in Hz) of the PSS estimation method. Below this value, PSS estimation does not feeds back the loop"
+       "and RS estimations are used instead (when available)")
+
+    ("expert.cfo_loop_ref_min",
+     bpo::value<float>(&args->expert.phy.cfo_loop_ref_min)->default_value(0),
+     "Tolerance (in Hz) of the RS estimation method. Below this value, RS estimation does not feeds back the loop")
+
+
+    ("expert.cfo_loop_pss_conv",
+     bpo::value<uint32_t>(&args->expert.phy.cfo_loop_pss_conv)->default_value(50),
+     "After the PSS estimation is below cfo_loop_pss_tol for cfo_loop_pss_timeout times consecutively, RS adjustments are allowed.")
 
-    ("expert.cfo_ema",
-     bpo::value<float>(&args->expert.phy.cfo_ema)->default_value(0.4),
-     "CFO Exponential Moving Average coefficient. Lower makes it more robust to noise "
-     "but vulnerable to periodic interruptions due to VCO corrections.")
+    ("expert.average_subframe_enabled",
+     bpo::value<bool>(&args->expert.phy.average_subframe_enabled)->default_value(true),
+     "Averages in the time domain the channel estimates within 1 subframe. Needs accurate CFO correction.")
 
     ("expert.time_correct_period",
      bpo::value<int>(&args->expert.phy.time_correct_period)->default_value(5),
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index 8a69a7f76..652a848bd 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -57,8 +57,9 @@ int radio_recv_wrapper_cs(void *obj, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsam
 }
 
 double callback_set_rx_gain(void *h, double gain) {
-  srslte::radio_multi *radio_handler = (srslte::radio_multi *) h;
-  return radio_handler->set_rx_gain_th(gain);
+  phch_recv *obj = (phch_recv*) h;
+  srslte::radio_multi *radio_h = obj->radio_h;
+  return radio_h->set_rx_gain_th(gain);
 }
 
 
@@ -97,9 +98,7 @@ void phch_recv::  init(srslte::radio_multi *_radio_handler, mac_interface_phy *_
 
   srslte_ue_cellsearch_set_nof_valid_frames(&cs, 2);
 
-  // Set options defined in expert section
-  set_ue_sync_opts(&cs.ue_sync);
-
+  last_gain = 40;
   if (do_agc) {
     srslte_ue_sync_start_agc(&cs.ue_sync, callback_set_rx_gain, last_gain);
   }
@@ -182,6 +181,11 @@ void phch_recv::radio_error() {
   radio_is_resetting=false;
 }
 
+void phch_recv::set_cfo(float cfo) {
+  Debug("set_ref_cfo=%f\n",cfo*15000);
+  srslte_ue_sync_set_cfo_ref(&ue_sync, cfo);
+}
+
 bool phch_recv::wait_radio_reset() {
   int cnt=0;
   while(cnt < 20 && radio_is_resetting) {
@@ -206,11 +210,16 @@ void phch_recv::set_time_adv_sec(float _time_adv_sec) {
 
 void phch_recv::set_ue_sync_opts(srslte_ue_sync_t *q) {
   if (worker_com->args->cfo_integer_enabled) {
-    srslte_ue_sync_cfo_i_detec_en(q, true);
+    srslte_ue_sync_set_cfo_i_enable(q, true);
   }
 
-  srslte_ue_sync_set_cfo_ema(q, worker_com->args->cfo_ema);
+  srslte_ue_sync_set_cfo_ema(q, worker_com->args->cfo_pss_ema);
   srslte_ue_sync_set_cfo_tol(q, worker_com->args->cfo_correct_tol_hz);
+  srslte_ue_sync_set_cfo_loop_bw(q, worker_com->args->cfo_loop_bw_pss, worker_com->args->cfo_loop_bw_ref,
+                                 worker_com->args->cfo_loop_pss_tol,
+                                 worker_com->args->cfo_loop_ref_min,
+                                 worker_com->args->cfo_loop_pss_tol,
+                                 worker_com->args->cfo_loop_pss_conv);
 
   int time_correct_period = worker_com->args->time_correct_period;
   if (time_correct_period > 0) {
@@ -318,15 +327,13 @@ int phch_recv::cell_search(int force_N_id_2) {
     return false;
   }
 
-  // Set options defined in expert section
-  set_ue_sync_opts(&ue_mib_sync.ue_sync);
-
   if (do_agc) {
     srslte_ue_sync_start_agc(&ue_mib_sync.ue_sync, callback_set_rx_gain, last_gain);
   }
 
   srslte_ue_sync_reset(&ue_mib_sync.ue_sync);
-  srslte_ue_sync_set_cfo(&ue_mib_sync.ue_sync, cellsearch_cfo);
+  srslte_ue_sync_copy_cfo(&ue_mib_sync.ue_sync, &cs.ue_sync);
+  Info("Copied cfo=%f Hz from cs_sync to ue_mib\n", srslte_ue_sync_get_cfo(&ue_mib_sync.ue_sync));
 
   /* Find and decode MIB */
   int sfn_offset;
@@ -338,7 +345,8 @@ int phch_recv::cell_search(int force_N_id_2) {
   cellsearch_cfo = srslte_ue_sync_get_cfo(&ue_mib_sync.ue_sync);
 
   srslte_ue_sync_reset(&ue_sync);
-  srslte_ue_sync_set_cfo(&ue_sync, cellsearch_cfo);
+  srslte_ue_sync_copy_cfo(&ue_sync, &ue_mib_sync.ue_sync);
+  Info("Copied cfo=%f Hz from ue_mib_sync to ue_sync\n", srslte_ue_sync_get_cfo(&ue_sync));
 
   if (ret == 1) {
     srslte_pbch_mib_unpack(bch_payload, &cell, NULL);
@@ -553,8 +561,6 @@ bool phch_recv::cell_select(uint32_t earfcn, srslte_cell_t cell) {
 
     current_earfcn = earfcn;
 
-    printf("cell select called set frequency\n");
-
     if (set_frequency()) {
       this->cell = cell;
       log_h->info("Cell Select: Configuring cell...\n");
@@ -670,7 +676,7 @@ void phch_recv::run_thread() {
 
         switch (cell_sync_sfn()) {
           case 1:
-            srslte_ue_sync_set_agc_period(&ue_sync, 20);
+            srslte_ue_sync_set_agc_period(&ue_sync, 4);
             if (!cell_search_in_progress) {
               phy_state = CELL_CAMP;
               log_h->info("Sync OK. Camping on cell PCI=%d...\n", cell.id);
@@ -731,6 +737,9 @@ void phch_recv::run_thread() {
               worker->set_cfo(ul_dl_factor * metrics.cfo / 15000);
               worker_com->set_sync_metrics(metrics);
 
+              Debug("current_cfo=%f, pss_stable_cnt=%d, cfo_pss=%f Hz\n",
+                      metrics.cfo, ue_sync.pss_stable_cnt, srslte_sync_get_cfo(&ue_sync.strack)*15000);
+
               worker->set_sample_offset(srslte_ue_sync_get_sfo(&ue_sync)/1000);
 
               /* Compute TX time: Any transmission happens in TTI+4 thus advance 4 ms the reception time */
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 2e6dadebd..fd6e959db 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -57,7 +57,9 @@ namespace srsue {
 
 phch_worker::phch_worker() : tr_exec(10240)
 {
-  phy = NULL; 
+  phy = NULL;
+  chest_loop = NULL;
+
   bzero(signal_buffer, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
 
   mem_initiated   = false;
@@ -104,9 +106,11 @@ void phch_worker::set_common(phch_common* phy_)
   phy = phy_;   
 }
 
-bool phch_worker::init(uint32_t max_prb, srslte::log *log_h)
+bool phch_worker::init(uint32_t max_prb, srslte::log *log_h, chest_feedback_itf *chest_loop)
 {
   this->log_h = log_h;
+  this->chest_loop = chest_loop;
+
   // ue_sync in phy.cc requires a buffer for 3 subframes
   for (uint32_t i=0;i<phy->args->nof_rx_ant;i++) {
     signal_buffer[i] = (cf_t*) srslte_vec_malloc(3 * sizeof(cf_t) * SRSLTE_SF_LEN_PRB(max_prb));
@@ -126,6 +130,8 @@ bool phch_worker::init(uint32_t max_prb, srslte::log *log_h)
     return false;
   }
 
+  srslte_chest_dl_average_subframe(&ue_dl.chest, phy->args->average_subframe_enabled);
+  srslte_chest_dl_cfo_estimate_enable(&ue_dl.chest, phy->args->cfo_ref_mask!=0, phy->args->cfo_ref_mask, phy->args->cfo_ref_ema);
   srslte_ue_ul_set_normalization(&ue_ul, true);
   srslte_ue_ul_set_cfo_enable(&ue_ul, true);
 
@@ -134,6 +140,10 @@ bool phch_worker::init(uint32_t max_prb, srslte::log *log_h)
   return true;
 }
 
+float phch_worker::get_ref_cfo() {
+  return srslte_chest_dl_get_cfo(&ue_dl.chest);
+}
+
 bool phch_worker::set_cell(srslte_cell_t cell_)
 {
   if (cell.id != cell_.id || !cell_initiated) {
@@ -221,6 +231,11 @@ void phch_worker::work_imp()
 
   bool snr_th_ok = 10*log10(srslte_chest_dl_get_snr(&ue_dl.chest))>1.0;
 
+  // Call feedback loop for chest
+  if (chest_loop && ((1<<(tti%10)) & phy->args->cfo_ref_mask)) {
+    chest_loop->set_cfo(srslte_chest_dl_get_cfo(&ue_dl.chest));
+  }
+
   if (chest_ok && snr_th_ok) {
 
     /***** Downlink Processing *******/
@@ -397,11 +412,8 @@ void phch_worker::compute_ri() {
 }
 
 bool phch_worker::extract_fft_and_pdcch_llr() {
-  bool decode_pdcch = false; 
-  if (phy->get_ul_rnti(tti) || phy->get_dl_rnti(tti) || phy->get_pending_rar(tti)) {
-    decode_pdcch = true; 
-  } 
-  
+  bool decode_pdcch = true;
+
   /* Without a grant, we might need to do fft processing if need to decode PHICH */
   if (phy->get_pending_ack(tti) || decode_pdcch) {
     
@@ -1276,13 +1288,18 @@ int phch_worker::read_ce_abs(float *ce_abs) {
   int sz = srslte_symbol_sz(cell.nof_prb);
   bzero(ce_abs, sizeof(float)*sz);
   int g = (sz - 12*cell.nof_prb)/2;
-  for (i = 0; i < 12*cell.nof_prb; i++) {
+/*  for (i = 0; i < 12*cell.nof_prb; i++) {
     ce_abs[g+i] = 20 * log10f(cabsf(ue_dl.ce_m[0][0][i]));
     if (isinf(ce_abs[g+i])) {
       ce_abs[g+i] = -80;
     }
   }
-  return sz;
+*/
+  uint32_t nrefs = 2*ue_dl.cell.nof_prb;
+  for (i=0;i<nrefs;i++) {
+    ce_abs[i] = 15000*0.463208685*cargf(ue_dl.chest.tmp_cfo_estimate[i])/M_PI;
+  }
+  return nrefs;
 }
 
 int phch_worker::read_pdsch_d(cf_t* pdsch_d)
@@ -1361,7 +1378,7 @@ void phch_worker::update_measurements()
     }
     
     // Compute SNR
-    phy->avg_snr_db = 10*log10(phy->avg_rsrp/phy->avg_noise);      
+    phy->avg_snr_db = 10*log10(phy->avg_rsrp/phy->avg_noise);
     
     // Store metrics
     dl_metrics.n      = phy->avg_noise;
@@ -1434,7 +1451,7 @@ void *plot_thread_run(void *arg) {
   plot_real_init(&pce);
   plot_real_setTitle(&pce, (char*) "Channel Response - Magnitude");
   plot_real_setLabels(&pce, (char*) "Index", (char*) "dB");
-  plot_real_setYAxisScale(&pce, -40, 40);
+  plot_real_setYAxisScale(&pce, -1000, 1000);
   
   plot_scatter_init(&pconst);
   plot_scatter_setTitle(&pconst, (char*) "PDSCH - Equalized Symbols");
diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc
index 6dfb2c6d7..0daecf0de 100644
--- a/srsue/src/phy/phy.cc
+++ b/srsue/src/phy/phy.cc
@@ -37,7 +37,6 @@
 #include "srslte/common/threads.h"
 #include "srslte/common/log.h"
 #include "phy/phy.h"
-#include "phy/phch_worker.h"
 
 #define Error(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
 #define Warning(fmt, ...) if (SRSLTE_DEBUG_ENABLED) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
@@ -134,7 +133,7 @@ void phy::run_thread() {
   // Add workers to workers pool and start threads
   for (uint32_t i=0;i<nof_workers;i++) {
     workers[i].set_common(&workers_common);
-    workers[i].init(SRSLTE_MAX_PRB, (srslte::log*) log_vec[i]);
+    workers[i].init(SRSLTE_MAX_PRB, (srslte::log*) log_vec[i], &sf_recv);
     workers_pool.init_worker(i, &workers[i], WORKERS_THREAD_PRIO, args->worker_cpu_mask);
   }
 
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index ca56db556..b7e744bd8 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -32,7 +32,6 @@
 #include <string>
 #include <algorithm>
 #include <iterator>
-#include <ue_base.h>
 
 using namespace srslte;
 
@@ -58,6 +57,7 @@ bool ue::init(all_args_t *args_)
   } else {
     logger_file.init(args->log.filename);
     logger_file.log("\n\n");
+    logger_file.log(get_build_string().c_str());
     logger = &logger_file;
   }
 
@@ -117,7 +117,11 @@ bool ue::init(all_args_t *args_)
   
   // Init layers
 
-  // PHY initis in background, start before radio
+  if (args->rf.rx_gain < 0) {
+    phy.set_agc_enable(true);
+  }
+
+    // PHY initis in background, start before radio
   args->expert.phy.nof_rx_ant = args->rf.nof_rx_ant;
   phy.init(&radio, &mac, &rrc, phy_log, &args->expert.phy);
 
@@ -160,8 +164,6 @@ bool ue::init(all_args_t *args_)
 
   if (args->rf.rx_gain < 0) {
     radio.start_agc(false);    
-    radio.set_tx_rx_gain_offset(10);
-    phy.set_agc_enable(true);
   } else {
     radio.set_rx_gain(args->rf.rx_gain);
   }
diff --git a/srsue/src/ue_base.cc b/srsue/src/ue_base.cc
index 94b9b5155..4b2c372ae 100644
--- a/srsue/src/ue_base.cc
+++ b/srsue/src/ue_base.cc
@@ -24,13 +24,14 @@
  *
  */
 
-
 #include "ue_base.h"
 #include "ue.h"
 #include "srslte/srslte.h"
+#include "srslte/build_info.h"
 #include <pthread.h>
 #include <iostream>
 #include <string>
+#include <sstream>
 #include <algorithm>
 #include <iterator>
 
@@ -58,6 +59,9 @@ ue_base* ue_base::get_instance(srsue_instance_type_t type)
 }
 
 ue_base::ue_base() {
+  // print build info
+  std::cout << std::endl << get_build_string() << std::endl;
+
   // load FFTW wisdom
   srslte_dft_load();
 }
@@ -116,4 +120,21 @@ srslte::LOG_LEVEL_ENUM ue_base::level(std::string l)
   }
 }
 
+std::string ue_base::get_build_mode()
+{
+  return std::string(srslte_get_build_mode());
+}
+
+std::string ue_base::get_build_info()
+{
+  return std::string(srslte_get_build_info());
+}
+
+std::string ue_base::get_build_string()
+{
+  std::stringstream ss;
+  ss << "Built in " << get_build_mode() << " mode using " << get_build_info() << "." << std::endl;
+  return ss.str();
+}
+
 } // namespace srsue
diff --git a/srsue/src/upper/nas.cc b/srsue/src/upper/nas.cc
index d8bd40c6e..801cab581 100644
--- a/srsue/src/upper/nas.cc
+++ b/srsue/src/upper/nas.cc
@@ -25,6 +25,11 @@
  */
 
 
+#include <unistd.h>
+#include <iostream>
+#include <iomanip>
+#include <fstream>
+#include <sstream>
 #include "srslte/asn1/liblte_rrc.h"
 #include "upper/nas.h"
 #include "srslte/common/bcd_helpers.h"
@@ -33,9 +38,17 @@ using namespace srslte;
 
 namespace srsue {
 
+
+/*********************************************************************
+ *   NAS
+ ********************************************************************/
+
 nas::nas()
-  : state(EMM_STATE_DEREGISTERED), plmn_selection(PLMN_SELECTED), is_guti_set(false), ip_addr(0), eps_bearer_id(0),
-    count_ul(0), count_dl(0) {}
+  : state(EMM_STATE_DEREGISTERED), plmn_selection(PLMN_SELECTED), have_guti(false), have_ctxt(false), ip_addr(0), eps_bearer_id(0)
+{
+  ctxt.rx_count = 0;
+  ctxt.tx_count = 0;
+}
 
 void nas::init(usim_interface_nas *usim_,
                rrc_interface_nas *rrc_,
@@ -51,23 +64,34 @@ void nas::init(usim_interface_nas *usim_,
   state = EMM_STATE_DEREGISTERED;
   plmn_selection = PLMN_NOT_SELECTED;
 
-  if (usim->get_home_plmn_id(&home_plmn)) {
+  if (!usim->get_home_plmn_id(&home_plmn)) {
     nas_log->error("Getting Home PLMN Id from USIM. Defaulting to 001-01\n");
     home_plmn.mcc = 61441; // This is 001
     home_plmn.mnc = 65281; // This is 01
   }
   cfg     = cfg_;
+
+  if((read_ctxt_file(&ctxt))) {
+    usim->generate_nas_keys(ctxt.k_asme, k_nas_enc, k_nas_int,
+                            ctxt.cipher_algo, ctxt.integ_algo);
+    nas_log->debug_hex(k_nas_enc, 32, "NAS encryption key - k_nas_enc");
+    nas_log->debug_hex(k_nas_int, 32, "NAS integrity key - k_nas_int");
+    have_guti = true;
+    have_ctxt = true;
+  }
 }
 
-void nas::stop() {}
+void nas::stop() {
+  write_ctxt_file(ctxt);
+}
 
 emm_state_t nas::get_state() {
   return state;
 }
 
 /*******************************************************************************
-UE interface
-*******************************************************************************/
+ * UE interface
+ ******************************************************************************/
 
 void nas::attach_request() {
   nas_log->info("Attach Request\n");
@@ -96,8 +120,8 @@ void nas::deattach_request() {
 }
 
 /*******************************************************************************
-RRC interface
-*******************************************************************************/
+ * RRC interface
+ ******************************************************************************/
 
 void nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) {
 
@@ -207,24 +231,39 @@ void nas::write_pdu(uint32_t lcid, byte_buffer_t *pdu) {
 }
 
 uint32_t nas::get_ul_count() {
-  return count_ul;
+  return ctxt.tx_count;
 }
 
 bool nas::get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi) {
-  if (is_guti_set) {
-    s_tmsi->mmec   = guti.mme_code;
-    s_tmsi->m_tmsi = guti.m_tmsi;
+  if (have_guti) {
+    s_tmsi->mmec   = ctxt.guti.mme_code;
+    s_tmsi->m_tmsi = ctxt.guti.m_tmsi;
     return true;
   } else {
     return false;
   }
 }
 
+bool nas::get_k_asme(uint8_t *k_asme_, uint32_t n) {
+  if(!have_ctxt) {
+    nas_log->error("K_asme requested before security context established\n");
+    return false;
+  }
+  if(NULL == k_asme_ || n < 32) {
+    nas_log->error("Invalid parameters to get_k_asme");
+    return false;
+  }
+
+  memcpy(k_asme_, ctxt.k_asme, 32);
+  return true;
+}
+
 /*******************************************************************************
-Security
-*******************************************************************************/
+ * Security
+ ******************************************************************************/
 
-void nas::integrity_generate(uint8_t *key_128,
+void nas::integrity_generate(uint8_t integ_algo,
+                             uint8_t *key_128,
                              uint32_t count,
                              uint8_t rb_id,
                              uint8_t direction,
@@ -269,10 +308,20 @@ void nas::cipher_decrypt() {
 
 }
 
+bool nas::check_cap_replay(LIBLTE_MME_UE_SECURITY_CAPABILITIES_STRUCT *caps)
+{
+  for(uint32_t i=0; i<8; i++) {
+    if(caps->eea[i] != eea_caps[i] || caps->eia[i] != eia_caps[i]) {
+      return false;
+    }
+  }
+  return true;
+}
+
 
 /*******************************************************************************
-Parsers
-*******************************************************************************/
+ * Parsers
+ ******************************************************************************/
 
 void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu) {
   LIBLTE_MME_ATTACH_ACCEPT_MSG_STRUCT attach_accept;
@@ -288,21 +337,19 @@ void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu) {
     //FIXME: Handle t3412.unit
     //FIXME: Handle tai_list
     if (attach_accept.guti_present) {
-      memcpy(&guti, &attach_accept.guti.guti, sizeof(LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT));
-      is_guti_set = true;
-      // TODO: log message to console
-    }
-    if (attach_accept.lai_present) {
+      memcpy(&ctxt.guti, &attach_accept.guti.guti, sizeof(LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT));
+      have_guti = true;
     }
+    if (attach_accept.lai_present) {}
     if (attach_accept.ms_id_present) {}
     if (attach_accept.emm_cause_present) {}
     if (attach_accept.t3402_present) {}
+    if (attach_accept.t3412_ext_present) {}
     if (attach_accept.t3423_present) {}
     if (attach_accept.equivalent_plmns_present) {}
     if (attach_accept.emerg_num_list_present) {}
     if (attach_accept.eps_network_feature_support_present) {}
     if (attach_accept.additional_update_result_present) {}
-    if (attach_accept.t3412_ext_present) {}
 
     liblte_mme_unpack_activate_default_eps_bearer_context_request_msg(&attach_accept.esm_msg,
                                                                       &act_def_eps_bearer_context_req);
@@ -313,13 +360,14 @@ void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu) {
       ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[2] << 8;
       ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[3];
 
-      nas_log->info("IP allocated by network %u.%u.%u.%u\n",
+      nas_log->info("Network attach successful. APN: %s, IP: %u.%u.%u.%u\n",
+                    act_def_eps_bearer_context_req.apn.apn.c_str(),
                     act_def_eps_bearer_context_req.pdn_addr.addr[0],
                     act_def_eps_bearer_context_req.pdn_addr.addr[1],
                     act_def_eps_bearer_context_req.pdn_addr.addr[2],
                     act_def_eps_bearer_context_req.pdn_addr.addr[3]);
 
-      nas_log->console("Network attach successful.   IP: %u.%u.%u.%u\n",
+      nas_log->console("Network attach successful. IP: %u.%u.%u.%u\n",
                        act_def_eps_bearer_context_req.pdn_addr.addr[0],
                        act_def_eps_bearer_context_req.pdn_addr.addr[1],
                        act_def_eps_bearer_context_req.pdn_addr.addr[2],
@@ -358,10 +406,9 @@ void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu) {
     state = EMM_STATE_REGISTERED;
     current_plmn = selecting_plmn;
 
-    count_dl++;
+    ctxt.rx_count++;
 
     // Send EPS bearer context accept and attach complete
-    count_ul++;
     act_def_eps_bearer_context_accept.eps_bearer_id = eps_bearer_id;
     act_def_eps_bearer_context_accept.proc_transaction_id = transaction_id;
     act_def_eps_bearer_context_accept.protocol_cnfg_opts_present = false;
@@ -369,10 +416,11 @@ void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu) {
                                                                    &attach_complete.esm_msg);
     liblte_mme_pack_attach_complete_msg(&attach_complete,
                                         LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY_AND_CIPHERED,
-                                        count_ul,
+                                        ctxt.tx_count,
                                         (LIBLTE_BYTE_MSG_STRUCT *) pdu);
-    integrity_generate(&k_nas_int[16],
-                       count_ul,
+    integrity_generate(ctxt.integ_algo,
+                       &k_nas_int[16],
+                       ctxt.tx_count,
                        lcid - 1,
                        SECURITY_DIRECTION_UPLINK,
                        &pdu->msg[5],
@@ -384,6 +432,7 @@ void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu) {
 
     nas_log->info("Sending Attach Complete\n");
     rrc->write_sdu(lcid, pdu);
+    ctxt.tx_count++;
 
   } else {
     nas_log->info("Not handling attach type %u\n", attach_accept.eps_attach_result);
@@ -407,7 +456,7 @@ void nas::parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu) {
   LIBLTE_MME_AUTHENTICATION_REQUEST_MSG_STRUCT auth_req;
   LIBLTE_MME_AUTHENTICATION_RESPONSE_MSG_STRUCT auth_res;
 
-  nas_log->info("Received Authentication Request\n");;
+  nas_log->info("Received Authentication Request\n");
   liblte_mme_unpack_authentication_request_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &auth_req);
 
   // Reuse the pdu for the response message
@@ -422,7 +471,15 @@ void nas::parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu) {
 
   bool net_valid;
   uint8_t res[16];
-  usim->generate_authentication_response(auth_req.rand, auth_req.autn, mcc, mnc, &net_valid, res);
+  usim->generate_authentication_response(auth_req.rand, auth_req.autn, mcc, mnc,
+                                         &net_valid, res, ctxt.k_asme);
+  nas_log->info("Generated k_asme=%s\n", hex_to_string(ctxt.k_asme, 32).c_str());
+  if(LIBLTE_MME_TYPE_OF_SECURITY_CONTEXT_FLAG_NATIVE == auth_req.nas_ksi.tsc_flag) {
+    ctxt.ksi = auth_req.nas_ksi.nas_ksi;
+  } else {
+    nas_log->error("NAS mapped security context not currently supported\n");
+    nas_log->console("Warning: NAS mapped security context not currently supported\n");
+  }
 
   if (net_valid) {
     nas_log->info("Network authentication successful\n");
@@ -438,9 +495,6 @@ void nas::parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu) {
     nas_log->console("Warning: Network authentication failure\n");
     pool->deallocate(pdu);
   }
-
-  // Reset DL counter (as per 24.301 5.4.3.2)
-  count_dl = 0;
 }
 
 void nas::parse_authentication_reject(uint32_t lcid, byte_buffer_t *pdu) {
@@ -451,108 +505,156 @@ void nas::parse_authentication_reject(uint32_t lcid, byte_buffer_t *pdu) {
 }
 
 void nas::parse_identity_request(uint32_t lcid, byte_buffer_t *pdu) {
-  nas_log->error("TODO:parse_identity_request\n");
+  LIBLTE_MME_ID_REQUEST_MSG_STRUCT  id_req;
+  LIBLTE_MME_ID_RESPONSE_MSG_STRUCT id_resp;
+
+  liblte_mme_unpack_identity_request_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &id_req);
+  nas_log->info("Received Identity Request. ID type: %d\n", id_req.id_type);
+
+  switch(id_req.id_type) {
+  case LIBLTE_MME_MOBILE_ID_TYPE_IMSI:
+    id_resp.mobile_id.type_of_id = LIBLTE_MME_MOBILE_ID_TYPE_IMSI;
+    usim->get_imsi_vec(id_resp.mobile_id.imsi, 15);
+    break;
+  case LIBLTE_MME_MOBILE_ID_TYPE_IMEI:
+    id_resp.mobile_id.type_of_id = LIBLTE_MME_MOBILE_ID_TYPE_IMEI;
+    usim->get_imei_vec(id_resp.mobile_id.imei, 15);
+    break;
+  default:
+    nas_log->error("Unhandled ID type: %d\n");
+    pool->deallocate(pdu);
+    return;
+  }
+
+  pdu->reset();
+  liblte_mme_pack_identity_response_msg(&id_resp, (LIBLTE_BYTE_MSG_STRUCT *) pdu);
+  rrc->write_sdu(lcid, pdu);
 }
 
-void nas::parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu) {
-  bool success;
+void nas::parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu)
+{
   LIBLTE_MME_SECURITY_MODE_COMMAND_MSG_STRUCT sec_mode_cmd;
   LIBLTE_MME_SECURITY_MODE_COMPLETE_MSG_STRUCT sec_mode_comp;
-  LIBLTE_MME_SECURITY_MODE_REJECT_MSG_STRUCT sec_mode_rej;
 
-  nas_log->info("Received Security Mode Command\n");
   liblte_mme_unpack_security_mode_command_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &sec_mode_cmd);
+  nas_log->info("Received Security Mode Command ksi: %d, eea: %s, eia: %s\n",
+                sec_mode_cmd.nas_ksi.nas_ksi,
+                ciphering_algorithm_id_text[sec_mode_cmd.selected_nas_sec_algs.type_of_eea],
+                integrity_algorithm_id_text[sec_mode_cmd.selected_nas_sec_algs.type_of_eia]);
+
+  if(sec_mode_cmd.nas_ksi.tsc_flag != LIBLTE_MME_TYPE_OF_SECURITY_CONTEXT_FLAG_NATIVE) {
+    nas_log->error("Mapped security context not supported\n");
+    pool->deallocate(pdu);
+    return;
+  }
 
-  ksi = sec_mode_cmd.nas_ksi.nas_ksi;
-  cipher_algo = (CIPHERING_ALGORITHM_ID_ENUM) sec_mode_cmd.selected_nas_sec_algs.type_of_eea;
-  integ_algo = (INTEGRITY_ALGORITHM_ID_ENUM) sec_mode_cmd.selected_nas_sec_algs.type_of_eia;
-  // FIXME: Handle nonce_ue, nonce_mme
-  // FIXME: Currently only handling ciphering EEA0 (null) and integrity EIA1,EIA2
-  // FIXME: Use selected_nas_sec_algs to choose correct algos
+  if (have_ctxt) {
+    if(sec_mode_cmd.nas_ksi.nas_ksi != ctxt.ksi) {
+      nas_log->warning("Sending Security Mode Reject due to key set ID mismatch\n");
+      send_security_mode_reject(LIBLTE_MME_EMM_CAUSE_SECURITY_MODE_REJECTED_UNSPECIFIED);
+      pool->deallocate(pdu);
+      return;
+    }
+  }
 
-  nas_log->debug("Security details: ksi=%d, eea=%s, eia=%s\n",
-                 ksi, ciphering_algorithm_id_text[cipher_algo], integrity_algorithm_id_text[integ_algo]);
+  // MME is setting up security context
 
+  // TODO: check nonce (not sent by Amari)
 
-  if (CIPHERING_ALGORITHM_ID_EEA0 != cipher_algo ||
-      (INTEGRITY_ALGORITHM_ID_128_EIA2 != integ_algo &&
-       INTEGRITY_ALGORITHM_ID_128_EIA1 != integ_algo) ||
-      sec_mode_cmd.nas_ksi.tsc_flag != LIBLTE_MME_TYPE_OF_SECURITY_CONTEXT_FLAG_NATIVE) {
-    sec_mode_rej.emm_cause = LIBLTE_MME_EMM_CAUSE_UE_SECURITY_CAPABILITIES_MISMATCH;
+  // Check capabilities replay
+  if(!check_cap_replay(&sec_mode_cmd.ue_security_cap)) {
     nas_log->warning("Sending Security Mode Reject due to security capabilities mismatch\n");
-    nas_log->console("Unsupported Security Mode Command settings: use ciphering algorithm EEA0 and integrity check EIA1 or EIA2.\n");
-    success = false;
-  } else {
-    // Generate NAS encryption key and integrity protection key
-    usim->generate_nas_keys(k_nas_enc, k_nas_int, cipher_algo, integ_algo);
-    nas_log->debug_hex(k_nas_enc, 32, "NAS encryption key - k_nas_enc");
-    nas_log->debug_hex(k_nas_int, 32, "NAS integrity key - k_nas_int");
-
-    // Check incoming MAC
-    uint8_t *inMAC = &pdu->msg[1];
-    uint8_t genMAC[4];
-    integrity_generate(&k_nas_int[16],
-                       count_dl,
-                       lcid - 1,
-                       SECURITY_DIRECTION_DOWNLINK,
-                       &pdu->msg[5],
-                       pdu->N_bytes - 5,
-                       genMAC);
+    send_security_mode_reject(LIBLTE_MME_EMM_CAUSE_UE_SECURITY_CAPABILITIES_MISMATCH);
+    pool->deallocate(pdu);
+    return;
+  }
 
-    nas_log->info_hex(inMAC, 4, "Incoming PDU MAC:");
-    nas_log->info_hex(genMAC, 4, "Generated PDU MAC:");
+  // Reset counterd (as per 24.301 5.4.3.2)
+  ctxt.rx_count = 0;
+  ctxt.tx_count = 0;
 
-    bool match = true;
-    for (int i = 0; i < 4; i++) {
-      if (inMAC[i] != genMAC[i]) {
-        match = false;
-      }
-    }
-    if (!match) {
-      sec_mode_rej.emm_cause = LIBLTE_MME_EMM_CAUSE_SECURITY_MODE_REJECTED_UNSPECIFIED;
-      nas_log->warning("Sending Security Mode Reject due to integrity check failure\n");
-      success = false;
-    } else {
+  ctxt.cipher_algo = (CIPHERING_ALGORITHM_ID_ENUM) sec_mode_cmd.selected_nas_sec_algs.type_of_eea;
+  ctxt.integ_algo  = (INTEGRITY_ALGORITHM_ID_ENUM) sec_mode_cmd.selected_nas_sec_algs.type_of_eia;
 
-      if (sec_mode_cmd.imeisv_req_present && LIBLTE_MME_IMEISV_REQUESTED == sec_mode_cmd.imeisv_req) {
-        sec_mode_comp.imeisv_present = true;
-        sec_mode_comp.imeisv.type_of_id = LIBLTE_MME_MOBILE_ID_TYPE_IMEISV;
-        usim->get_imei_vec(sec_mode_comp.imeisv.imeisv, 15);
-        sec_mode_comp.imeisv.imeisv[14] = 5;
-        sec_mode_comp.imeisv.imeisv[15] = 3;
-      } else {
-        sec_mode_comp.imeisv_present = false;
-      }
+  // Check capabilities
+  if(!eea_caps[ctxt.cipher_algo] || !eia_caps[ctxt.integ_algo]) {
+    nas_log->warning("Sending Security Mode Reject due to security capabilities mismatch\n");
+    send_security_mode_reject(LIBLTE_MME_EMM_CAUSE_UE_SECURITY_CAPABILITIES_MISMATCH);
+    pool->deallocate(pdu);
+    return;
+  }
 
-      // Reuse pdu for response
-      pdu->reset();
-      liblte_mme_pack_security_mode_complete_msg(&sec_mode_comp,
-                                                 LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY_AND_CIPHERED,
-                                                 count_ul,
-                                                 (LIBLTE_BYTE_MSG_STRUCT *) pdu);
-      integrity_generate(&k_nas_int[16],
-                         count_ul,
-                         lcid - 1,
-                         SECURITY_DIRECTION_UPLINK,
-                         &pdu->msg[5],
-                         pdu->N_bytes - 5,
-                         &pdu->msg[1]);
-      nas_log->info("Sending Security Mode Complete nas_count_ul=%d, RB=%s\n",
-                    count_ul,
-                    rrc->get_rb_name(lcid).c_str());
-      success = true;
+  // Generate NAS keys
+  usim->generate_nas_keys(ctxt.k_asme, k_nas_enc, k_nas_int, ctxt.cipher_algo, ctxt.integ_algo);
+  nas_log->debug_hex(k_nas_enc, 32, "NAS encryption key - k_nas_enc");
+  nas_log->debug_hex(k_nas_int, 32, "NAS integrity key - k_nas_int");
+
+  nas_log->debug("Generating integrity check. integ_algo:%d, count_dl:%d, lcid:%d\n",
+                 ctxt.integ_algo, ctxt.rx_count, lcid);
+
+  // Check incoming MAC
+  uint8_t *inMAC = &pdu->msg[1];
+  uint8_t genMAC[4];
+  integrity_generate(ctxt.integ_algo,
+                     &k_nas_int[16],
+                     ctxt.rx_count,
+                     lcid - 1,
+                     SECURITY_DIRECTION_DOWNLINK,
+                     &pdu->msg[5],
+                     pdu->N_bytes - 5,
+                     genMAC);
+
+  nas_log->info_hex(inMAC, 4, "Incoming PDU MAC:");
+  nas_log->info_hex(genMAC, 4, "Generated PDU MAC:");
+
+  ctxt.rx_count++;
+
+  bool match = true;
+  for (int i = 0; i < 4; i++) {
+    if (inMAC[i] != genMAC[i]) {
+      match = false;
     }
   }
+  if(!match) {
+    nas_log->warning("Sending Security Mode Reject due to integrity check failure\n");
+    send_security_mode_reject(LIBLTE_MME_EMM_CAUSE_SECURITY_MODE_REJECTED_UNSPECIFIED);
+    pool->deallocate(pdu);
+    return;
+  }
 
-  count_dl++;
+  // Take security context into use
+  have_ctxt = true;
 
-  if (!success) {
-    // Reuse pdu for response
-    pdu->reset();
-    liblte_mme_pack_security_mode_reject_msg(&sec_mode_rej, (LIBLTE_BYTE_MSG_STRUCT *) pdu);
+  if (sec_mode_cmd.imeisv_req_present && LIBLTE_MME_IMEISV_REQUESTED == sec_mode_cmd.imeisv_req) {
+    sec_mode_comp.imeisv_present = true;
+    sec_mode_comp.imeisv.type_of_id = LIBLTE_MME_MOBILE_ID_TYPE_IMEISV;
+    usim->get_imei_vec(sec_mode_comp.imeisv.imeisv, 15);
+    sec_mode_comp.imeisv.imeisv[14] = 5;
+    sec_mode_comp.imeisv.imeisv[15] = 3;
+  } else {
+    sec_mode_comp.imeisv_present = false;
   }
 
-  rrc->write_sdu(lcid, pdu);
+  // Send response
+  byte_buffer_t *sdu = pool_allocate;
+  liblte_mme_pack_security_mode_complete_msg(&sec_mode_comp,
+                                             LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY_AND_CIPHERED,
+                                             ctxt.tx_count,
+                                             (LIBLTE_BYTE_MSG_STRUCT *) sdu);
+  integrity_generate(ctxt.integ_algo,
+                     &k_nas_int[16],
+                     ctxt.tx_count,
+                     lcid - 1,
+                     SECURITY_DIRECTION_UPLINK,
+                     &sdu->msg[5],
+                     sdu->N_bytes - 5,
+                     &sdu->msg[1]);
+  nas_log->info("Sending Security Mode Complete nas_current_ctxt.tx_count=%d, RB=%s\n",
+                ctxt.tx_count,
+                rrc->get_rb_name(lcid).c_str());
+  rrc->write_sdu(lcid, sdu);
+  ctxt.tx_count++;
+  pool->deallocate(pdu);
 }
 
 void nas::parse_service_reject(uint32_t lcid, byte_buffer_t *pdu) {
@@ -561,15 +663,20 @@ void nas::parse_service_reject(uint32_t lcid, byte_buffer_t *pdu) {
 
 void nas::parse_esm_information_request(uint32_t lcid, byte_buffer_t *pdu) {
   nas_log->error("TODO:parse_esm_information_request\n");
+
 }
 
 void nas::parse_emm_information(uint32_t lcid, byte_buffer_t *pdu) {
-  nas_log->error("TODO:parse_emm_information\n");
+  liblte_mme_unpack_emm_information_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &emm_info);
+  std::string str = emm_info_str(&emm_info);
+  nas_log->info("Received EMM Information: %s\n", str.c_str());
+  nas_log->console("%s\n", str.c_str());
+  ctxt.rx_count++;
 }
 
 /*******************************************************************************
-Senders
-*******************************************************************************/
+ * Senders
+ ******************************************************************************/
 
 void nas::send_attach_request() {
   LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT attach_req;
@@ -579,30 +686,17 @@ void nas::send_attach_request() {
   attach_req.eps_attach_type = LIBLTE_MME_EPS_ATTACH_TYPE_EPS_ATTACH;
 
   for (i = 0; i < 8; i++) {
-    attach_req.ue_network_cap.eea[i] = false;
-    attach_req.ue_network_cap.eia[i] = false;
+    attach_req.ue_network_cap.eea[i] = eea_caps[i];
+    attach_req.ue_network_cap.eia[i] = eia_caps[i];
   }
-  attach_req.ue_network_cap.eea[0] = true; // EEA0 supported
-  attach_req.ue_network_cap.eia[0] = true; // EIA0 supported
-  attach_req.ue_network_cap.eia[1] = true; // EIA1 supported
-  attach_req.ue_network_cap.eia[2] = true; // EIA2 supported
-
-  attach_req.ue_network_cap.uea_present = false; // UMTS encryption algos
-  attach_req.ue_network_cap.uia_present = false; // UMTS integrity algos
-
-  attach_req.ms_network_cap_present = false; // A/Gb mode (2G) or Iu mode (3G)
-
-  attach_req.eps_mobile_id.type_of_id = LIBLTE_MME_EPS_MOBILE_ID_TYPE_IMSI;
-  usim->get_imsi_vec(attach_req.eps_mobile_id.imsi, 15);
-
-  // ESM message (PDN connectivity request) for first default bearer
-  gen_pdn_connectivity_request(&attach_req.esm_msg);
 
+  attach_req.ue_network_cap.uea_present = false;  // UMTS encryption algos
+  attach_req.ue_network_cap.uia_present = false;  // UMTS integrity algos
+  attach_req.ms_network_cap_present = false;      // A/Gb mode (2G) or Iu mode (3G)
   attach_req.old_p_tmsi_signature_present = false;
   attach_req.additional_guti_present = false;
   attach_req.last_visited_registered_tai_present = false;
   attach_req.drx_param_present = false;
-  attach_req.ms_network_cap_present = false;
   attach_req.old_lai_present = false;
   attach_req.tmsi_status_present = false;
   attach_req.ms_cm2_present = false;
@@ -613,11 +707,47 @@ void nas::send_attach_request() {
   attach_req.device_properties_present = false;
   attach_req.old_guti_type_present = false;
 
-  // Pack the message
-  liblte_mme_pack_attach_request_msg(&attach_req, (LIBLTE_BYTE_MSG_STRUCT *) msg);
+  // ESM message (PDN connectivity request) for first default bearer
+  gen_pdn_connectivity_request(&attach_req.esm_msg);
+
+  // GUTI or IMSI attach
+  if(have_guti && have_ctxt) {
+    attach_req.eps_mobile_id.type_of_id = LIBLTE_MME_EPS_MOBILE_ID_TYPE_GUTI;
+    memcpy(&attach_req.eps_mobile_id.guti, &ctxt.guti, sizeof(LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT));
+    attach_req.old_guti_type         = LIBLTE_MME_GUTI_TYPE_NATIVE;
+    attach_req.old_guti_type_present = true;
+    attach_req.nas_ksi.tsc_flag      = LIBLTE_MME_TYPE_OF_SECURITY_CONTEXT_FLAG_NATIVE;
+    attach_req.nas_ksi.nas_ksi       = ctxt.ksi;
+    nas_log->info("Requesting GUTI attach. "
+                  "m_tmsi: %x, mcc: %x, mnc: %x, mme_group_id: %x, mme_code: %x\n",
+                  ctxt.guti.m_tmsi, ctxt.guti.mcc, ctxt.guti.mnc, ctxt.guti.mme_group_id, ctxt.guti.mme_code);
+    liblte_mme_pack_attach_request_msg(&attach_req,
+                                       LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY,
+                                       ctxt.tx_count,
+                                       (LIBLTE_BYTE_MSG_STRUCT *) msg);
+
+    // Add MAC
+    integrity_generate(ctxt.integ_algo,
+                       &k_nas_int[16],
+                       ctxt.tx_count,
+                       cfg.lcid-1,
+                       SECURITY_DIRECTION_UPLINK,
+                       &msg->msg[5],
+                       msg->N_bytes - 5,
+                       &msg->msg[1]);
+  } else {
+    attach_req.eps_mobile_id.type_of_id = LIBLTE_MME_EPS_MOBILE_ID_TYPE_IMSI;
+    usim->get_imsi_vec(attach_req.eps_mobile_id.imsi, 15);
+    nas_log->info("Requesting IMSI attach (IMSI=%s)\n", usim->get_imsi_str().c_str());
+    liblte_mme_pack_attach_request_msg(&attach_req, (LIBLTE_BYTE_MSG_STRUCT *) msg);
+  }
 
   nas_log->info("Sending attach request\n");
   rrc->write_sdu(cfg.lcid, msg);
+
+  if (have_ctxt) {
+    ctxt.tx_count++;
+  }
 }
 
 void nas::gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg) {
@@ -641,27 +771,36 @@ void nas::gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg) {
   liblte_mme_pack_pdn_connectivity_request_msg(&pdn_con_req, msg);
 }
 
+void nas::send_security_mode_reject(uint8_t cause) {
+  byte_buffer_t *msg = pool_allocate;
+
+  LIBLTE_MME_SECURITY_MODE_REJECT_MSG_STRUCT sec_mode_rej;
+  sec_mode_rej.emm_cause = cause;
+  liblte_mme_pack_security_mode_reject_msg(&sec_mode_rej, (LIBLTE_BYTE_MSG_STRUCT *) msg);
+  rrc->write_sdu(cfg.lcid, msg);
+}
+
 void nas::send_identity_response() {}
 
 void nas::send_service_request() {
   byte_buffer_t *msg = pool_allocate;
-  count_ul++;
 
   // Pack the service request message directly
   msg->msg[0] = (LIBLTE_MME_SECURITY_HDR_TYPE_SERVICE_REQUEST << 4) | (LIBLTE_MME_PD_EPS_MOBILITY_MANAGEMENT);
   msg->N_bytes++;
-  msg->msg[1] = (ksi & 0x07) << 5;
-  msg->msg[1] |= count_ul & 0x1F;
+  msg->msg[1] = (ctxt.ksi & 0x07) << 5;
+  msg->msg[1] |= ctxt.tx_count & 0x1F;
   msg->N_bytes++;
 
   uint8_t mac[4];
-  integrity_generate(&k_nas_int[16],
-                      count_ul,
-                      cfg.lcid-1,
-                      SECURITY_DIRECTION_UPLINK,
-                      &msg->msg[0],
-                      2,
-                      &mac[0]);
+  integrity_generate(ctxt.integ_algo,
+                     &k_nas_int[16],
+                     ctxt.tx_count,
+                     cfg.lcid-1,
+                     SECURITY_DIRECTION_UPLINK,
+                     &msg->msg[0],
+                     2,
+                     &mac[0]);
   // Set the short MAC
   msg->msg[2] = mac[2];
   msg->N_bytes++;
@@ -669,8 +808,167 @@ void nas::send_service_request() {
   msg->N_bytes++;
   nas_log->info("Sending service request\n");
   rrc->write_sdu(cfg.lcid, msg);
+  ctxt.tx_count++;
 }
 
 void nas::send_esm_information_response() {}
 
+
+/*******************************************************************************
+ * Security context persistence file
+ ******************************************************************************/
+
+bool nas::read_ctxt_file(nas_sec_ctxt *ctxt)
+{
+  std::ifstream file;
+  if(!ctxt) {
+    return false;
+  }
+
+  file.open(".ctxt", std::ios::in);
+  if(file.is_open()) {
+    if(!readvar(file, "m_tmsi=",        &ctxt->guti.m_tmsi))        {return false;}
+    if(!readvar(file, "mcc=",           &ctxt->guti.mcc))           {return false;}
+    if(!readvar(file, "mnc=",           &ctxt->guti.mnc))           {return false;}
+    if(!readvar(file, "mme_group_id=",  &ctxt->guti.mme_group_id))  {return false;}
+    if(!readvar(file, "mme_code=",      &ctxt->guti.mme_code))      {return false;}
+    if(!readvar(file, "tx_count=",      &ctxt->tx_count))           {return false;}
+    if(!readvar(file, "rx_count=",      &ctxt->rx_count))           {return false;}
+    if(!readvar(file, "int_alg=",       &ctxt->integ_algo))         {return false;}
+    if(!readvar(file, "enc_alg=",       &ctxt->cipher_algo))        {return false;}
+    if(!readvar(file, "ksi=",           &ctxt->ksi))                {return false;}
+
+    if(!readvar(file, "k_asme=",        ctxt->k_asme, 32))          {return false;}
+
+    file.close();
+    have_guti = true;
+    nas_log->info("Read GUTI from file "
+                  "m_tmsi: %x, mcc: %x, mnc: %x, mme_group_id: %x, mme_code: %x\n",
+                  ctxt->guti.m_tmsi,
+                  ctxt->guti.mcc,
+                  ctxt->guti.mnc,
+                  ctxt->guti.mme_group_id,
+                  ctxt->guti.mme_code);
+    have_ctxt = true;
+    nas_log->info("Read security ctxt from file .ctxt. "
+                  "ksi: %x, k_asme: %s, tx_count: %x, rx_count: %x, int_alg: %d, enc_alg: %d\n",
+                  ctxt->ksi,
+                  hex_to_string(ctxt->k_asme,32).c_str(),
+                  ctxt->tx_count,
+                  ctxt->rx_count,
+                  ctxt->integ_algo,
+                  ctxt->cipher_algo);
+    return true;
+
+  } else {
+    return false;
+  }
+}
+
+bool nas::write_ctxt_file(nas_sec_ctxt ctxt)
+{
+  if (!have_guti || !have_ctxt) {
+    return false;
+  }
+  std::ofstream file;
+  file.open(".ctxt", std::ios::out | std::ios::trunc);
+  if (file.is_open()) {
+    file << "m_tmsi="       << (int) ctxt.guti.m_tmsi         << std::endl;
+    file << "mcc="          << (int) ctxt.guti.mcc            << std::endl;
+    file << "mnc="          << (int) ctxt.guti.mnc            << std::endl;
+    file << "mme_group_id=" << (int) ctxt.guti.mme_group_id   << std::endl;
+    file << "mme_code="     << (int) ctxt.guti.mme_code       << std::endl;
+    file << "tx_count="     << (int) ctxt.tx_count            << std::endl;
+    file << "rx_count="     << (int) ctxt.rx_count            << std::endl;
+    file << "int_alg="      << (int) ctxt.integ_algo          << std::endl;
+    file << "enc_alg="      << (int) ctxt.cipher_algo         << std::endl;
+    file << "ksi="          << (int) ctxt.ksi                 << std::endl;
+
+    file << "k_asme="       << hex_to_string(ctxt.k_asme, 32) << std::endl;
+
+    nas_log->info("Saved GUTI to file "
+                  "m_tmsi: %x, mcc: %x, mnc: %x, mme_group_id: %x, mme_code: %x\n",
+                  ctxt.guti.m_tmsi,
+                  ctxt.guti.mcc,
+                  ctxt.guti.mnc,
+                  ctxt.guti.mme_group_id,
+                  ctxt.guti.mme_code);
+    nas_log->info("Saved security ctxt to file .ctxt. "
+                  "ksi: %x, k_asme: %s, tx_count: %x, rx_count: %x, int_alg: %d, enc_alg: %d\n",
+                  ctxt.ksi,
+                  hex_to_string(ctxt.k_asme,32).c_str(),
+                  ctxt.tx_count,
+                  ctxt.rx_count,
+                  ctxt.integ_algo,
+                  ctxt.cipher_algo);
+    file.close();
+    return true;
+  } else {
+    return false;
+  }
+}
+
+/*********************************************************************
+ *   Conversion helpers
+ ********************************************************************/
+std::string nas::hex_to_string(uint8_t *hex, int size)
+{
+  std::stringstream ss;
+
+  ss << std::hex << std::setfill('0');
+  for(int i=0; i<size; i++) {
+    ss << std::setw(2) << static_cast<unsigned>(hex[i]);
+  }
+  return ss.str();
+}
+
+bool nas::string_to_hex(std::string hex_str, uint8_t *hex, uint32_t len)
+{
+  static const char* const lut = "0123456789abcdef";
+  uint32_t str_len = hex_str.length();
+  if(str_len & 1) {
+    return false; // uneven hex_str length
+  }
+  if(str_len > len*2) {
+    return false; // not enough space in hex buffer
+  }
+  for(uint32_t i=0; i<str_len; i+=2)
+  {
+    char a = hex_str[i];
+    const char* p = std::lower_bound(lut, lut + 16, a);
+    if (*p != a) {
+      return false; // invalid char
+    }
+    char b = hex_str[i+1];
+    const char* q = std::lower_bound(lut, lut + 16, b);
+    if (*q != b) {
+      return false; // invalid char
+    }
+    hex[i/2] = ((p - lut) << 4) | (q - lut);
+  }
+  return true;
+}
+
+std::string nas::emm_info_str(LIBLTE_MME_EMM_INFORMATION_MSG_STRUCT *info)
+{
+  std::stringstream ss;
+  if(info->full_net_name_present) {
+    ss << info->full_net_name.name;
+  }
+  if(info->short_net_name_present) {
+    ss << " (" << info->short_net_name.name << ")";
+  }
+  if(info->utc_and_local_time_zone_present) {
+    ss << " " << (int)info->utc_and_local_time_zone.day;
+    ss << "/" << (int)info->utc_and_local_time_zone.month;
+    ss << "/" << (int)info->utc_and_local_time_zone.year;
+    ss << " " << (int)info->utc_and_local_time_zone.hour;
+    ss << ":" << (int)info->utc_and_local_time_zone.minute;
+    ss << ":" << (int)info->utc_and_local_time_zone.second;
+    ss << " TZ:" << (int)info->utc_and_local_time_zone.tz;
+  }
+  return ss.str();
+}
+
+
 } // namespace srsue
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index de275bea9..ca0c31cb0 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -27,8 +27,9 @@
 
 #include <unistd.h>
 #include <iostream>
-#include <fstream>
 #include <sstream>
+#include <stdlib.h>
+#include <time.h>
 #include "upper/rrc.h"
 #include "srslte/asn1/liblte_rrc.h"
 #include "srslte/common/security.h"
@@ -99,7 +100,6 @@ void rrc::init(phy_interface_rrc *phy_,
   args.supported_bands[0] = 7;
   args.nof_supported_bands = 1;
   args.feature_group = 0xe6041c00;
-  args.stmsi_attach = false;
 
   t301 = mac_timers->timer_get_unique_id();
   t310 = mac_timers->timer_get_unique_id();
@@ -117,6 +117,9 @@ void rrc::init(phy_interface_rrc *phy_,
   set_rrc_default();
   set_phy_default();
   set_mac_default();
+
+  // set seed for rand (used in attach)
+  srand(time(NULL));
 }
 
 void rrc::stop() {
@@ -592,66 +595,6 @@ void rrc::timer_expired(uint32_t timeout_id) {
 *
 *******************************************************************************/
 
-bool rrc::read_stimsi_file(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi) {
-  std::ifstream file;
-  std::string   line;
-  if (!s_tmsi) {
-    return false;
-  }
-
-  const char *mmec_str  = "mmec=";
-  size_t mmec_str_len   = strlen(mmec_str);
-  const char *mtmsi_str = "mtmsi=";
-  size_t mtmsi_str_len  = strlen(mtmsi_str);
-
-  file.open(".stmsi", std::ios::in);
-  if (file.is_open()) {
-    bool read_ok = true;
-    if (std::getline(file, line)) {
-      if (!line.substr(0,mmec_str_len).compare(mmec_str)) {
-        s_tmsi->mmec = atoi(line.substr(5).c_str());
-      } else {
-        read_ok = false;
-      }
-    } else {
-      read_ok = false;
-    }
-    if (std::getline(file, line)) {
-      if (!line.substr(0,mtmsi_str_len).compare(mtmsi_str)) {
-        s_tmsi->m_tmsi = atoi(line.substr(mtmsi_str_len).c_str());
-      } else {
-        read_ok = false;
-      }
-    } else {
-      read_ok = false;
-    }
-    file.close();
-    if (read_ok) {
-      rrc_log->info("Read S-TMSI value: %x:%x\n", s_tmsi->mmec, s_tmsi->m_tmsi);
-      return true;
-    } else {
-      rrc_log->error("Invalid s-tmsi persistent file format\n");
-      return false;
-    }
-  } else {
-    return false;
-  }
-}
-
-bool rrc::write_stimsi_file(LIBLTE_RRC_S_TMSI_STRUCT s_tmsi) {
-  std::ofstream file;
-  file.open(".stmsi", std::ios::out | std::ios::trunc);
-  if (file.is_open()) {
-    file << "mmec="  << (int) s_tmsi.mmec << std::endl;
-    file << "mtmsi=" << (int) s_tmsi.m_tmsi << std::endl;
-    rrc_log->info("Saved S-TMSI in persistent file: %x:%x\n", s_tmsi.mmec, s_tmsi.m_tmsi);
-    file.close();
-    return true;
-  } else {
-    return false;
-  }
-}
-
 void rrc::send_con_request() {
   rrc_log->debug("Preparing RRC Connection Request\n");
   LIBLTE_RRC_UL_CCCH_MSG_STRUCT ul_ccch_msg;
@@ -659,30 +602,13 @@ void rrc::send_con_request() {
 
   // Prepare ConnectionRequest packet
   ul_ccch_msg.msg_type = LIBLTE_RRC_UL_CCCH_MSG_TYPE_RRC_CON_REQ;
-  bool valid_stmsi = false;
 
-  if (nas->get_s_tmsi(&s_tmsi) || (args.stmsi_attach && !first_stimsi_attempt)) {
+  if (nas->get_s_tmsi(&s_tmsi)) {
     ul_ccch_msg.msg.rrc_con_req.ue_id_type = LIBLTE_RRC_CON_REQ_UE_ID_TYPE_S_TMSI;
-    if (nas->get_s_tmsi(&s_tmsi)) {
-      ul_ccch_msg.msg.rrc_con_req.ue_id.s_tmsi = s_tmsi;
-      valid_stmsi = true;
-    } else {
-      first_stimsi_attempt = true;
-      if (args.stmsi_value.m_tmsi) {
-        ul_ccch_msg.msg.rrc_con_req.ue_id.s_tmsi = args.stmsi_value;
-        valid_stmsi = true;
-      } else {
-        if (!read_stimsi_file(&ul_ccch_msg.msg.rrc_con_req.ue_id.s_tmsi)) {
-          rrc_log->warning("Could not read S-TMSI from persistent file. Trying normal attach.\n");
-        } else {
-          valid_stmsi = true;
-        }
-      }
-    }
-  }
-  if (!valid_stmsi) {
+    ul_ccch_msg.msg.rrc_con_req.ue_id.s_tmsi = s_tmsi;
+  } else {
     ul_ccch_msg.msg.rrc_con_req.ue_id_type = LIBLTE_RRC_CON_REQ_UE_ID_TYPE_RANDOM_VALUE;
-    ul_ccch_msg.msg.rrc_con_req.ue_id.random = 1000;
+    ul_ccch_msg.msg.rrc_con_req.ue_id.random = rand() % 2^40;
   }
 
   ul_ccch_msg.msg.rrc_con_req.cause = LIBLTE_RRC_CON_REQ_EST_CAUSE_MO_SIGNALLING;
@@ -956,14 +882,6 @@ void rrc::handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGU
     nas_sdu->N_bytes = reconfig->ded_info_nas_list[i].N_bytes;
     nas->write_pdu(lcid, nas_sdu);
   }
-
-  // Get S-TMSI from NAS and store it in persistent file
-  LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
-  if (nas->get_s_tmsi(&s_tmsi)) {
-    if (!write_stimsi_file(s_tmsi)) {
-      rrc_log->warning("Could not store S-TMSI in persistent file\n");
-    }
-  }
 }
 
   /* Actions upon reception of RRCConnectionRelease 5.3.8.3 */
@@ -1247,7 +1165,9 @@ void rrc::parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu) {
       integ_algo = (INTEGRITY_ALGORITHM_ID_ENUM) dl_dcch_msg.msg.security_mode_cmd.sec_algs.int_alg;
 
       // Configure PDCP for security
-      usim->generate_as_keys(nas->get_ul_count(), k_rrc_enc, k_rrc_int, k_up_enc, k_up_int, cipher_algo, integ_algo);
+      uint8_t k_asme[32];
+      nas->get_k_asme(k_asme, 32);
+      usim->generate_as_keys(k_asme, nas->get_ul_count()-1, k_rrc_enc, k_rrc_int, k_up_enc, k_up_int, cipher_algo, integ_algo);
       pdcp->config_security(lcid, k_rrc_enc, k_rrc_int, cipher_algo, integ_algo);
       send_security_mode_complete(lcid, pdu);
       break;
diff --git a/srsue/src/upper/usim.cc b/srsue/src/upper/usim.cc
index e1d2c204f..7ae53988b 100644
--- a/srsue/src/upper/usim.cc
+++ b/srsue/src/upper/usim.cc
@@ -39,9 +39,11 @@ usim::usim() : initiated(false)
 void usim::init(usim_args_t *args, srslte::log *usim_log_)
 {
   usim_log = usim_log_;
+  imsi_str = args->imsi;
+  imei_str = args->imei;
 
-  const char *imsi_str = args->imsi.c_str();
-  const char *imei_str = args->imei.c_str();
+  const char *imsi_c = args->imsi.c_str();
+  const char *imei_c = args->imei.c_str();
   uint32_t    i;
 
   if(32 == args->op.length()) {
@@ -63,7 +65,7 @@ void usim::init(usim_args_t *args, srslte::log *usim_log_)
     for(i=0; i<15; i++)
     {
       imsi *= 10;
-      imsi += imsi_str[i] - '0';
+      imsi += imsi_c[i] - '0';
     }
   } else {
     usim_log->error("Invalid length for ISMI: %d should be %d", args->imsi.length(), 15);
@@ -75,7 +77,7 @@ void usim::init(usim_args_t *args, srslte::log *usim_log_)
     for(i=0; i<15; i++)
     {
       imei *= 10;
-      imei += imei_str[i] - '0';
+      imei += imei_c[i] - '0';
     }
   } else {
     usim_log->error("Invalid length for IMEI: %d should be %d", args->imei.length(), 15);
@@ -103,16 +105,25 @@ void usim::stop()
   NAS interface
 *******************************************************************************/
 
-void usim::get_imsi_vec(uint8_t* imsi_, uint32_t n)
+std::string usim::get_imsi_str()
+{
+  return imsi_str;
+}
+std::string usim::get_imei_str()
+{
+  return imei_str;
+}
+
+bool usim::get_imsi_vec(uint8_t* imsi_, uint32_t n)
 {
   if (!initiated) {
     fprintf(stderr, "USIM not initiated!\n");
-    return;
+    return false;
   }
 
   if(NULL == imsi_ || n < 15) {
     usim_log->error("Invalid parameters to get_imsi_vec");
-    return;
+    return false;
   }
 
   uint64_t temp = imsi;
@@ -120,18 +131,19 @@ void usim::get_imsi_vec(uint8_t* imsi_, uint32_t n)
     imsi_[i] = temp % 10;
     temp /= 10;
   }
+  return true;
 }
 
-void usim::get_imei_vec(uint8_t* imei_, uint32_t n)
+bool usim::get_imei_vec(uint8_t* imei_, uint32_t n)
 {
   if (!initiated) {
     fprintf(stderr, "USIM not initiated!\n");
-    return;
+    return false;
   }
 
   if(NULL == imei_ || n < 15) {
     usim_log->error("Invalid parameters to get_imei_vec");
-    return;
+    return false;
   }
 
   uint64 temp = imei;
@@ -140,13 +152,14 @@ void usim::get_imei_vec(uint8_t* imei_, uint32_t n)
     imei_[i] = temp % 10;
     temp /= 10;
   }
+  return true;
 }
 
-int usim::get_home_plmn_id(LIBLTE_RRC_PLMN_IDENTITY_STRUCT *home_plmn_id)
+bool usim::get_home_plmn_id(LIBLTE_RRC_PLMN_IDENTITY_STRUCT *home_plmn_id)
 {
   if (!initiated) {
     fprintf(stderr, "USIM not initiated!\n");
-    return -1;
+    return false;
   }
 
   int mcc_len = 3;
@@ -180,7 +193,7 @@ int usim::get_home_plmn_id(LIBLTE_RRC_PLMN_IDENTITY_STRUCT *home_plmn_id)
   usim_log->info("Read Home PLMN Id=%s\n",
                  plmn_id_to_string(*home_plmn_id).c_str());
 
-  return 0;
+  return true;
 }
 
 void usim::generate_authentication_response(uint8_t  *rand,
@@ -188,16 +201,18 @@ void usim::generate_authentication_response(uint8_t  *rand,
                                             uint16_t  mcc,
                                             uint16_t  mnc,
                                             bool     *net_valid,
-                                            uint8_t  *res)
+                                            uint8_t  *res,
+                                            uint8_t  *k_asme)
 {
   if(auth_algo_xor == auth_algo) {
-    gen_auth_res_xor(rand, autn_enb, mcc, mnc, net_valid, res);
+    gen_auth_res_xor(rand, autn_enb, mcc, mnc, net_valid, res, k_asme);
   } else {
-    gen_auth_res_milenage(rand, autn_enb, mcc, mnc, net_valid, res);
+    gen_auth_res_milenage(rand, autn_enb, mcc, mnc, net_valid, res, k_asme);
   }
 }
 
-void usim::generate_nas_keys(uint8_t *k_nas_enc,
+void usim::generate_nas_keys(uint8_t *k_asme,
+                             uint8_t *k_nas_enc,
                              uint8_t *k_nas_int,
                              CIPHERING_ALGORITHM_ID_ENUM cipher_algo,
                              INTEGRITY_ALGORITHM_ID_ENUM integ_algo)
@@ -214,7 +229,8 @@ void usim::generate_nas_keys(uint8_t *k_nas_enc,
   RRC interface
 *******************************************************************************/
 
-void usim::generate_as_keys(uint32_t count_ul,
+void usim::generate_as_keys(uint8_t *k_asme,
+                            uint32_t count_ul,
                             uint8_t *k_rrc_enc,
                             uint8_t *k_rrc_int,
                             uint8_t *k_up_enc,
@@ -251,7 +267,8 @@ void usim::gen_auth_res_milenage( uint8_t  *rand,
                                   uint16_t  mcc,
                                   uint16_t  mnc,
                                   bool     *net_valid,
-                                  uint8_t  *res)
+                                  uint8_t  *res,
+                                  uint8_t  *k_asme)
 {
   uint32_t i;
   uint8_t  sqn[6];
@@ -320,7 +337,8 @@ void usim::gen_auth_res_xor(uint8_t  *rand,
                             uint16_t  mcc,
                             uint16_t  mnc,
                             bool     *net_valid,
-                            uint8_t  *res)
+                            uint8_t  *res,
+                            uint8_t  *k_asme)
 {
   uint32_t i;
   uint8_t  sqn[6];
diff --git a/srsue/test/upper/usim_test.cc b/srsue/test/upper/usim_test.cc
index c8a248184..4ac468673 100644
--- a/srsue/test/upper/usim_test.cc
+++ b/srsue/test/upper/usim_test.cc
@@ -70,6 +70,7 @@ int main(int argc, char **argv)
   srslte::log_filter usim_log("USIM");
   bool    net_valid;
   uint8_t res[16];
+  uint8_t k_asme[32];
 
   usim_args_t args;
   args.algo = "milenage";
@@ -81,7 +82,7 @@ int main(int argc, char **argv)
 
   srsue::usim usim;
   usim.init(&args, &usim_log);
-  usim.generate_authentication_response(rand_enb, autn_enb, mcc, mnc, &net_valid, res);
+  usim.generate_authentication_response(rand_enb, autn_enb, mcc, mnc, &net_valid, res, k_asme);
 
   assert(net_valid == true);
 }
diff --git a/srsue/ue.conf.example b/srsue/ue.conf.example
index 867221e87..7bd6fae18 100644
--- a/srsue/ue.conf.example
+++ b/srsue/ue.conf.example
@@ -137,13 +137,7 @@ enable = false
 # equalizer_mode:       Selects equalizer mode. Valid modes are: "mmse", "zf" or any 
 #                       non-negative real number to indicate a regularized zf coefficient.
 #                       Default is MMSE.
-# cfo_ema:              CFO Exponential Moving Average coefficient. Lower makes it more robust to noise
-#                       but vulnerable to periodic interruptions due to VCO corrections.
-# cfo_integer_enabled:  Enables integer CFO estimation and correction. This needs improvement
-#                       and may lead to incorrect synchronization. Use with caution. 
-# cfo_correct_tol_hz:   Tolerance (in Hz) for digial CFO compensation. Lower tolerance means that 
-#                       a new table will be generated more often. 
-# time_correct_period:  Period for sampling time offset correction. Default is 10 (ue_sync.c), 
+# time_correct_period:  Period for sampling time offset correction. Default is 10 (ue_sync.c),
 #                       good for long channels. For best performance at highest SNR reduce it to 1.  
 # sfo_correct_disable:  Disables phase correction before channel estimation to compensate for 
 #                       sampling frequency offset. Default is enabled. 
@@ -155,10 +149,28 @@ enable = false
 #
 # pregenerate_signals:  Pregenerate uplink signals after attach. Improves CPU performance.
 #
+# average_subframe_enabled: Averages in the time domain the channel estimates within 1 subframe.
+#                           Needs accurate CFO correction.
+#
 # metrics_csv_enable:   Write UE metrics to CSV file.
 #
 # metrics_csv_filename: File path to use for CSV metrics.
 #
+# cfo_integer_enabled:  Enables integer CFO estimation and correction. This needs improvement
+#                       and may lead to incorrect synchronization. Use with caution.
+# cfo_correct_tol_hz:   Tolerance (in Hz) for digial CFO compensation. Lower tolerance means that
+#                       a new table will be generated more often.
+#
+# cfo_pss_ema:           CFO Exponential Moving Average coefficient for PSS estimation during TRACK.
+# cfo_ref_ema:           CFO Exponential Moving Average coefficient for RS estimation after PSS acquisition
+# cfo_ref_mask:          Bitmask for subframes on which to run RS estimation (set to 0 to disable, default sf=[1, 5])
+# cfo_loop_bw:           CFO feedback loop bandwidth for samples from PSS or RS
+# cfo_loop_pss_tol:      Tolerance (in Hz) of the PSS estimation method. Below this value, PSS estimation does not feeds back the loop
+#                        and RS estimations are used instead (when available)
+# cfo_loop_ref_min:      Tolerance (in Hz) of the RS estimation method. Below this value, RS estimation does not feeds back the loop
+# cfo_loop_pss_timeout:  After the PSS estimation is below cfo_loop_pss_tol for cfo_loop_pss_timeout times consecutively,
+#                        RS adjustments are allowed.
+#
 #####################################################################
 [expert]
 #ip_netmask          = 255.255.255.0
@@ -172,17 +184,27 @@ enable = false
 #attach_enable_64qam = false
 #nof_phy_threads     = 2
 #equalizer_mode      = mmse
-#cfo_ema             = 0.4
-#cfo_integer_enabled = false
-#cfo_correct_tol_hz  = 50
 #time_correct_period = 5
 #sfo_correct_disable = false
 #sss_algorithm       = full
 #estimator_fil_w     = 0.1
+#average_subframe_enabled = true
 #pregenerate_signals = false
 #metrics_csv_enable  = false
 #metrics_csv_filename = /tmp/ue_metrics.csv
 
+# CFO related values
+#cfo_integer_enabled = false
+#cfo_correct_tol_hz  = 0
+#cfo_pss_ema         = 0.1
+#cfo_ref_ema         = 0.01
+#cfo_ref_mask        = 1023
+#cfo_loop_bw_pss     = 0.05
+#cfo_loop_bw_ref     = 0.01
+#cfo_loop_pss_tol    = 300
+#cfo_loop_ref_min    = 0
+#cfo_loop_pss_conv   = 50
+
 #####################################################################
 # Manual RF calibration
 #