diff --git a/lib/examples/cell_measurement.c b/lib/examples/cell_measurement.c
index eec7548e4..71c0864c8 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 26c681346..6608f0817 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, 
                                           data[0],
@@ -775,7 +794,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));
@@ -939,7 +958,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");
@@ -975,7 +994,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/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index 94a94dd0d..21e40dde9 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -471,7 +471,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 081b332c7..151c530b1 100644
--- a/lib/include/srslte/phy/sync/sync.h
+++ b/lib/include/srslte/phy/sync/sync.h
@@ -74,20 +74,8 @@ typedef struct SRSLTE_API {
   uint32_t fft_size;
   uint32_t frame_size;
   uint32_t max_offset;
-  bool enable_cfo_corr;
-  bool enable_cfo_pss;
-  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;
@@ -103,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 {
@@ -136,33 +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);
 
-SRSLTE_API float srslte_sync_cfo_estimate(srslte_sync_t *q,
-                                          cf_t *input,
-                                          int find_offset);
-
 /* 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 */
@@ -180,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);
 
-/* Set integer CFO */
-SRSLTE_API void srslte_sync_set_cfo_i(srslte_sync_t *q, 
-                                      int cfo_i); 
+/* 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);
 
-SRSLTE_API void srslte_sync_set_cfo_enable(srslte_sync_t *q, 
-                                           bool enable); 
+/* 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);
+
+/* 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);
 
@@ -204,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);
@@ -216,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 0d12f846c..16936c062 100644
--- a/lib/include/srslte/phy/ue/ue_sync.h
+++ b/lib/include/srslte/phy/ue/ue_sync.h
@@ -88,12 +88,7 @@ typedef struct SRSLTE_API {
   bool file_mode; 
   float file_cfo; 
   srslte_cfo_t file_cfo_correct; 
-
-  bool  mean_cfo_isunset;
-  float cfo;
-  float mean_cfo;
-  float cfo_ema_alpha;
-
+  
   srslte_ue_sync_state_t state;
   
   uint32_t frame_len; 
@@ -115,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; 
@@ -170,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); 
@@ -180,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);
@@ -223,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/phy/ch_estimation/chest_dl.c b/lib/src/phy/ch_estimation/chest_dl.c
index d76397cf0..cdd1af7f8 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 */
   uint32_t npilots = SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id);
