diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index e15aa473f..acc598296 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.1);
+  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); 
   
@@ -655,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
@@ -677,6 +693,12 @@ int main(int argc, char **argv) {
                   }
                 }
               }
+
+              // Feed-back ue_sync with chest_dl CFO estimation
+              if (sfidx == 5 && prog_args.enable_cfo_ref) {
+                srslte_ue_sync_set_cfo_ref(&ue_sync, srslte_chest_dl_get_cfo(&ue_dl.chest));
+              }
+
             }else{ // MBSFN subframe
               n = srslte_ue_dl_decode_mbsfn(&ue_dl, 
                                           sf_buffer,
@@ -773,7 +795,7 @@ int main(int argc, char **argv) {
             /* Print basic Parameters */
             PRINT_LINE("   nof layers: %d", ue_dl.pdsch_cfg.nof_layers);
             PRINT_LINE("nof codewords: %d", SRSLTE_RA_DL_GRANT_NOF_TB(&ue_dl.pdsch_cfg.grant));
-            PRINT_LINE("          CFO: %+7.2f kHz", srslte_ue_sync_get_cfo(&ue_sync));
+            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));
@@ -973,7 +995,7 @@ void *plot_thread_run(void *arg) {
           tmp_plot2[g+i] = -80;
         }
       }
-      plot_real_setNewData(&pce, tmp_plot2, sz);        
+      plot_real_setNewData(&pce, tmp_plot2, sz);
       
       if (!prog_args.input_file_name) {
         if (plot_track) {
diff --git a/lib/include/srslte/phy/ch_estimation/chest_dl.h b/lib/include/srslte/phy/ch_estimation/chest_dl.h
index 229495a9b..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];
@@ -84,8 +85,9 @@ typedef struct {
   float noise_estimate[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
   float cfo;
 
-  bool cfo_estimate_enable;
+  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];
@@ -95,6 +97,7 @@ typedef struct {
   srslte_chest_dl_noise_alg_t noise_alg; 
   int last_nof_antennas;
 
+  bool average_subframe;
 } srslte_chest_dl_t;
 
 
@@ -148,7 +151,13 @@ 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);
+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); 
 
diff --git a/lib/include/srslte/phy/ue/ue_sync.h b/lib/include/srslte/phy/ue/ue_sync.h
index 11d7893c8..3619156f7 100644
--- a/lib/include/srslte/phy/ue/ue_sync.h
+++ b/lib/include/srslte/phy/ue/ue_sync.h
@@ -115,6 +115,8 @@ typedef struct SRSLTE_API {
   float cfo_current_value;
   float cfo_loop_bw;
   float cfo_pss_tol;
+  float cfo_ref_tol;
+  float cfo_ref_max;
 
   uint32_t peak_idx;
   int next_rf_sample_offset;
@@ -197,11 +199,15 @@ SRSLTE_API void srslte_ue_sync_copy_cfo(srslte_ue_sync_t *q,
 
 SRSLTE_API void srslte_ue_sync_set_cfo_loop_bw(srslte_ue_sync_t *q,
                                                float bw,
-                                               float pss_tol);
+                                               float pss_tol,
+                                               float ref_tol,
+                                               float ref_max);
 
 SRSLTE_API void srslte_ue_sync_set_cfo_ema(srslte_ue_sync_t *q,
                                            float ema);
 
+SRSLTE_API void srslte_ue_sync_set_cfo_ref(srslte_ue_sync_t *q, float res_cfo);
+
 SRSLTE_API void srslte_ue_sync_set_cfo_i_enable(srslte_ue_sync_t *q,
                                                 bool enable);
 
diff --git a/lib/src/phy/ch_estimation/chest_dl.c b/lib/src/phy/ch_estimation/chest_dl.c
index 60d62a004..e04d30e69 100644
--- a/lib/src/phy/ch_estimation/chest_dl.c
+++ b/lib/src/phy/ch_estimation/chest_dl.c
@@ -102,25 +102,30 @@ int srslte_chest_dl_init(srslte_chest_dl_t *q, uint32_t max_prb)
     }
 
     q->tmp_noise = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