@@ -453,8 +513,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)
@@ -476,10 +537,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++) {
@@ -519,7 +576,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/rf/rf_blade_imp.c b/lib/src/phy/rf/rf_blade_imp.c
index 86b5383b9..4a8173f29 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_utils.c b/lib/src/phy/rf/rf_utils.c
index 0ef082a23..68857d41d 100644
--- a/lib/src/phy/rf/rf_utils.c
+++ b/lib/src/phy/rf/rf_utils.c
@@ -122,13 +122,8 @@ int rf_mib_decoder(srslte_rf_t *rf, uint32_t nof_rx_antennas,cell_search_cfg_t *
   srslte_rf_set_rx_srate(rf, (float) srate);
   
   INFO("Starting receiver...\n", 0);
-  srslte_rf_start_rx_stream(rf, false);
-    
-  // Set CFO if available
-  if (cfo) {
-    srslte_ue_sync_set_cfo(&ue_mib.ue_sync, *cfo);
-  }
-  
+  srslte_rf_start_rx_stream(rf, true);
+
   /* 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 34009fbad..ac29bac21 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; 
   
@@ -66,31 +64,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;
     }
@@ -146,18 +147,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);
@@ -173,53 +179,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);
 
@@ -235,30 +239,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;
 }
@@ -290,30 +278,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_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_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_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(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_set_sss_filt_enable(srslte_sync_t *q, bool enable) {
+  q->sss_filtering_enabled = enable;
 }
 
-void srslte_sync_set_cfo_i(srslte_sync_t *q, int cfo_i) {
-  q->cfo_i = cfo_i;
+void srslte_sync_set_pss_filt_enable(srslte_sync_t *q, bool enable) {
+  q->pss_filtering_enabled = enable;
 }
 
-void srslte_sync_set_cfo_enable(srslte_sync_t *q, bool enable) {
-  q->enable_cfo_corr = enable; 
+void srslte_sync_set_cfo_cp_enable(srslte_sync_t *q, bool enable) {
+  q->cfo_cp_enable = enable;
 }
 
-void srslte_sync_set_cfo_ema_alpha(srslte_sync_t *q, float alpha) {
-  q->cfo_ema_alpha = alpha;
+void srslte_sync_set_cfo_pss_enable(srslte_sync_t *q, bool enable) {
+  q->cfo_pss_enable = enable;
 }
 
-float srslte_sync_get_last_peak_value(srslte_sync_t *q) {
-  return q->peak_value;
+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) {
@@ -324,18 +341,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) {
@@ -345,7 +361,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; 
 }
 
@@ -353,7 +370,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; 
@@ -369,8 +386,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));    
@@ -413,28 +430,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;
   }
 
@@ -451,20 +461,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; 
@@ -472,6 +485,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) {
@@ -480,126 +496,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);
+
+    /* 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_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;
       }
     }
 
-    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;
-    }
-    if (peak_pos < 0) {
-      fprintf(stderr, "Error calling finding PSS sequence at : %d  \n", peak_pos);
-      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->peak_value >= q->threshold || q->threshold == 0) {
 
-      ret = SRSLTE_SYNC_FOUND;
+      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;
+        }
 
-      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));
+        // 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);
-          ret = SRSLTE_SYNC_FOUND_NOSPACE;
+          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 (q->enable_cfo_pss) {
-        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;
-          } else {
-            q->mean_cfo2 = SRSLTE_VEC_EMA(cfo2, q->mean_cfo2, q->cfo_ema_alpha);
+      // 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 {
+          DEBUG("Not enough room to detect CP length. Peak position: %d\n", peak_pos);
+        }
+
+        ret = SRSLTE_SYNC_FOUND;
+      } else {
+        ret = SRSLTE_SYNC_FOUND_NOSPACE;
       }
     } else {
       ret = SRSLTE_SYNC_NOFOUND;
@@ -607,7 +669,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 25f8a07b3..704340b2f 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 8adf77dd6..b5d89743f 100644
--- a/lib/src/phy/ue/ue_sync.c
+++ b/lib/src/phy/ue/ue_sync.c
@@ -47,9 +47,14 @@
 #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 DO_CFO_IN_SYNC
+#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];
@@ -74,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");
@@ -100,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;
@@ -111,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) {
@@ -183,16 +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->mean_cfo_isunset = true;
-    q->mean_cfo         = 0;
-    q->cfo_ema_alpha    = 0.4;
     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 */
@@ -227,10 +267,28 @@ int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q,
       }
     }
 
-#ifndef DO_CFO_IN_SYNC
-    // Disable CFO correction in sync object and do it here every subframe
-    srslte_sync_set_cfo_enable(&q->strack, false);
-#endif
+    // 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;
   }
@@ -267,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;
@@ -283,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;
@@ -301,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);
@@ -369,42 +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);
-  q->cfo_ema_alpha = 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) {
-#ifdef DO_CFO_IN_SYNC
-  return 15000 * srslte_sync_get_cfo(&q->strack);
-#else
-  return 15000 * q->mean_cfo;
-#endif
+  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;
@@ -423,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) {
@@ -451,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);
@@ -468,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;
@@ -508,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
@@ -560,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;
   }
 
@@ -595,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 */
@@ -627,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],
@@ -642,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:
@@ -676,56 +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;
 
-#ifndef DO_CFO_IN_SYNC
-          if (q->correct_cfo) {
-            /* We found that CP-based correction performs better in low SNR than PSS-based.
-             *
-             * Estimate, average and correct here instead of inside sync object
-             */
-            q->cfo = srslte_sync_cfo_estimate(&q->strack, input_buffer[0], 0);
-            if (q->mean_cfo_isunset) {
-              q->mean_cfo = q->cfo;
-              q->mean_cfo_isunset = false;
-            } else {
-              /* compute exponential moving average CFO */
-              q->mean_cfo = SRSLTE_VEC_EMA(q->cfo, q->mean_cfo, q->cfo_ema_alpha);
+          // 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);
             }
-            srslte_cfo_correct(&q->strack.cfocorr2, input_buffer[0], input_buffer[0], -q->mean_cfo / q->fft_size);
           }
 
-#endif
-
           /* Every SF idx 0 and 5, find peak around known position q->peak_idx */
-          if (q->sf_idx == 0 || q->sf_idx == 5) {
-
+          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: 
@@ -735,52 +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++;       
-          } 
+            }
+
+            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 ec2809dfe..e50be05a6 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 ecdedf63b..d7b960ea5 100644
--- a/srsue/hdr/phy/phch_common.h
+++ b/srsue/hdr/phy/phch_common.h
@@ -42,134 +42,141 @@
 
 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;
-
-    /* Power control variables */
-    float pathloss;
-    float cur_pathloss;
-    float p0_preamble;     
-    float cur_radio_power; 
-    float cur_pusch_power;
-    float avg_rsrp;
-    float avg_rsrp_dbm;
-    float avg_rsrq_db; 
-    float rx_gain_offset;
-    float avg_snr_db; 
-    float avg_noise; 
-
-    bool     pcell_meas_enabled;
-    uint32_t pcell_report_period;
-
-    // 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);
-    bool is_any_pending_ack();
-
-    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();
-    void reset();
-    
-  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;
+
+  /* Power control variables */
+  float pathloss;
+  float cur_pathloss;
+  float p0_preamble;
+  float cur_radio_power;
+  float cur_pusch_power;
+  float avg_rsrp;
+  float avg_rsrp_dbm;
+  float avg_rsrq_db;
+  float rx_gain_offset;
+  float avg_snr_db;
+  float avg_noise;
+
+  bool     pcell_meas_enabled;
+  uint32_t pcell_report_period;
+
+  // 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);
+  bool is_any_pending_ack();
+
+  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();
+  void reset();
+
+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
 
diff --git a/srsue/hdr/phy/phch_recv.h b/srsue/hdr/phy/phch_recv.h
index 4f54580f1..28576c25b 100644
--- a/srsue/hdr/phy/phch_recv.h
+++ b/srsue/hdr/phy/phch_recv.h
@@ -45,7 +45,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();
@@ -74,6 +74,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, uint32_t *earfcn = NULL);
 
diff --git a/srsue/hdr/phy/phch_worker.h b/srsue/hdr/phy/phch_worker.h
index 067a3a8fb..4f8df27db 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 */
@@ -114,6 +116,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/upper/nas.h b/srsue/hdr/upper/nas.h
index d79eddea8..2e795098e 100644
--- a/srsue/hdr/upper/nas.h
+++ b/srsue/hdr/upper/nas.h
@@ -116,20 +116,18 @@ private:
 
   LIBLTE_MME_EMM_INFORMATION_MSG_STRUCT emm_info;
 
-  // Identifiers
-  LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT guti;
-  bool have_guti;
-
   // 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;
+    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;
 
@@ -174,13 +172,42 @@ private:
   void gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg);
   void send_security_mode_reject(uint8_t cause);
 
-  // guti persistence file
-  bool read_guti_file(LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT *guti);
-  bool write_guti_file(LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT guti);
-
   // 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/src/main.cc b/srsue/src/main.cc
index 113f4d5c0..6c821dc6d 100644
--- a/srsue/src/main.cc
+++ b/srsue/src/main.cc
@@ -199,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_ema",
-     bpo::value<float>(&args->expert.phy.cfo_ema)->default_value(0.3),
-     "CFO Exponential Moving Average coefficient. Lower makes it more robust to noise "
-     "but vulnerable to periodic interruptions due to VCO corrections.")
+    ("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.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 d9b1b7f72..e3ab21c0e 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -154,8 +154,12 @@ void phch_recv::radio_error()
   radio_is_resetting=false;
 }
 