-
-
     if (!q->tmp_noise) {
       perror("malloc");
       goto clean_exit;
     }
-    q->pilot_estimates = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
 
+    q->tmp_cfo_estimate = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
+    if (!q->tmp_cfo_estimate) {
+      perror("malloc");
+      goto clean_exit;
+    }
+
+    q->pilot_estimates = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
     if (!q->pilot_estimates) {
       perror("malloc");
       goto clean_exit;
     }      
+
     q->pilot_estimates_average = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
     if (!q->pilot_estimates_average) {
       perror("malloc");
       goto clean_exit;
-    }      
-    q->pilot_recv_signal = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
+    }
 
+    q->pilot_recv_signal = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
     if (!q->pilot_recv_signal) {
       perror("malloc");
       goto clean_exit;
@@ -175,6 +180,9 @@ void srslte_chest_dl_free(srslte_chest_dl_t *q)
   if (q->tmp_noise) {
     free(q->tmp_noise);
   }
+  if (q->tmp_cfo_estimate) {
+    free(q->tmp_cfo_estimate);
+  }
   srslte_interp_linear_vector_free(&q->srslte_interp_linvec);
   srslte_interp_linear_free(&q->srslte_interp_lin);
   srslte_interp_linear_free(&q->srslte_interp_lin_mbsfn);
@@ -241,6 +249,10 @@ static float estimate_noise_pilots(srslte_chest_dl_t *q, uint32_t port_id)
 {
   int nref=SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id);
 
+  if (q->average_subframe) {
+    nref /= 4; 
+  }
+
   /* Substract noisy pilot estimates */
   srslte_vec_sub_ccc(q->pilot_estimates_average, q->pilot_estimates, q->tmp_noise, nref);  
   
@@ -305,9 +317,13 @@ static void interpolate_pilots(srslte_chest_dl_t *q, cf_t *pilot_estimates, cf_t
   uint32_t nsymbols = (ch_mode == SRSLTE_SF_MBSFN ) ? srslte_refsignal_mbsfn_nof_symbols() + 1 : srslte_refsignal_cs_nof_symbols(port_id);
   uint32_t fidx_offset = 0;
   /* Interpolate in the frequency domain */
- 
+
+  if (q->average_subframe) {
+    nsymbols = 1;
+  }
+
    // we add one to nsymbols to allow for inclusion of the non-mbms references in the channel estimation       
-  for (l=0;l<(nsymbols);l++) {
+  for (l=0;l<nsymbols;l++) {
     if (ch_mode == SRSLTE_SF_MBSFN) {
       if (l == 0) {
         fidx_offset = srslte_refsignal_cs_fidx(q->cell, l, port_id, 0);
@@ -329,34 +345,41 @@ static void interpolate_pilots(srslte_chest_dl_t *q, cf_t *pilot_estimates, cf_t
   }
  
   /* Now interpolate in the time domain between symbols */
-  if (ch_mode == SRSLTE_SF_MBSFN) {
-    srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(2), &cesymb(1), 2, 1);
-    srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(2), &cesymb(6), &cesymb(3), 4, 3);
-    srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(6), &cesymb(10), &cesymb(7), 4, 3);
-    srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(6), &cesymb(10), &cesymb(10), &cesymb(11), 4, 1);
-   } else {
-    if (SRSLTE_CP_ISNORM(q->cell.cp)) {
-      if (nsymbols == 4) {
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(4),  &cesymb(1), 4, 3);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(4), &cesymb(7),  &cesymb(5), 3, 2);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(7), &cesymb(11), &cesymb(8), 4, 3);
-        srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(7), &cesymb(11), &cesymb(11), &cesymb(12), 4, 2);
-      } else {
-        srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(8), &cesymb(1), &cesymb(1), &cesymb(0), 7, 1);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(8), &cesymb(2), 7, 6);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(8), &cesymb(9), 7, 5);
-      }    
+  if (q->average_subframe) {
+    // If we average per subframe, just copy the estimates in the time domain
+    for (l=0;l<2*SRSLTE_CP_NSYMB(q->cell.cp);l++) {
+      memcpy(&ce[l*SRSLTE_NRE*q->cell.nof_prb], ce, sizeof(cf_t)*SRSLTE_NRE*q->cell.nof_prb);
+    }
+  } else {
+    if (ch_mode == SRSLTE_SF_MBSFN) {
+      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(2), &cesymb(1), 2, 1);
+      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(2), &cesymb(6), &cesymb(3), 4, 3);
+      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(6), &cesymb(10), &cesymb(7), 4, 3);
+      srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(6), &cesymb(10), &cesymb(10), &cesymb(11), 4, 1);
     } else {
-      if (nsymbols == 4) {
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(3), &cesymb(1), 3, 2);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(3), &cesymb(6), &cesymb(4), 3, 2);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(6), &cesymb(9), &cesymb(7), 3, 2);
-        srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(6), &cesymb(9), &cesymb(9), &cesymb(10), 3, 2);
+      if (SRSLTE_CP_ISNORM(q->cell.cp)) {
+        if (nsymbols == 4) {
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(4),  &cesymb(1), 4, 3);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(4), &cesymb(7),  &cesymb(5), 3, 2);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(7), &cesymb(11), &cesymb(8), 4, 3);
+          srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(7), &cesymb(11), &cesymb(11), &cesymb(12), 4, 2);
+        } else {
+          srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(8), &cesymb(1), &cesymb(1), &cesymb(0), 7, 1);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(8), &cesymb(2), 7, 6);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(8), &cesymb(9), 7, 5);
+        }
       } else {
-        srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(7), &cesymb(1), &cesymb(1), &cesymb(0), 6, 1);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(7), &cesymb(2), 6, 5);
-        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(7), &cesymb(8), 6, 4);
-      }    
+        if (nsymbols == 4) {
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(3), &cesymb(1), 3, 2);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(3), &cesymb(6), &cesymb(4), 3, 2);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(6), &cesymb(9), &cesymb(7), 3, 2);
+          srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(6), &cesymb(9), &cesymb(9), &cesymb(10), 3, 2);
+        } else {
+          srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(7), &cesymb(1), &cesymb(1), &cesymb(0), 6, 1);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(7), &cesymb(2), 6, 5);
+          srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(7), &cesymb(8), 6, 4);
+        }
+      }
     }
   }
 }