-bool phch_recv::wait_radio_reset()
-{
+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) {
     sleep(1);
@@ -182,10 +186,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_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) {
@@ -231,10 +241,6 @@ bool phch_recv::set_cell() {
 
   // Reset ue_sync and set CFO/gain from search procedure
   srslte_ue_sync_reset(&ue_sync);
-  srslte_ue_sync_set_cfo(&ue_sync, search_p.get_last_cfo());
-  if (do_agc) {
-    srslte_ue_sync_start_agc(&ue_sync, callback_set_rx_gain, search_p.get_last_gain());
-  }
 
   cell_is_set = true;
 
@@ -634,6 +640,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 */
@@ -827,7 +836,6 @@ phch_recv::search::ret_code phch_recv::search::run(srslte_cell_t *cell)
   }
 
   srslte_ue_sync_reset(&ue_mib_sync.ue_sync);
-  srslte_ue_sync_set_cfo(&ue_mib_sync.ue_sync, cfo);
 
   /* Find and decode MIB */
   int sfn_offset;
@@ -1157,11 +1165,17 @@ void phch_recv::scell_recv::init(srslte::log *log_h)
     return;
   }
   srslte_sync_cp_en(&sync_find, false);
-  srslte_sync_set_cfo_enable(&sync_find, false);
-  srslte_sync_set_cfo_ema_alpha(&sync_find, 0.8);
   srslte_sync_set_threshold(&sync_find, 1.2);
   srslte_sync_set_em_alpha(&sync_find, 0.0);
 
+  // Configure FIND object behaviour (this configuration is always the same)
+  srslte_sync_set_cfo_ema_alpha(&sync_find,    0.2);
+  srslte_sync_set_cfo_i_enable(&sync_find,     false);
+  srslte_sync_set_cfo_cp_enable(&sync_find,    true);
+  srslte_sync_set_cfo_pss_enable(&sync_find,   true);
+  srslte_sync_set_pss_filt_enable(&sync_find,  true);
+  srslte_sync_set_sss_filt_enable(&sync_find,  true);
+
   reset();
 }
 
@@ -1201,7 +1215,6 @@ int phch_recv::scell_recv::find_cells(cf_t *input_buffer, float rx_gain_offset,
     if (cell.id%3 != n_id_2) {
       srslte_sync_set_N_id_2(&sync_find, n_id_2);
 
-      sync_find.enable_cfo_pss = true;
       srslte_sync_find_ret_t sync_res = srslte_sync_find(&sync_find, input_buffer, 0, &peak_idx);
 
       switch(sync_res) {
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 5ae0b242a..c56ce9eb8 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;
@@ -105,9 +107,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));
@@ -127,15 +131,20 @@ 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);
-  srslte_ue_ul_set_cfo_tol(&ue_ul, phy->args->cfo_correct_tol_hz);
 
   mem_initiated = true;
 
   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) {
@@ -223,6 +232,11 @@ void phch_worker::work_imp()
 
   bool snr_th_ok = 10*log10(srslte_chest_dl_get_snr(&ue_dl.chest))>-10.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 *******/
@@ -396,11 +410,8 @@ void phch_worker::work_imp()
 
 
 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) {
     
@@ -1216,13 +1227,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)
@@ -1299,7 +1315,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;
@@ -1372,7 +1388,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 917c0b480..d871c384e 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/upper/nas.cc b/srsue/src/upper/nas.cc
index 2b01efcec..62e23d852 100644
--- a/srsue/src/upper/nas.cc
+++ b/srsue/src/upper/nas.cc
@@ -38,71 +38,10 @@ using namespace srslte;
 
 namespace srsue {
 
+
 /*********************************************************************
- *   Conversion helpers
+ *   NAS
  ********************************************************************/
-std::string 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 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 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();
-}
-
 
 nas::nas()
   : state(EMM_STATE_DEREGISTERED), plmn_selection(PLMN_SELECTED), have_guti(false), ip_addr(0), eps_bearer_id(0)
@@ -132,13 +71,13 @@ void nas::init(usim_interface_nas *usim_,
   }
   cfg     = cfg_;
 
-  if((have_guti = read_guti_file(&guti))) {
-    if((have_ctxt = 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");
-    }
+  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;
   }
 }
 