@@ -392,6 +415,15 @@ static void average_pilots(srslte_chest_dl_t *q, cf_t *input, cf_t *output, uint
   uint32_t nsymbols = (ch_mode == SRSLTE_SF_MBSFN)?srslte_refsignal_mbsfn_nof_symbols(port_id):srslte_refsignal_cs_nof_symbols(port_id);
   uint32_t nref = (ch_mode == SRSLTE_SF_MBSFN)?6*q->cell.nof_prb:2*q->cell.nof_prb;
 
+  // Average in the time domain if enabled
+  if (q->average_subframe) {
+    for (int l=1;l<nsymbols;l++) {
+      srslte_vec_sum_ccc(&input[l*nref], input, input, nref);
+    }
+    srslte_vec_sc_prod_cfc(input, 1.0/((float) nsymbols), input, nref);
+    nsymbols = 1;
+  }
+
   // Average in the frequency domain
   for (int l=0;l<nsymbols;l++) {
     srslte_conv_same_cf(&input[l*nref], q->smooth_filter, &output[l*nref], nref, q->smooth_filter_len);    
@@ -424,11 +456,11 @@ float chest_estimate_cfo(srslte_chest_dl_t *q)
   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_noise[i*npilots/4],
+                             &q->tmp_cfo_estimate[i*npilots/4],
                              npilots/4);
   }
   // Average all angles
-  cf_t sum = srslte_vec_acc_cc(q->tmp_noise, npilots/2);
+  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;
@@ -459,7 +491,7 @@ 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 = chest_estimate_cfo(q);
+    q->cfo = SRSLTE_VEC_EMA(chest_estimate_cfo(q), q->cfo, q->cfo_ema);
   }
     
   /* Compute RSRP for the channel estimates in this port */
@@ -542,8 +574,14 @@ int srslte_chest_dl_estimate_multi_mbsfn(srslte_chest_dl_t *q, cf_t *input[SRSLT
   return SRSLTE_SUCCESS;
 }
 
-void srslte_chest_dl_cfo_estimate_enable(srslte_chest_dl_t *q, bool enable, uint32_t mask)
+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;
 }
diff --git a/lib/src/phy/ue/ue_sync.c b/lib/src/phy/ue/ue_sync.c
index dfd609e8f..58489b92b 100644
--- a/lib/src/phy/ue/ue_sync.c
+++ b/lib/src/phy/ue/ue_sync.c
@@ -48,7 +48,9 @@
 #define DEFAULT_SFO_EMA_COEFF                 0.1
 
 #define DEFAULT_CFO_BW      0.7
-#define DEFAULT_CFO_PSS_TOL 80 // typical accuracy of PSS estimation
+#define DEFAULT_CFO_PSS_TOL 80  // typical accuracy of PSS estimation. Avoids ping-pong effect
+#define DEFAULT_CFO_REF_TOL 5  // typical accuracy of REF estimation
+#define DEFAULT_CFO_REF_MAX 300 // Maximum detection offset of REF based estimation
 
 cf_t dummy_buffer0[15*2048/2];
 cf_t dummy_buffer1[15*2048/2];
@@ -215,6 +217,8 @@ int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q,
 
     q->max_prb = max_prb;
 
+    q->cfo_ref_max = DEFAULT_CFO_REF_MAX;
+    q->cfo_ref_tol = DEFAULT_CFO_REF_TOL;
     q->cfo_pss_tol = DEFAULT_CFO_PSS_TOL;
     q->cfo_loop_bw = DEFAULT_CFO_BW;
     q->cfo_correct_enable = true;
@@ -398,9 +402,11 @@ void srslte_ue_sync_get_last_timestamp(srslte_ue_sync_t *q, srslte_timestamp_t *
   memcpy(timestamp, &q->last_timestamp, sizeof(srslte_timestamp_t));
 }
 
-void srslte_ue_sync_set_cfo_loop_bw(srslte_ue_sync_t *q, float bw, float pss_tol) {
+void srslte_ue_sync_set_cfo_loop_bw(srslte_ue_sync_t *q, float bw, float pss_tol, float ref_tol, float ref_max) {
   q->cfo_loop_bw = bw;
   q->cfo_pss_tol = pss_tol;
+  q->cfo_ref_tol = ref_tol;
+  q->cfo_ref_max = ref_max;
 }
 
 void srslte_ue_sync_set_cfo_ema(srslte_ue_sync_t *q, float ema) {
@@ -408,6 +414,13 @@ void srslte_ue_sync_set_cfo_ema(srslte_ue_sync_t *q, float ema) {
   srslte_sync_set_cfo_ema_alpha(&q->strack, ema);
 }
 
+void srslte_ue_sync_set_cfo_ref(srslte_ue_sync_t *q, float ref_cfo)
+{
+  if (fabsf(ref_cfo)*15000 > q->cfo_ref_tol && fabsf(ref_cfo)*15000 < q->cfo_ref_max) {
+    q->cfo_current_value += ref_cfo*q->cfo_loop_bw;
+  }
+}
+
 uint32_t srslte_ue_sync_get_sfidx(srslte_ue_sync_t *q) {
   return q->sf_idx;    
 }