@@ -151,8 +90,8 @@ emm_state_t nas::get_state() {
 }
 
 /*******************************************************************************
-UE interface
-*******************************************************************************/
+ * UE interface
+ ******************************************************************************/
 
 void nas::attach_request() {
   nas_log->info("Attach Request\n");
@@ -181,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) {
 
@@ -297,8 +236,8 @@ uint32_t nas::get_ul_count() {
 
 bool nas::get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi) {
   if (have_guti) {
-    s_tmsi->mmec   = guti.mme_code;
-    s_tmsi->m_tmsi = guti.m_tmsi;
+    s_tmsi->mmec   = ctxt.guti.mme_code;
+    s_tmsi->m_tmsi = ctxt.guti.m_tmsi;
     return true;
   } else {
     return false;
@@ -320,8 +259,8 @@ bool nas::get_k_asme(uint8_t *k_asme_, uint32_t n) {
 }
 
 /*******************************************************************************
-Security
-*******************************************************************************/
+ * Security
+ ******************************************************************************/
 
 void nas::integrity_generate(uint8_t integ_algo,
                              uint8_t *key_128,
@@ -381,8 +320,8 @@ bool nas::check_cap_replay(LIBLTE_MME_UE_SECURITY_CAPABILITIES_STRUCT *caps)
 
 
 /*******************************************************************************
-Parsers
-*******************************************************************************/
+ * Parsers
+ ******************************************************************************/
 
 void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu) {
   LIBLTE_MME_ATTACH_ACCEPT_MSG_STRUCT attach_accept;
@@ -398,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));
+      memcpy(&ctxt.guti, &attach_accept.guti.guti, sizeof(LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT));
       have_guti = true;
-      write_guti_file(guti);
-    }
-    if (attach_accept.lai_present) {
     }
+    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);
@@ -423,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],
@@ -735,8 +673,8 @@ void nas::parse_emm_information(uint32_t lcid, byte_buffer_t *pdu) {
 }
 
 /*******************************************************************************
-Senders
-*******************************************************************************/
+ * Senders
+ ******************************************************************************/
 
 void nas::send_attach_request() {
   LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT attach_req;
@@ -774,14 +712,14 @@ void nas::send_attach_request() {
   // 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, &guti, sizeof(LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT));
+    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",
-                  guti.m_tmsi, guti.mcc, guti.mnc, guti.mme_group_id, guti.mme_code);
+                  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,
@@ -874,103 +812,94 @@ void nas::send_service_request() {
 
 void nas::send_esm_information_response() {}
 
-bool nas::read_guti_file(LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT *guti)
+
+/*******************************************************************************
+ * Security context persistence file
+ ******************************************************************************/
+
+bool nas::read_ctxt_file(nas_sec_ctxt *ctxt)
 {
   std::ifstream file;
-  std::string   line;
-  if (!guti) {
+  if(!ctxt) {
     return false;
   }
 
-  const char *m_tmsi_str         = "m_tmsi=";
-  size_t m_tmsi_str_len         = strlen(m_tmsi_str);
-  const char *mcc_str           = "mcc=";
-  size_t mcc_str_len            = strlen(mcc_str);
-  const char *mnc_str           = "mnc=";
-  size_t mnc_str_len            = strlen(mnc_str);
-  const char *mme_group_id_str  = "mme_group_id=";
-  size_t mme_group_id_str_len   = strlen(mme_group_id_str);
-  const char *mme_code_str      = "mme_code=";
-  size_t mme_code_str_len       = strlen(mme_code_str);
-
-  file.open(".guti", std::ios::in);
-  if (file.is_open()) {
-    bool read_ok = true;
-    if (std::getline(file, line)) {
-      if (!line.substr(0,m_tmsi_str_len).compare(m_tmsi_str)) {
-        guti->m_tmsi = atoi(line.substr(m_tmsi_str_len).c_str());
-      } else {
-        read_ok = false;
-      }
-    } else {
-      read_ok = false;
-    }
-    if (std::getline(file, line)) {
-      if (!line.substr(0,mcc_str_len).compare(mcc_str)) {
-        guti->mcc = atoi(line.substr(mcc_str_len).c_str());
-      } else {
-        read_ok = false;
-      }
-    } else {
-      read_ok = false;
-    }
-    if (std::getline(file, line)) {
-      if (!line.substr(0,mnc_str_len).compare(mnc_str)) {
-        guti->mnc = atoi(line.substr(mnc_str_len).c_str());
-      } else {
-        read_ok = false;
-      }
-    } else {
-      read_ok = false;
-    }
-    if (std::getline(file, line)) {
-      if (!line.substr(0,mme_group_id_str_len).compare(mme_group_id_str)) {
-        guti->mme_group_id = atoi(line.substr(mme_group_id_str_len).c_str());
-      } else {
-        read_ok = false;
-      }
-    } else {
-      read_ok = false;
-    }
-    if (std::getline(file, line)) {
-      if (!line.substr(0,mme_code_str_len).compare(mme_code_str)) {
-        guti->mme_code = atoi(line.substr(mme_code_str_len).c_str());
-      } else {
-        read_ok = false;
-      }
-    } else {
-      read_ok = 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();
-    if (read_ok) {
-      nas_log->info("Read GUTI from file .guti. "
-                    "m_tmsi: %x, mcc: %x, mnc: %x, mme_group_id: %x, mme_code: %x\n",
-                    guti->m_tmsi, guti->mcc, guti->mnc, guti->mme_group_id, guti->mme_code);
-      return true;
-    } else {
-      nas_log->error("Invalid GUTI file format\n");
-      return false;
-    }
+    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_guti_file(LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT guti) {
-  std::ofstream file;
-  if (!have_guti) {
+bool nas::write_ctxt_file(nas_sec_ctxt ctxt)
+{
+  if (!have_guti || !have_ctxt) {
     return false;
   }
-  file.open(".guti", std::ios::out | std::ios::trunc);
+  std::ofstream file;
+  file.open(".ctxt", std::ios::out | std::ios::trunc);
   if (file.is_open()) {
-    file << "m_tmsi="       << (int) guti.m_tmsi << std::endl;
-    file << "mcc="          << (int) guti.mcc << std::endl;
-    file << "mnc="          << (int) guti.mnc << std::endl;
-    file << "mme_group_id=" << (int) guti.mme_group_id << std::endl;
-    file << "mme_code="     << (int) guti.mme_code << std::endl;
-    nas_log->info("Saved GUTI to file .guti. "
+    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",
-                  guti.m_tmsi, guti.mcc, guti.mnc, guti.mme_group_id, guti.mme_code);
+                  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 {
@@ -978,129 +907,66 @@ bool nas::write_guti_file(LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT guti) {
   }
 }
 
-bool nas::read_ctxt_file(nas_sec_ctxt *ctxt)
+/*********************************************************************
+ *   Conversion helpers
+ ********************************************************************/
+std::string nas::hex_to_string(uint8_t *hex, int size)
 {
-  std::ifstream file;
-  std::string   line;
-  if (!ctxt) {
-    return false;
-  }
+  std::stringstream ss;
 
-  const char *ksi_str       = "ksi=";
-  size_t ksi_str_len        = strlen(ksi_str);
-  const char *k_asme_str    = "k_asme=";
-  size_t k_asme_str_len     = strlen(k_asme_str);
-  const char *tx_count_str  = "tx_count=";
-  size_t tx_count_str_len   = strlen(tx_count_str);
-  const char *rx_count_str  = "rx_count=";
-  size_t rx_count_str_len   = strlen(rx_count_str);
-  const char *int_alg_str  = "int_alg=";
-  size_t int_alg_str_len   = strlen(int_alg_str);
-  const char *enc_alg_str  = "enc_alg=";
-  size_t enc_alg_str_len   = strlen(enc_alg_str);
+  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();
+}
 
-  file.open(".ctxt", std::ios::in);
-  if (file.is_open()) {
-    bool read_ok = true;
-    if (std::getline(file, line)) {
-      if (!line.substr(0,ksi_str_len).compare(ksi_str)) {
-        ctxt->ksi = atoi(line.substr(ksi_str_len).c_str());
-      } else {
-        read_ok = false;
-      }
-    } else {
-      read_ok = false;
-    }
-    if (std::getline(file, line)) {
-      if (!line.substr(0,k_asme_str_len).compare(k_asme_str)) {
-        std::string tmp = line.substr(k_asme_str_len);
-        if(!string_to_hex(tmp, ctxt->k_asme, 32)) {
-          read_ok = false;
-        }
-      } else {
-        read_ok = false;
-      }
-    } else {
-      read_ok = false;
-    }
-    if (std::getline(file, line)) {
-      if (!line.substr(0,tx_count_str_len).compare(tx_count_str)) {
-        ctxt->tx_count = atoi(line.substr(tx_count_str_len).c_str());
-      } else {
-        read_ok = false;
-      }
-    } else {
-      read_ok = false;
-    }
-    if (std::getline(file, line)) {
-      if (!line.substr(0,rx_count_str_len).compare(rx_count_str)) {
-        ctxt->rx_count = atoi(line.substr(rx_count_str_len).c_str());
-      } else {
-        read_ok = false;
-      }
-    } else {
-      read_ok = false;
-    }
-    if (std::getline(file, line)) {
-      if (!line.substr(0,int_alg_str_len).compare(int_alg_str)) {
-        ctxt->integ_algo = (srslte::INTEGRITY_ALGORITHM_ID_ENUM)atoi(line.substr(int_alg_str_len).c_str());
-      } else {
-        read_ok = false;
-      }
-    } else {
-      read_ok = false;
-    }
-    if (std::getline(file, line)) {
-      if (!line.substr(0,enc_alg_str_len).compare(enc_alg_str)) {
-        ctxt->cipher_algo = (srslte::CIPHERING_ALGORITHM_ID_ENUM)atoi(line.substr(enc_alg_str_len).c_str());
-      } else {
-        read_ok = false;
-      }
-    } else {
-      read_ok = false;
+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
     }
-    file.close();
-    if (read_ok) {
-      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 {
-      nas_log->error("Invalid security ctxt file format\n");
-      return false;
+    char b = hex_str[i+1];
+    const char* q = std::lower_bound(lut, lut + 16, b);
+    if (*q != b) {
+      return false; // invalid char
     }
-  } else {
-    return false;
+    hex[i/2] = ((p - lut) << 4) | (q - lut);
   }
+  return true;
 }
 
-bool nas::write_ctxt_file(nas_sec_ctxt ctxt)
+std::string nas::emm_info_str(LIBLTE_MME_EMM_INFORMATION_MSG_STRUCT *info)
 {
-  std::ofstream file;
-  file.open(".ctxt", std::ios::out | std::ios::trunc);
-  if (file.is_open()) {
-    file << "ksi="      << (int) ctxt.ksi << std::endl;
-    file << "k_asme="   << hex_to_string(ctxt.k_asme, 32) << 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;
-    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;
+  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();
 }
 
 
diff --git a/srsue/test/upper/usim_test.cc b/srsue/test/upper/usim_test.cc
index 00003bc28..4ac468673 100644
--- a/srsue/test/upper/usim_test.cc
+++ b/srsue/test/upper/usim_test.cc
@@ -61,7 +61,6 @@ KASME   : a8.27.57.5e.ea.1a.10.17.3a.a1.bf.ce.4b.0c.21.85.e0.51.ef.bd.91.7f.fe.f
 
 uint8_t rand_enb[] = {0x88, 0x38, 0xc3, 0x55, 0xc8, 0x78, 0xaa, 0x57, 0x21, 0x49, 0xfe, 0x69, 0xdb, 0x68, 0x6b, 0x5a};
 uint8_t autn_enb[] = {0xd7, 0x44, 0x51, 0x9b, 0x25, 0xaa, 0x80, 0x00, 0x84, 0xba, 0x37, 0xb0, 0xf6, 0x73, 0x4d, 0xd1};
-uint8_t kasme[] = {0xa8, 0x27, 0x57, 0x5e, 0xea, 0x1a, 0x10, 0x17, 0x3a, 0xa1, 0xbf, 0xce, 0x4b, 0x0c, 0x21, 0x85, 0xe0, 0x51, 0xef, 0xbd, 0x91, 0x7f, 0xfe, 0xf5, 0x1f, 0x74, 0x29, 0x61, 0xf9, 0x03, 0x7a, 0x35};
 
 uint16 mcc = 208;
 uint16 mnc = 93;
@@ -71,18 +70,19 @@ 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";
   args.amf = "8000";
-  args.imei = "356092040793011";
+  args.imei = "35609204079301";
   args.imsi = "208930000000001";
   args.k = "8BAF473F2F8FD09487CCCBD7097C6862";
   args.op = "11111111111111111111111111111111";
 
   srsue::usim usim;
   usim.init(&args, &usim_log);
-  usim.generate_authentication_response(rand_enb, autn_enb, mcc, mnc, &net_valid, res, kasme);
+  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 3373c562f..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.3
-#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
 #