diff --git a/CMakeLists.txt b/CMakeLists.txt
index fe2d99690..60484ae5a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -66,6 +66,7 @@ configure_file(
 ########################################################################
 option(ENABLE_SRSUE    "Build srsUE application"                  ON)
 option(ENABLE_SRSENB   "Build srsENB application"                 ON)
+option(DISABLE_SIMD    "disable simd instructions"                OFF)
 
 option(ENABLE_GUI      "Enable GUI (using srsGUI)"                ON)
 option(ENABLE_BLADERF  "Enable BladeRF"                           ON)
@@ -287,9 +288,16 @@ if(CMAKE_C_COMPILER_ID MATCHES "GNU" OR CMAKE_C_COMPILER_ID MATCHES "Clang")
   if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "arm")
     set(CMAKE_C_FLAGS  "${CMAKE_C_FLAGS} -mfpu=neon -march=native -DIS_ARM -DHAVE_NEON")
     message(STATUS "have ARM")
+    set(HAVE_NEON "True")
+  else(${CMAKE_SYSTEM_PROCESSOR} MATCHES "arm")
+    set(HAVE_NEON "False")
   endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES "arm")
   set(CMAKE_REQUIRED_FLAGS ${CMAKE_C_FLAGS})
 
+ if(NOT HAVE_SSE AND NOT HAVE_NEON AND NOT DISABLE_SIMD)
+  message(FATAL_ERROR "no SIMD instructions found")
+ endif(NOT HAVE_SSE AND NOT HAVE_NEON AND NOT DISABLE_SIMD)
+
   if(NOT WIN32)
       ADD_CXX_COMPILER_FLAG_IF_AVAILABLE(-fvisibility=hidden HAVE_VISIBILITY_HIDDEN)
   endif(NOT WIN32)
diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index fa5483003..dafff03b9 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -700,7 +700,7 @@ int main(int argc, char **argv) {
   uint32_t sfn; 
   srslte_refsignal_t csr_refs;
   srslte_refsignal_t mbsfn_refs;
-  
+
   srslte_debug_handle_crash(argc, argv);
 
 #ifdef DISABLE_RF
diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 6293d0e70..2b267efde 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -352,7 +352,7 @@ int main(int argc, char **argv) {
   uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
   int sfn_offset;
   float cfo = 0; 
-  
+
   srslte_debug_handle_crash(argc, argv);
 
   parse_args(&prog_args, argc, argv);
@@ -417,8 +417,8 @@ int main(int argc, char **argv) {
         fprintf(stderr, "Error opening rf\n");
         exit(-1);
       }
-      srslte_rf_set_rx_gain(&rf, 50);      
-      cell_detect_config.init_agc = 50; 
+      srslte_rf_set_rx_gain(&rf, srslte_rf_get_rx_gain(&rf));
+      cell_detect_config.init_agc = srslte_rf_get_rx_gain(&rf);
     }
     
     sigset_t sigset;
@@ -546,7 +546,14 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-  srslte_chest_dl_cfo_estimate_enable(&ue_dl.chest, prog_args.enable_cfo_ref, 0xff, 0.005);
+  // Disable CP based CFO estimation during find
+  ue_sync.cfo_current_value = cfo/15000;
+  ue_sync.cfo_is_copied = true;
+  ue_sync.cfo_correct_enable_find = true;
+  srslte_sync_set_cfo_cp_enable(&ue_sync.sfind, false, 0);
+
+
+  srslte_chest_dl_cfo_estimate_enable(&ue_dl.chest, prog_args.enable_cfo_ref, 1023);
   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 */
@@ -595,7 +602,7 @@ int main(int argc, char **argv) {
   bzero(&old_dl_dci, sizeof(srslte_ra_dl_dci_t));
 #endif
 
-  ue_sync.cfo_correct_enable = !prog_args.disable_cfo;
+  ue_sync.cfo_correct_enable_track = !prog_args.disable_cfo;
   
   srslte_pbch_decode_reset(&ue_mib.pbch);
             
diff --git a/lib/include/srslte/common/log.h b/lib/include/srslte/common/log.h
index 9bec779df..93f7657cb 100644
--- a/lib/include/srslte/common/log.h
+++ b/lib/include/srslte/common/log.h
@@ -54,6 +54,12 @@ static const char log_level_text[LOG_LEVEL_N_ITEMS][16] = {"None   ",
                                                            "Info   ",
                                                            "Debug  "};
 
+static const char log_level_text_short[LOG_LEVEL_N_ITEMS][16] = {"[-]",
+                                                                 "[E]",
+                                                                 "[W]",
+                                                                 "[I]",
+                                                                 "[D]"};
+
 class log
 {
 public:
@@ -63,6 +69,8 @@ public:
     tti = 0;
     level = LOG_LEVEL_NONE;
     hex_limit = 0;
+    show_layer_en = true;
+    level_text_short = true;
   }
 
   log(std::string service_name_) {
@@ -70,12 +78,21 @@ public:
     tti = 0;
     level = LOG_LEVEL_NONE;
     hex_limit = 0;
+    show_layer_en = true;
+    level_text_short = true;
   }
 
   // This function shall be called at the start of every tti for printing tti
   void step(uint32_t tti_) {
     tti = tti_;
+    add_string_en  = false;
   }
+
+  void prepend_string(std::string s) {
+    add_string_en  = true;
+    add_string_val = s;
+  }
+
   uint32_t get_tti() {
     return tti;
   }
@@ -93,6 +110,12 @@ public:
   int get_hex_limit() {
     return hex_limit;
   }
+  void set_log_level_short(bool enable) {
+    level_text_short = enable;
+  }
+  void show_layer(bool enable) {
+    show_layer_en = enable;
+  }
 
   // Pure virtual methods for logging
   virtual void console(std::string message, ...) = 0;
@@ -107,18 +130,17 @@ public:
   virtual void info_hex(uint8_t *hex, int size, std::string message, ...){error("info_hex not implemented.\n");}
   virtual void debug_hex(uint8_t *hex, int size, std::string message, ...){error("debug_hex not implemented.\n");}
 
-  // Same with line and file info
-  virtual void error_line(std::string file, int line, std::string message, ...){error("error_line not implemented.\n");}
-  virtual void warning_line(std::string file, int line, std::string message, ...){error("warning_line not implemented.\n");}
-  virtual void info_line(std::string file, int line, std::string message, ...){error("info_line not implemented.\n");}
-  virtual void debug_line(std::string file, int line, std::string message, ...){error("debug_line not implemented.\n");}
-
 protected:
   std::string get_service_name() { return service_name; }
   uint32_t        tti;
   LOG_LEVEL_ENUM  level;
   int             hex_limit;
   std::string     service_name;
+
+  bool        show_layer_en;
+  bool        level_text_short;
+  bool        add_string_en;
+  std::string add_string_val;
 };
 
 } // namespace srslte
diff --git a/lib/include/srslte/common/log_filter.h b/lib/include/srslte/common/log_filter.h
index ae232f135..7e4014190 100644
--- a/lib/include/srslte/common/log_filter.h
+++ b/lib/include/srslte/common/log_filter.h
@@ -37,6 +37,8 @@
 
 #include <stdarg.h>
 #include <string>
+
+#include "srslte/phy/common/timestamp.h"
 #include "srslte/common/log.h"
 #include "srslte/common/logger.h"
 #include "srslte/common/logger_stdout.h"
@@ -66,15 +68,25 @@ public:
   void info_hex(uint8_t *hex, int size, std::string message, ...);
   void debug_hex(uint8_t *hex, int size, std::string message, ...);
 
-  void error_line(std::string file, int line, std::string message, ...);
-  void warning_line(std::string file, int line, std::string message, ...);
-  void info_line(std::string file, int line, std::string message, ...);
-  void debug_line(std::string file, int line, std::string message, ...);
+  class time_itf {
+  public:
+    virtual srslte_timestamp_t get_time() = 0;
+  };
+
+  typedef enum {
+    TIME,
+    EPOCH
+  } time_format_t;
+
+  void set_time_src(time_itf *source, time_format_t format);
 
 private:
   logger *logger_h;
   bool    do_tti;
 
+  time_itf      *time_src;
+  time_format_t time_format;
+
   logger_stdout def_logger_stdout;
 
   void all_log(srslte::LOG_LEVEL_ENUM level, uint32_t tti, char *msg);
diff --git a/lib/include/srslte/common/metrics_hub.h b/lib/include/srslte/common/metrics_hub.h
index a9b77d5f6..ca6d0beac 100644
--- a/lib/include/srslte/common/metrics_hub.h
+++ b/lib/include/srslte/common/metrics_hub.h
@@ -54,7 +54,6 @@ private:
   void run_period() {
     if (m) {
       metrics_t metric;
-      bzero(&metric, sizeof(metrics_t));
       m->get_metrics(metric);
       for (uint32_t i=0;i<listeners.size();i++) {
         listeners[i]->set_metrics(metric);
diff --git a/lib/include/srslte/phy/ch_estimation/chest_dl.h b/lib/include/srslte/phy/ch_estimation/chest_dl.h
index 91fda41e4..c1f5579a1 100644
--- a/lib/include/srslte/phy/ch_estimation/chest_dl.h
+++ b/lib/include/srslte/phy/ch_estimation/chest_dl.h
@@ -79,6 +79,7 @@ typedef struct {
 
   srslte_interp_linsrslte_vec_t srslte_interp_linvec; 
   srslte_interp_lin_t srslte_interp_lin; 
+  srslte_interp_lin_t srslte_interp_lin_3;
   srslte_interp_lin_t srslte_interp_lin_mbsfn;
   float rssi[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS]; 
   float rsrp[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS]; 
@@ -87,7 +88,6 @@ typedef struct {
 
   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];
@@ -153,22 +153,33 @@ SRSLTE_API int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q,
 
 SRSLTE_API void srslte_chest_dl_cfo_estimate_enable(srslte_chest_dl_t *q,
                                                     bool enable,
-                                                    uint32_t mask,
-                                                    float ema);
+                                                    uint32_t mask);
 
 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_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_snr_ant_port(srslte_chest_dl_t *q,
+                                                  uint32_t ant_idx,
+                                                  uint32_t port_idx);
+
 SRSLTE_API float srslte_chest_dl_get_rssi(srslte_chest_dl_t *q);
 
 SRSLTE_API float srslte_chest_dl_get_rsrq(srslte_chest_dl_t *q);
 
+SRSLTE_API float srslte_chest_dl_get_rsrq_ant_port(srslte_chest_dl_t *q,
+                                                   uint32_t ant_idx,
+                                                   uint32_t port);
+
+SRSLTE_API float srslte_chest_dl_get_rsrp_ant_port(srslte_chest_dl_t *q,
+                                                   uint32_t ant_idx,
+                                                   uint32_t port);
+
 SRSLTE_API float srslte_chest_dl_get_rsrp_port(srslte_chest_dl_t *q,
                                                      uint32_t port);
 
diff --git a/lib/include/srslte/phy/common/phy_logger.h b/lib/include/srslte/phy/common/phy_logger.h
new file mode 100644
index 000000000..d02efbb47
--- /dev/null
+++ b/lib/include/srslte/phy/common/phy_logger.h
@@ -0,0 +1,59 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsUE library.
+ *
+ * srsUE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsUE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+/******************************************************************************
+ * File:        phy_logger.h
+ * Description: Interface for logging output
+ *****************************************************************************/
+
+#ifndef PHY_LOGGER_H
+#define PHY_LOGGER_H
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <string.h>
+#include <sys/types.h>
+
+#ifdef __cplusplus
+    extern "C" {
+#endif // __cplusplus
+typedef enum {LOG_LEVEL_INFO, LOG_LEVEL_DEBUG, LOG_LEVEL_ERROR} phy_logger_level_t;
+
+typedef void (*phy_log_handler_t)(phy_logger_level_t log_level, void *ctx, char *str);
+
+void srslte_phy_log_register_handler(void *ctx, phy_log_handler_t handler); 
+
+ void srslte_phy_log_print(phy_logger_level_t log_level, const char *format, ...);
+
+#ifdef __cplusplus
+}
+#endif // C++ 
+
+#endif // LOGGER_H
diff --git a/lib/include/srslte/phy/phch/dci.h b/lib/include/srslte/phy/phch/dci.h
index 6976a6b24..79a79c4df 100644
--- a/lib/include/srslte/phy/phch/dci.h
+++ b/lib/include/srslte/phy/phch/dci.h
@@ -137,7 +137,9 @@ SRSLTE_API srslte_dci_format_t srslte_dci_format_from_string(char *str);
 
 SRSLTE_API char* srslte_dci_format_string(srslte_dci_format_t format);
 
-SRSLTE_API int srslte_dci_location_set(srslte_dci_location_t *c, 
+SRSLTE_API char* srslte_dci_format_string_short(srslte_dci_format_t format);
+
+SRSLTE_API int srslte_dci_location_set(srslte_dci_location_t *c,
                                        uint32_t L, 
                                        uint32_t nCCE);
 
@@ -178,6 +180,15 @@ SRSLTE_API uint32_t srslte_dci_format_sizeof(srslte_dci_format_t format,
                                              uint32_t nof_prb, 
                                              uint32_t nof_ports);
 
+SRSLTE_API uint32_t srslte_dci_dl_info(char *info_str,
+                                    uint32_t str_len,
+                                    srslte_ra_dl_dci_t *dci_msg,
+                                    srslte_dci_format_t format);
+
+SRSLTE_API uint32_t srslte_dci_ul_info(char *info_str,
+                                       uint32_t len,
+                                       srslte_ra_ul_dci_t *dci_msg);
+
 // This is for backwards compatibility only for tm1 formats
 SRSLTE_API uint32_t srslte_dci_format_sizeof_lut(srslte_dci_format_t format, 
                                                  uint32_t nof_prb);
diff --git a/lib/include/srslte/phy/phch/pdcch.h b/lib/include/srslte/phy/phch/pdcch.h
index 9bb896b34..0e0e63335 100644
--- a/lib/include/srslte/phy/phch/pdcch.h
+++ b/lib/include/srslte/phy/phch/pdcch.h
@@ -167,7 +167,15 @@ SRSLTE_API uint32_t srslte_pdcch_ue_locations(srslte_pdcch_t *q,
 SRSLTE_API uint32_t srslte_pdcch_ue_locations_ncce(uint32_t nof_cce, 
                                                    srslte_dci_location_t *c, 
                                                    uint32_t max_candidates, 
-                                                   uint32_t nsubframe, uint16_t rnti); 
+                                                   uint32_t nsubframe,
+                                                   uint16_t rnti);
+
+SRSLTE_API uint32_t srslte_pdcch_ue_locations_ncce_L(uint32_t nof_cce,
+                                                     srslte_dci_location_t *c,
+                                                     uint32_t max_candidates,
+                                                     uint32_t nsubframe,
+                                                     uint16_t rnti,
+                                                     int L);
 
 /* Function for generation of common search space DCI locations */
 SRSLTE_API uint32_t srslte_pdcch_common_locations(srslte_pdcch_t *q, 
diff --git a/lib/include/srslte/phy/sync/cfo.h b/lib/include/srslte/phy/sync/cfo.h
index 5b9b3c0dd..506732b74 100644
--- a/lib/include/srslte/phy/sync/cfo.h
+++ b/lib/include/srslte/phy/sync/cfo.h
@@ -40,9 +40,6 @@
 #include "srslte/config.h"
 #include "srslte/phy/utils/cexptab.h"
 
-/** If the frequency is changed more than the tolerance, a new table is generated */
-#define SRSLTE_CFO_TOLERANCE    0.00001
-
 #define SRSLTE_CFO_CEXPTAB_SIZE 4096
 
 typedef struct SRSLTE_API {
diff --git a/lib/include/srslte/phy/sync/sync.h b/lib/include/srslte/phy/sync/sync.h
index 3146761ce..232bc5fc4 100644
--- a/lib/include/srslte/phy/sync/sync.h
+++ b/lib/include/srslte/phy/sync/sync.h
@@ -102,11 +102,14 @@ typedef struct SRSLTE_API {
   bool cfo_i_initiated;
 
   float cfo_cp_mean;
+  float cfo_pss;
   float cfo_pss_mean;
   int   cfo_i_value;
 
   float cfo_ema_alpha;
 
+  uint32_t cfo_cp_nsymbols;
+
   srslte_cfo_t cfo_corr_frame;
   srslte_cfo_t cfo_corr_symbol;
 
@@ -203,7 +206,9 @@ SRSLTE_API void srslte_sync_copy_cfo(srslte_sync_t *q,
 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);
+                                              bool enable,
+                                              uint32_t nof_symbols);
+
 SRSLTE_API void srslte_sync_set_cfo_pss_enable(srslte_sync_t *q,
                                                bool enable);
 
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index 02341c08d..6974a0b99 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -126,7 +126,9 @@ typedef struct SRSLTE_API {
   srslte_dci_msg_t pending_ul_dci_msg; 
   uint16_t pending_ul_dci_rnti; 
   
-  float sample_offset; 
+  float sample_offset;
+
+  float last_phich_corr;
 }srslte_ue_dl_t;
 
 /* This function shall be called just after the initial synchronization */
diff --git a/lib/include/srslte/phy/ue/ue_sync.h b/lib/include/srslte/phy/ue/ue_sync.h
index 16936c062..233b87b11 100644
--- a/lib/include/srslte/phy/ue/ue_sync.h
+++ b/lib/include/srslte/phy/ue/ue_sync.h
@@ -62,6 +62,16 @@
 #include "srslte/phy/io/filesource.h"
 
 
+#define DEFAULT_CFO_BW_PSS  0.05
+#define DEFAULT_CFO_PSS_MIN 400  // typical bias of PSS estimation.
+#define DEFAULT_CFO_BW_REF  0.08
+#define DEFAULT_CFO_REF_MIN 0    // typical bias of REF estimation
+#define DEFAULT_CFO_REF_MAX DEFAULT_CFO_PSS_MIN  // Maximum detection offset of REF based estimation
+
+#define DEFAULT_PSS_STABLE_TIMEOUT     20  // Time after which the PSS is considered to be stable and we accept REF-CFO
+
+#define DEFAULT_CFO_EMA_TRACK 0.05
+
 typedef enum SRSLTE_API { SF_FIND, SF_TRACK} srslte_ue_sync_state_t;
 
 //#define MEASURE_EXEC_TIME 
@@ -112,7 +122,8 @@ typedef struct SRSLTE_API {
   bool decode_sss_on_track; 
 
   bool  cfo_is_copied;
-  bool  cfo_correct_enable;
+  bool  cfo_correct_enable_track;
+  bool  cfo_correct_enable_find;
   float cfo_current_value;
   float cfo_loop_bw_pss;
   float cfo_loop_bw_ref;
diff --git a/lib/include/srslte/phy/utils/debug.h b/lib/include/srslte/phy/utils/debug.h
index 9405d0865..52290ca11 100644
--- a/lib/include/srslte/phy/utils/debug.h
+++ b/lib/include/srslte/phy/utils/debug.h
@@ -37,6 +37,7 @@
 
 #include <stdio.h>
 #include "srslte/config.h"
+#include "srslte/phy/common/phy_logger.h"
 
 #define SRSLTE_VERBOSE_DEBUG 2
 #define SRSLTE_VERBOSE_INFO  1
@@ -48,6 +49,7 @@ SRSLTE_API void get_time_interval(struct timeval * tdata);
 #define SRSLTE_DEBUG_ENABLED 1
 
 SRSLTE_API extern int srslte_verbose;
+SRSLTE_API extern int handler_registered;
 
 #define SRSLTE_VERBOSE_ISINFO() (srslte_verbose>=SRSLTE_VERBOSE_INFO)
 #define SRSLTE_VERBOSE_ISDEBUG() (srslte_verbose>=SRSLTE_VERBOSE_DEBUG)
@@ -57,17 +59,23 @@ SRSLTE_API extern int srslte_verbose;
 #define PRINT_INFO srslte_verbose=SRSLTE_VERBOSE_INFO
 #define PRINT_NONE srslte_verbose=SRSLTE_VERBOSE_NONE
 
-#define DEBUG(_fmt, ...) if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_DEBUG) \
-  fprintf(stdout, "[DEBUG]: " _fmt, ##__VA_ARGS__)
+#define DEBUG(_fmt, ...) if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_DEBUG && !handler_registered)\
+        {  fprintf(stdout, "[DEBUG]: " _fmt, ##__VA_ARGS__);  }\
+        else{  srslte_phy_log_print(LOG_LEVEL_DEBUG, _fmt, ##__VA_ARGS__); }
 
-#define INFO(_fmt, ...) if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_INFO) \
-  fprintf(stdout, "[INFO]:  " _fmt, ##__VA_ARGS__)
+#define INFO(_fmt, ...) if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_INFO   && !handler_registered) \
+        {  fprintf(stdout, "[INFO]: " _fmt, ##__VA_ARGS__);  }\
+        else{  srslte_phy_log_print(LOG_LEVEL_INFO, _fmt, ##__VA_ARGS__); }
 
 #if CMAKE_BUILD_TYPE==Debug
 /* In debug mode, it prints out the  */
-#define ERROR(_fmt, ...) fprintf(stderr, "\e[31m%s.%d: " _fmt "\e[0m\n", __FILE__, __LINE__, ##__VA_ARGS__)
+#define ERROR(_fmt, ...) if (!handler_registered)\
+    {   fprintf(stderr, "\e[31m%s.%d: " _fmt "\e[0m\n", __FILE__, __LINE__, ##__VA_ARGS__);}\
+        else {srslte_phy_log_print(LOG_LEVEL_ERROR, _fmt, ##__VA_ARGS__);} // 
 #else
-#define ERROR(_fmt, ...) fprintf(stderr, "[ERROR in %s]:" _fmt "\n", __FUNCTION__, ##__VA_ARGS__)
+#define ERROR(_fmt, ...) if (!handler_registered)\
+        {   fprintf(stderr, "[ERROR in %s]:" _fmt "\n", __FUNCTION__, ##__VA_ARGS__);}\
+        else{srslte_phy_log_print(LOG_LEVEL_ERROR, _fmt, ##__VA_ARGS__);} // 
 #endif /* CMAKE_BUILD_TYPE==Debug */
 
 void srslte_debug_handle_crash(int argc, char **argv);
diff --git a/lib/include/srslte/phy/utils/simd.h b/lib/include/srslte/phy/utils/simd.h
index 226e2ecae..7ea203290 100644
--- a/lib/include/srslte/phy/utils/simd.h
+++ b/lib/include/srslte/phy/utils/simd.h
@@ -56,7 +56,7 @@
 /*
  * AVX Macros
  */
-#ifdef LV_HAVE_AVX2
+#ifdef LV_HAVE_AVX
 
 #define _MM256_MULJ_PS(X) _mm256_permute_ps(_MM256_CONJ_PS(X), 0b10110001)
 #define _MM256_CONJ_PS(X) (_mm256_xor_ps(X, _mm256_set_ps(-0.0f, 0.0f, -0.0f, 0.0f, -0.0f, 0.0f, -0.0f, 0.0f)))
@@ -72,7 +72,7 @@
 #define _MM256_PROD_PS(a, b) _mm256_addsub_ps(_mm256_mul_ps(a,_mm256_moveldup_ps(b)),\
                               _mm256_mul_ps(_mm256_shuffle_ps(a,a,0xB1),_mm256_movehdup_ps(b)))
 #endif /* LV_HAVE_FMA */
-#endif /* LV_HAVE_AVX2 */
+#endif /* LV_HAVE_AVX */
 
 
 /*
diff --git a/lib/include/srslte/phy/utils/vector.h b/lib/include/srslte/phy/utils/vector.h
index a4028e495..210d3eef0 100644
--- a/lib/include/srslte/phy/utils/vector.h
+++ b/lib/include/srslte/phy/utils/vector.h
@@ -150,6 +150,10 @@ SRSLTE_API void srslte_vec_abs_square_cf(const cf_t *x, float *abs_square, const
 /* Copy 256 bit aligned vector */
 SRSLTE_API void srs_vec_cf_cpy(const cf_t *src, cf_t *dst, const int len);
 
+SRSLTE_API void srslte_vec_interleave(const cf_t *x, const cf_t *y, cf_t *z, const int len);
+
+SRSLTE_API void srslte_vec_interleave_add(const cf_t *x, const cf_t *y, cf_t *z, const int len);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/lib/include/srslte/phy/utils/vector_simd.h b/lib/include/srslte/phy/utils/vector_simd.h
index 54ac55f98..31725edb3 100644
--- a/lib/include/srslte/phy/utils/vector_simd.h
+++ b/lib/include/srslte/phy/utils/vector_simd.h
@@ -122,6 +122,9 @@ SRSLTE_API void srslte_vec_convert_fi_simd(const float *x, int16_t *z, const flo
 
 SRSLTE_API void srslte_vec_cp_simd(const cf_t *src, cf_t *dst, int len);
 
+SRSLTE_API void srslte_vec_interleave_simd(const cf_t *x, const cf_t *y, cf_t *z, const int len);
+
+SRSLTE_API void srslte_vec_interleave_add_simd(const cf_t *x, const cf_t *y, cf_t *z, const int len);
 
 /* SIMD Find Max functions */
 SRSLTE_API uint32_t srslte_vec_max_fi_simd(const float *x, const int len);
diff --git a/lib/include/srslte/srslte.h b/lib/include/srslte/srslte.h
index f4806b36d..36af629d7 100644
--- a/lib/include/srslte/srslte.h
+++ b/lib/include/srslte/srslte.h
@@ -48,6 +48,7 @@
 #include "srslte/phy/common/timestamp.h"
 #include "srslte/phy/common/sequence.h"
 #include "srslte/phy/common/phy_common.h"
+#include "srslte/phy/common/phy_logger.h"
             
 #include "srslte/phy/ch_estimation/chest_ul.h"
 #include "srslte/phy/ch_estimation/chest_dl.h"
diff --git a/lib/src/common/log_filter.cc b/lib/src/common/log_filter.cc
index 899a224d6..6720c21ad 100644
--- a/lib/src/common/log_filter.cc
+++ b/lib/src/common/log_filter.cc
@@ -37,16 +37,24 @@ namespace srslte{
 
 log_filter::log_filter()
 {
-  do_tti = false; 
+  do_tti      = false;
+  time_src    = NULL;
+  time_format = TIME;
 }
 
 log_filter::log_filter(std::string layer)
 {
+  do_tti      = false;
+  time_src    = NULL;
+  time_format = TIME;
   init(layer, &def_logger_stdout, tti);
 }
 
 log_filter::log_filter(std::string layer, logger *logger_, bool tti)
 {
+  do_tti      = false;
+  time_src    = NULL;
+  time_format = TIME;
   init(layer, logger_, tti);
 }
 
@@ -65,10 +73,20 @@ void log_filter::all_log(srslte::LOG_LEVEL_ENUM level,
     std::stringstream ss;
 
     ss << now_time() << " ";
-    ss << "[" <<get_service_name() << "] ";
-    ss << log_level_text[level] << " ";
-    if(do_tti)
+    if (show_layer_en) {
+      ss << "[" <<get_service_name() << "] ";
+    }
+    if (level_text_short) {
+      ss << log_level_text_short[level] << " ";
+    } else {
+      ss << log_level_text[level] << " ";
+    }
+    if(do_tti) {
       ss << "[" << std::setfill('0') << std::setw(5) << tti << "] ";
+    }
+    if (add_string_en) {
+      ss << add_string_val << " ";
+    }
     ss << msg;
 
     str_ptr s_ptr(new std::string(ss.str()));
@@ -86,11 +104,22 @@ void log_filter::all_log(srslte::LOG_LEVEL_ENUM level,
     std::stringstream ss;
 
     ss << now_time() << " ";
-    ss << "[" <<get_service_name() << "] ";
-    ss << log_level_text[level] << " ";
-    if(do_tti)
-      ss << "[" << std::setfill('0') << std::setw(5) << tti << "] ";
+    if (show_layer_en) {
+      ss << "[" <<get_service_name() << "] ";
+    }
+    if (level_text_short) {
+      ss << log_level_text_short[level] << " ";
+    } else {
+      ss << log_level_text[level] << " ";
+    }
 
+    if(do_tti) {
+      ss << "[" << std::setfill('0') << std::setw(5) << tti << "] ";
+    }
+
+    if (add_string_en) {
+      ss << add_string_val << " ";
+    }
     ss << msg;
     
     if (msg[strlen(msg)-1] != '\n') {
@@ -105,27 +134,6 @@ void log_filter::all_log(srslte::LOG_LEVEL_ENUM level,
   }
 }
 
-void log_filter::all_log_line(srslte::LOG_LEVEL_ENUM level,
-                              uint32_t               tti,
-                              std::string            file,
-                              int                    line,
-                              char                  *msg)
-{
-  if(logger_h) {
-    std::stringstream ss;
-
-    ss << now_time() << " ";
-    ss << "[" <<get_service_name() << "] ";
-    ss << log_level_text[level] << " ";
-    if(do_tti)
-      ss << "[" << std::setfill('0') << std::setw(5) << tti << "] ";
-    ss << msg;
-
-    str_ptr s_ptr(new std::string(ss.str()));
-    logger_h->log(s_ptr);
-  }
-}
-
 void log_filter::console(std::string message, ...) {
   char     *args_msg;
   va_list   args;
@@ -226,75 +234,45 @@ void log_filter::debug_hex(uint8_t *hex, int size, std::string message, ...) {
   }
 }
 
-void log_filter::error_line(std::string file, int line, std::string message, ...)
-{
-  if (level >= LOG_LEVEL_ERROR) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log_line(LOG_LEVEL_ERROR, tti, file, line, args_msg);
-    va_end(args);
-    free(args_msg);
-  }
+void log_filter::set_time_src(time_itf *source, time_format_t format) {
+  this->time_src    = source;
+  this->time_format = format;
 }
 
-void log_filter::warning_line(std::string file, int line, std::string message, ...)
-{
-  if (level >= LOG_LEVEL_WARNING) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log_line(LOG_LEVEL_WARNING, tti, file, line, args_msg);
-    va_end(args);
-    free(args_msg);
-  }
-}
-
-void log_filter::info_line(std::string file, int line, std::string message, ...)
-{
-  if (level >= LOG_LEVEL_INFO) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log_line(LOG_LEVEL_INFO, tti, file, line, args_msg);
-    va_end(args);
-    free(args_msg);
-  }
-}
-
-void log_filter::debug_line(std::string file, int line, std::string message, ...)
-{
-  if (level >= LOG_LEVEL_DEBUG) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log_line(LOG_LEVEL_DEBUG, tti, file, line, args_msg);
-    va_end(args);
-    free(args_msg);
-  }
-}
-
-
-
 std::string log_filter::now_time()
 {
   struct timeval rawtime;
   struct tm * timeinfo;
   char buffer[64];
   char us[16];
-  
-  gettimeofday(&rawtime, NULL);
-  timeinfo = localtime(&rawtime.tv_sec);
-  
-  strftime(buffer,64,"%H:%M:%S",timeinfo);
-  strcat(buffer,".");
-  snprintf(us,16,"%06ld",rawtime.tv_usec);
-  strcat(buffer,us);
-  
+
+  srslte_timestamp_t now;
+  uint64_t usec_epoch;
+
+  if (!time_src) {
+    gettimeofday(&rawtime, NULL);
+    timeinfo = localtime(&rawtime.tv_sec);
+
+    if (time_format == TIME) {
+      strftime(buffer, 64, "%H:%M:%S", timeinfo);
+      strcat(buffer, ".");
+      snprintf(us, 16, "%06ld", rawtime.tv_usec);
+      strcat(buffer, us);
+    } else {
+      usec_epoch = rawtime.tv_sec * 1000000 + rawtime.tv_usec;
+      snprintf(buffer, 64, "%ld", usec_epoch);
+    }
+  } else {
+    now = time_src->get_time();
+
+    if (time_format == TIME) {
+      snprintf(buffer, 64, "%ld:%06u", now.full_secs, (uint32_t) (now.frac_secs * 1e6));
+    } else {
+      usec_epoch = now.full_secs * 1000000 + (uint32_t) (now.frac_secs * 1e6);
+      snprintf(buffer, 64, "%ld", usec_epoch);
+    }
+  }
+
   return std::string(buffer);
 }
 
diff --git a/lib/src/common/pdu_queue.cc b/lib/src/common/pdu_queue.cc
index 6b4c8bfd9..3a116e641 100644
--- a/lib/src/common/pdu_queue.cc
+++ b/lib/src/common/pdu_queue.cc
@@ -25,10 +25,10 @@
  */
 
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 #include "srslte/common/pdu_queue.h"
 
diff --git a/lib/src/common/threads.c b/lib/src/common/threads.c
index ec730d5de..0533aa1b6 100644
--- a/lib/src/common/threads.c
+++ b/lib/src/common/threads.c
@@ -51,6 +51,8 @@ bool threads_new_rt_cpu(pthread_t *thread, void *(*start_routine) (void*), void
   pthread_attr_t attr;
   struct sched_param param;
   cpu_set_t cpuset;
+  bool attr_enable = false;
+
   if (prio_offset >= 0) {
     param.sched_priority = sched_get_priority_max(SCHED_FIFO) - prio_offset;  
     pthread_attr_init(&attr);
@@ -64,6 +66,21 @@ bool threads_new_rt_cpu(pthread_t *thread, void *(*start_routine) (void*), void
       perror("pthread_attr_setschedparam");
       fprintf(stderr, "Error not enough privileges to set Scheduling priority\n");
     }
+    attr_enable = true;
+  } else if (prio_offset == -2) {
+    param.sched_priority = 0;
+    pthread_attr_init(&attr);
+    if (pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED)) {
+      perror("pthread_attr_setinheritsched");
+    }
+    if (pthread_attr_setschedpolicy(&attr, SCHED_OTHER)) {
+      perror("pthread_attr_setschedpolicy");
+    }
+    if (pthread_attr_setschedparam(&attr, &param)) {
+      perror("pthread_attr_setschedparam");
+      fprintf(stderr, "Error not enough privileges to set Scheduling priority\n");
+    }
+    attr_enable = true;
   }
   if(cpu > 0) {
     if(cpu > 50) {
@@ -86,7 +103,7 @@ bool threads_new_rt_cpu(pthread_t *thread, void *(*start_routine) (void*), void
     }
   }
 
-  int err = pthread_create(thread, prio_offset >= 0 ? &attr : NULL, start_routine, arg);
+  int err = pthread_create(thread, attr_enable ? &attr : NULL, start_routine, arg);
   if (err) {
     if (EPERM == err) {
       perror("Warning: Failed to create thread with real-time priority. Creating it with normal priority");
@@ -102,7 +119,7 @@ bool threads_new_rt_cpu(pthread_t *thread, void *(*start_routine) (void*), void
   } else {
     ret = true; 
   }
-  if (prio_offset >= 0) {
+  if (attr_enable) {
     pthread_attr_destroy(&attr);
   }
   return ret; 
diff --git a/lib/src/phy/ch_estimation/chest_dl.c b/lib/src/phy/ch_estimation/chest_dl.c
index 35122958d..d63933ced 100644
--- a/lib/src/phy/ch_estimation/chest_dl.c
+++ b/lib/src/phy/ch_estimation/chest_dl.c
@@ -141,6 +141,11 @@ int srslte_chest_dl_init(srslte_chest_dl_t *q, uint32_t max_prb)
       goto clean_exit; 
     }
 
+    if (srslte_interp_linear_init(&q->srslte_interp_lin_3, 4*max_prb, SRSLTE_NRE/4)) {
+      fprintf(stderr, "Error initializing interpolator\n");
+      goto clean_exit;
+    }
+
     if (srslte_interp_linear_init(&q->srslte_interp_lin_mbsfn, 6*max_prb, SRSLTE_NRE/6)) {
       fprintf(stderr, "Error initializing interpolator\n");
       goto clean_exit;
@@ -185,6 +190,7 @@ void srslte_chest_dl_free(srslte_chest_dl_t *q)
   }
   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_3);
   srslte_interp_linear_free(&q->srslte_interp_lin_mbsfn);
   if (q->pilot_estimates) {
     free(q->pilot_estimates);
@@ -238,6 +244,11 @@ int srslte_chest_dl_set_cell(srslte_chest_dl_t *q, srslte_cell_t cell)
         return SRSLTE_ERROR;
       }
 
+      if (srslte_interp_linear_resize(&q->srslte_interp_lin_3, 4 * q->cell.nof_prb, SRSLTE_NRE / 4)) {
+        fprintf(stderr, "Error initializing interpolator\n");
+        return SRSLTE_ERROR;
+      }
+
     }
     ret = SRSLTE_SUCCESS;
   }
@@ -245,12 +256,16 @@ int srslte_chest_dl_set_cell(srslte_chest_dl_t *q, srslte_cell_t cell)
 }
 
 /* Uses the difference between the averaged and non-averaged pilot estimates */
-static float estimate_noise_pilots(srslte_chest_dl_t *q, uint32_t port_id) 
+static float estimate_noise_pilots(srslte_chest_dl_t *q, uint32_t port_id, srslte_sf_t ch_mode)
 {
   int nref=SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id);
 
   if (q->average_subframe) {
-    nref /= 4; 
+    if (ch_mode == SRSLTE_SF_MBSFN) {
+      nref /= 4;
+    } else {
+      nref /= 2;
+    }
   }
 
   /* Substract noisy pilot estimates */
@@ -337,10 +352,18 @@ static void interpolate_pilots(srslte_chest_dl_t *q, cf_t *pilot_estimates, cf_t
                                     fidx_offset, SRSLTE_NRE/6-fidx_offset);
       }
     } else {
-      fidx_offset = srslte_refsignal_cs_fidx(q->cell, l, port_id, 0);
-      srslte_interp_linear_offset(&q->srslte_interp_lin, &pilot_estimates[2*q->cell.nof_prb*l],
-                                  &ce[srslte_refsignal_cs_nsymbol(l,q->cell.cp, port_id) * q->cell.nof_prb * SRSLTE_NRE], 
-                                  fidx_offset, SRSLTE_NRE/2-fidx_offset); 
+      if (q->average_subframe) {
+        fidx_offset = SRSLTE_MIN(srslte_refsignal_cs_fidx(q->cell, 0, port_id, 0),
+                                 srslte_refsignal_cs_fidx(q->cell, 1, port_id, 0));
+        srslte_interp_linear_offset(&q->srslte_interp_lin_3, &pilot_estimates[q->cell.nof_prb * l],
+                                    &ce[srslte_refsignal_cs_nsymbol(l, q->cell.cp, port_id) * q->cell.nof_prb
+                                        * SRSLTE_NRE], fidx_offset, SRSLTE_NRE / 4 - fidx_offset);
+      } else {
+        fidx_offset = srslte_refsignal_cs_fidx(q->cell, l, port_id, 0);
+        srslte_interp_linear_offset(&q->srslte_interp_lin, &pilot_estimates[2 * q->cell.nof_prb * l],
+                                    &ce[srslte_refsignal_cs_nsymbol(l, q->cell.cp, port_id) * q->cell.nof_prb
+                                        * SRSLTE_NRE], fidx_offset, SRSLTE_NRE / 2 - fidx_offset);
+      }
     }  
   }
  
@@ -417,11 +440,31 @@ static void average_pilots(srslte_chest_dl_t *q, cf_t *input, cf_t *output, uint
 
   // 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);
+    if (ch_mode == SRSLTE_SF_MBSFN) {
+      for (int l = 1; l < nsymbols; l++) {
+        srslte_vec_sum_ccc(&input[l * nref], input, input, nref);
+      }
+      srslte_vec_sc_prod_cfc(input, 1.0f / ((float) nsymbols), input, nref);
+      nsymbols = 1;
+    } else {
+      cf_t *temp = output; // Use ouput as temporal buffer
+
+      if (srslte_refsignal_cs_fidx(q->cell, 0, port_id, 0) < 3) {
+        srslte_vec_interleave(input, &input[nref], temp, nref);
+        for (int l = 2; l < nsymbols - 1; l += 2) {
+          srslte_vec_interleave_add(&input[l * nref], &input[(l + 1) * nref], temp, nref);
+        }
+      } else {
+        srslte_vec_interleave(&input[nref], input, temp, nref);
+        for (int l = 2; l < nsymbols - 1; l += 2) {
+          srslte_vec_interleave_add(&input[(l + 1) * nref], &input[l * nref], temp, nref);
+        }
+      }
+      nref *= 2;
+      srslte_vec_sc_prod_cfc(temp, 2.0f / (float) nsymbols, input, nref);
+
+      nsymbols = 1;
     }
-    srslte_vec_sc_prod_cfc(input, 1.0/((float) nsymbols), input, nref);
-    nsymbols = 1;
   }
 
   // Average in the frequency domain
@@ -467,7 +510,11 @@ float chest_estimate_cfo(srslte_chest_dl_t *q)
 }
 
 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) {
+  if (q->cfo_estimate_enable && ((1<<sf_idx) & q->cfo_estimate_sf_mask)) {
+    q->cfo = chest_estimate_cfo(q);
+  }
+
+  if (ce != NULL) {
     /* Smooth estimates (if applicable) and interpolate */
     if (q->smooth_filter_len == 0 || (q->smooth_filter_len == 3 && q->smooth_filter[0] == 0)) {
       interpolate_pilots(q, q->pilot_estimates, ce, port_id, ch_mode);
@@ -478,7 +525,7 @@ void chest_interpolate_noise_est(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, ui
   
     /* Estimate noise power */
     if (q->noise_alg == SRSLTE_NOISE_ALG_REFS && q->smooth_filter_len > 0) {
-      q->noise_estimate[rxant_id][port_id] = estimate_noise_pilots(q, port_id);                  
+      q->noise_estimate[rxant_id][port_id] = estimate_noise_pilots(q, port_id, ch_mode);
     } else if (q->noise_alg == SRSLTE_NOISE_ALG_PSS) {
       if (sf_idx == 0 || sf_idx == 5) {
         q->noise_estimate[rxant_id][port_id] = estimate_noise_pss(q, input, ce);
@@ -490,18 +537,11 @@ 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);
   float energy = cabsf(srslte_vec_acc_cc(q->pilot_estimates, npilots)/npilots);
   q->rsrp[rxant_id][port_id] = energy*energy;
-  if (port_id == 0) {
-    /* compute rssi only for port 0 */
-    q->rssi[rxant_id][port_id] = srslte_chest_dl_rssi(q, input, port_id);     
-  }
+  q->rssi[rxant_id][port_id] = srslte_chest_dl_rssi(q, input, port_id);
 }
 
 int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, uint32_t sf_idx, uint32_t port_id, uint32_t rxant_id) 
@@ -581,9 +621,8 @@ 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)
+void srslte_chest_dl_cfo_estimate_enable(srslte_chest_dl_t *q, bool enable, uint32_t mask)
 {
-  q->cfo_ema              = ema;
   q->cfo_estimate_enable  = enable;
   q->cfo_estimate_sf_mask = mask;
 }
@@ -605,10 +644,21 @@ float srslte_chest_dl_get_snr(srslte_chest_dl_t *q) {
   int nref=SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, 0);
   return srslte_vec_acc_ff(q->snr_vector, nref)/nref; 
 #else
-  return srslte_chest_dl_get_rsrp(q)/srslte_chest_dl_get_noise_estimate(q);
+  float rsrp  = 0;
+  for (int i=0;i<q->last_nof_antennas;i++) {
+    for (int j=0;j<q->cell.nof_ports;j++) {
+      rsrp += q->rsrp[i][j]/q->cell.nof_ports;
+    }
+  }
+  return rsrp/srslte_chest_dl_get_noise_estimate(q);
 #endif
 }
 
+
+float srslte_chest_dl_get_snr_ant_port(srslte_chest_dl_t *q, uint32_t ant_idx, uint32_t port_idx) {
+  return srslte_chest_dl_get_rsrp_ant_port(q, ant_idx, port_idx)/srslte_chest_dl_get_noise_estimate(q);
+}
+
 float srslte_chest_dl_get_rssi(srslte_chest_dl_t *q) {
   float n = 0; 
   for (int i=0;i<q->last_nof_antennas;i++) {
@@ -629,6 +679,14 @@ float srslte_chest_dl_get_rsrq(srslte_chest_dl_t *q) {
   
 }
 
+float srslte_chest_dl_get_rsrq_ant_port(srslte_chest_dl_t *q, uint32_t ant_idx, uint32_t port_idx) {
+  return q->cell.nof_prb*q->rsrp[ant_idx][port_idx] / q->rssi[ant_idx][port_idx];
+}
+
+float srslte_chest_dl_get_rsrp_ant_port(srslte_chest_dl_t *q, uint32_t ant_idx, uint32_t port) {
+  return q->rsrp[ant_idx][port];
+}
+
 float srslte_chest_dl_get_rsrp_port(srslte_chest_dl_t *q, uint32_t port) {
   float n = 0;
   for (int i = 0; i < q->last_nof_antennas; i++) {
diff --git a/lib/src/phy/ch_estimation/test/CMakeLists.txt b/lib/src/phy/ch_estimation/test/CMakeLists.txt
index 8e1208ef5..c02c39043 100644
--- a/lib/src/phy/ch_estimation/test/CMakeLists.txt
+++ b/lib/src/phy/ch_estimation/test/CMakeLists.txt
@@ -39,10 +39,10 @@ add_test(chest_test_dl_cellid2 chest_test_dl -c 2 -r 50)
 ########################################################################
 
 add_executable(chest_test_ul chest_test_ul.c)
-target_link_libraries(chest_test_ul srslte_phy)
+target_link_libraries(chest_test_ul srslte_phy srslte_common)
 
 add_executable(refsignal_ul_test_all refsignal_ul_test.c)
-target_link_libraries(refsignal_ul_test_all srslte_phy)
+target_link_libraries(refsignal_ul_test_all srslte_phy srslte_common)
 
 add_test(chest_test_ul_cellid0 chest_test_ul -c 0 -r 50) 
 add_test(chest_test_ul_cellid1 chest_test_ul -c 1 -r 50) 
diff --git a/lib/src/phy/common/phy_logger.c b/lib/src/phy/common/phy_logger.c
new file mode 100644
index 000000000..09393236a
--- /dev/null
+++ b/lib/src/phy/common/phy_logger.c
@@ -0,0 +1,61 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsUE library.
+ *
+ * srsUE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsUE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <errno.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <string.h>
+#include <sys/types.h>
+#include <stdarg.h>
+#include "srslte/srslte.h"
+#include "srslte/phy/common/phy_logger.h"
+/*********************************************************************
+    Functions for external logging 
+*********************************************************************/
+static phy_log_handler_t phy_log_handler; 
+static void *callback_ctx = NULL; 
+
+void srslte_phy_log_register_handler(void *ctx, phy_log_handler_t handler) {
+  phy_log_handler  = handler; 
+  callback_ctx = ctx; 
+  handler_registered++;
+}
+
+ void srslte_phy_log_print(phy_logger_level_t log_level, const char *format, ...) {
+  va_list   args;
+  va_start(args, format);
+  if (phy_log_handler) {
+    char *args_msg = NULL;
+    if(vasprintf(&args_msg, format, args) > 0) {
+      phy_log_handler(log_level, callback_ctx, args_msg);
+    }
+    if (args_msg) {
+      free(args_msg); 
+    }
+  } 
+  va_end(args);
+}
\ No newline at end of file
diff --git a/lib/src/phy/fec/test/viterbi_test.h b/lib/src/phy/fec/test/viterbi_test.h
index 7d94e7c7e..38665305e 100644
--- a/lib/src/phy/fec/test/viterbi_test.h
+++ b/lib/src/phy/fec/test/viterbi_test.h
@@ -52,7 +52,24 @@ static expected_errors_t expected_errors[] = {
     {-1, -1, -1, true, -1.0, -1}
 };
 
-#else
+#elif HAVE_NEON
+
+static expected_errors_t expected_errors[] = {
+    {1000, 1, 40, true, 0.0, 7282},
+    {1000, 1, 40, true, 2.0, 725},
+    {1000, 1, 40, true, 3.0, 176},
+    {1000, 1, 40, true, 4.5, 24},
+
+    {100, 1, 1000, true, 0.0, 13208},
+    {100, 1, 1000, true, 2.0, 939},
+    {100, 1, 1000, true, 3.0, 110},
+    {100, 1, 1000, true, 4.5, 5},
+
+    {-1, -1, -1, true, -1.0, -1}
+};
+
+
+#else 
 
 static expected_errors_t expected_errors[] = {
     {1000, 1, 40, true, 0.0, 5363},
diff --git a/lib/src/phy/fec/turbodecoder.c b/lib/src/phy/fec/turbodecoder.c
index 7803fde1b..54a0ed109 100644
--- a/lib/src/phy/fec/turbodecoder.c
+++ b/lib/src/phy/fec/turbodecoder.c
@@ -95,7 +95,7 @@ void srslte_tdec_iteration_par(srslte_tdec_t * h, int16_t* input[SRSLTE_TDEC_MAX
 #ifdef LV_HAVE_SSE
   srslte_tdec_simd_iteration(&h->tdec_simd, input, long_cb);      
 #else
-  srslte_vec_convert_if(input[0], h->input_conv, 0.01, 3*long_cb+12);
+  srslte_vec_convert_if(input[0], 0.01, h->input_conv, 3*long_cb+12);
   srslte_tdec_gen_iteration(&h->tdec_gen, h->input_conv, long_cb);
 #endif
 }
@@ -156,7 +156,7 @@ int srslte_tdec_run_all_par(srslte_tdec_t * h, int16_t * input[SRSLTE_TDEC_MAX_N
 #ifdef LV_HAVE_SSE
   return srslte_tdec_simd_run_all(&h->tdec_simd, input, output, nof_iterations, long_cb);  
 #else
-  srslte_vec_convert_if(input[0], h->input_conv, 0.01, 3*long_cb+12);
+  srslte_vec_convert_if(input[0], 0.01, h->input_conv, 3*long_cb+12);
   return srslte_tdec_gen_run_all(&h->tdec_gen, h->input_conv, output[0], nof_iterations, long_cb);
 #endif
 }
diff --git a/lib/src/phy/phch/dci.c b/lib/src/phy/phch/dci.c
index f252d003b..7d4ccd1ea 100644
--- a/lib/src/phy/phch/dci.c
+++ b/lib/src/phy/phch/dci.c
@@ -360,6 +360,101 @@ uint32_t dci_format2B_sizeof(uint32_t nof_prb, uint32_t nof_ports) {
   
 }
 
+uint32_t srslte_dci_dl_info(char *info_str, uint32_t len, srslte_ra_dl_dci_t *dci_msg, srslte_dci_format_t format)
+{
+  int n = 0;
+
+  switch(dci_msg->alloc_type) {
+    case SRSLTE_RA_ALLOC_TYPE0:
+      n += snprintf(&info_str[n], len-n, "type0={rbg=0x%x}, ", dci_msg->type0_alloc.rbg_bitmask);
+      break;
+    case SRSLTE_RA_ALLOC_TYPE1:
+      n += snprintf(&info_str[n], len-n, "type1={vrb=0x%x, rbg_s=%d, sh=%d}, ",
+                    dci_msg->type1_alloc.vrb_bitmask, dci_msg->type1_alloc.rbg_subset, dci_msg->type1_alloc.shift);
+      break;
+    case SRSLTE_RA_ALLOC_TYPE2:
+      n += snprintf(&info_str[n], len-n, "type2={riv=%d, rb=(%d,%d), mode=%s",
+                    dci_msg->type2_alloc.riv,
+                    dci_msg->type2_alloc.RB_start, dci_msg->type2_alloc.RB_start+dci_msg->type2_alloc.L_crb-1,
+                    dci_msg->type2_alloc.mode==SRSLTE_RA_TYPE2_LOC?"local":"dist");
+      if (dci_msg->type2_alloc.mode==SRSLTE_RA_TYPE2_LOC) {
+        n += snprintf(&info_str[n], len-n, ", ngap=%s, nprb1a=%d",
+                      dci_msg->type2_alloc.n_gap==SRSLTE_RA_TYPE2_NG1?"ng1":"ng2",
+                      dci_msg->type2_alloc.n_prb1a==SRSLTE_RA_TYPE2_NPRB1A_2?2:3);
+      }
+      n += snprintf(&info_str[n], len-n, "}, ");
+      break;
+  }
+  n += snprintf(&info_str[n], len-n, "pid=%d, ", dci_msg->harq_process);
+
+  n += snprintf(&info_str[n], len-n, "mcs={");
+  if (dci_msg->tb_en[0]) {
+    n += snprintf(&info_str[n], len-n, "%d", dci_msg->mcs_idx);
+    if (dci_msg->tb_en[1]) {
+      n += snprintf(&info_str[n], len-n, ",");
+    } else {
+      n += snprintf(&info_str[n], len-n, "}, ");
+    }
+  }
+  if (dci_msg->tb_en[1]) {
+    n += snprintf(&info_str[n], len - n, "%d}, ", dci_msg->mcs_idx_1);
+  }
+  n += snprintf(&info_str[n], len-n, "rv={");
+  if (dci_msg->tb_en[0]) {
+    n += snprintf(&info_str[n], len-n, "%d", dci_msg->rv_idx);
+    if (dci_msg->tb_en[1]) {
+      n += snprintf(&info_str[n], len-n, ",");
+    } else {
+      n += snprintf(&info_str[n], len-n, "}, ");
+    }
+  }
+  if (dci_msg->tb_en[1]) {
+    n += snprintf(&info_str[n], len - n, "%d}, ", dci_msg->rv_idx_1);
+  }
+  n += snprintf(&info_str[n], len-n, "ndi={");
+  if (dci_msg->tb_en[0]) {
+    n += snprintf(&info_str[n], len-n, "%d", dci_msg->ndi);
+    if (dci_msg->tb_en[1]) {
+      n += snprintf(&info_str[n], len-n, ",");
+    } else {
+      n += snprintf(&info_str[n], len-n, "}, ");
+    }
+  }
+  if (dci_msg->tb_en[1]) {
+    n += snprintf(&info_str[n], len - n, "%d}, ", dci_msg->ndi_1);
+  }
+
+  if (format == SRSLTE_DCI_FORMAT1 || format == SRSLTE_DCI_FORMAT1A || format == SRSLTE_DCI_FORMAT1B) {
+    n += snprintf(&info_str[n], len-n, "tpc_pucch=%d, ", dci_msg->tpc_pucch);
+  }
+  if (format == SRSLTE_DCI_FORMAT2 || format == SRSLTE_DCI_FORMAT2A || format == SRSLTE_DCI_FORMAT2B) {
+    n += snprintf(&info_str[n], len-n, "tb_sw=%d, pinfo=%d, ", dci_msg->tb_cw_swap, dci_msg->pinfo);
+  }
+  return n;
+}
+
+uint32_t srslte_dci_ul_info(char *info_str, uint32_t len, srslte_ra_ul_dci_t *dci_msg)
+{
+  int n = 0;
+
+  n += snprintf(&info_str[n], len-n, "riv=%d, rb=(%d,%d), ", dci_msg->type2_alloc.riv,
+                dci_msg->type2_alloc.RB_start, dci_msg->type2_alloc.RB_start+dci_msg->type2_alloc.L_crb-1);
+
+  switch(dci_msg->freq_hop_fl) {
+    case SRSLTE_RA_PUSCH_HOP_DISABLED:
+      n += snprintf(&info_str[n], len-n, "f_h=n/a, ");
+      break;
+    default:
+      n += snprintf(&info_str[n], len-n, "f_h=%d, ", dci_msg->freq_hop_fl);
+      break;
+  }
+  n += snprintf(&info_str[n], len-n, "mcs=%d, rv=%d, ndi=%d, ", dci_msg->mcs_idx, dci_msg->rv_idx, dci_msg->ndi);
+  n += snprintf(&info_str[n], len-n, "tpc_pusch=%d, dmrs_cs=%d, cqi=%s, ",
+                dci_msg->tpc_pusch, dci_msg->n_dmrs, dci_msg->cqi_request?"yes":"no");
+
+  return n;
+}
+
 uint32_t srslte_dci_format_sizeof(srslte_dci_format_t format, uint32_t nof_prb, uint32_t nof_ports) {
   switch (format) {
   case SRSLTE_DCI_FORMAT0:
@@ -1302,6 +1397,32 @@ char* srslte_dci_format_string(srslte_dci_format_t format) {
   }
 }
 
+
+char* srslte_dci_format_string_short(srslte_dci_format_t format) {
+  switch (format) {
+    case SRSLTE_DCI_FORMAT0:
+      return "0";
+    case SRSLTE_DCI_FORMAT1:
+      return "1";
+    case SRSLTE_DCI_FORMAT1A:
+      return "1A";
+    case SRSLTE_DCI_FORMAT1B:
+      return "1B";
+    case SRSLTE_DCI_FORMAT1C:
+      return "1C";
+    case SRSLTE_DCI_FORMAT1D:
+      return "1D";
+    case SRSLTE_DCI_FORMAT2:
+      return "2";
+    case SRSLTE_DCI_FORMAT2A:
+      return "2A";
+    case SRSLTE_DCI_FORMAT2B:
+      return "2B";
+    default:
+      return "N/A"; // fatal error
+  }
+}
+
 void srslte_dci_msg_type_fprint(FILE *f, srslte_dci_msg_type_t type) {
   switch (type.type) {
   case SRSLTE_DCI_MSG_TYPE_PUSCH_SCHED:
diff --git a/lib/src/phy/phch/pdcch.c b/lib/src/phy/phch/pdcch.c
index 58d5bf6db..ca8480bc6 100644
--- a/lib/src/phy/phch/pdcch.c
+++ b/lib/src/phy/phch/pdcch.c
@@ -218,13 +218,20 @@ uint32_t srslte_pdcch_ue_locations(srslte_pdcch_t *q, srslte_dci_location_t *c,
   return srslte_pdcch_ue_locations_ncce(q->nof_cce, c, max_candidates, nsubframe, rnti);
 }
 
+
+uint32_t srslte_pdcch_ue_locations_ncce(uint32_t nof_cce, srslte_dci_location_t *c, uint32_t max_candidates,
+                                        uint32_t nsubframe, uint16_t rnti)
+{
+  return srslte_pdcch_ue_locations_ncce_L(nof_cce, c, max_candidates, nsubframe, rnti, -1);
+}
+
 /** 36.213 v9.1.1 
  * Computes up to max_candidates UE-specific candidates for DCI messages and saves them 
  * in the structure pointed by c.
  * Returns the number of candidates saved in the array c.   
  */
-uint32_t srslte_pdcch_ue_locations_ncce(uint32_t nof_cce, srslte_dci_location_t *c, uint32_t max_candidates,
-                        uint32_t nsubframe, uint16_t rnti) {
+uint32_t srslte_pdcch_ue_locations_ncce_L(uint32_t nof_cce, srslte_dci_location_t *c, uint32_t max_candidates,
+                        uint32_t nsubframe, uint16_t rnti, int Ls) {
   
   int l; // this must be int because of the for(;;--) loop
   uint32_t i, k, L, m; 
@@ -241,24 +248,26 @@ uint32_t srslte_pdcch_ue_locations_ncce(uint32_t nof_cce, srslte_dci_location_t
   // All aggregation levels from 8 to 1
   for (l = 3; l >= 0; l--) {
     L = (1 << l);
-    // For all candidates as given in table 9.1.1-1
-    for (i = 0; i < nof_candidates[l]; i++) {
-      if (nof_cce >= L) {
-        ncce = L * ((Yk + i) % (nof_cce / L));
-        // Check if candidate fits in c vector and in CCE region
-        if (k < max_candidates  && ncce + L <= nof_cce) 
-        {            
-          c[k].L = l;
-          c[k].ncce = ncce;
-          
-          DEBUG("UE-specific SS Candidate %d: nCCE: %d, L: %d\n",
-              k, c[k].ncce, c[k].L);            
+    if (Ls<0 || Ls==L) {
+      // For all candidates as given in table 9.1.1-1
+      for (i = 0; i < nof_candidates[l]; i++) {
+        if (nof_cce >= L) {
+          ncce = L * ((Yk + i) % (nof_cce / L));
+          // Check if candidate fits in c vector and in CCE region
+          if (k < max_candidates  && ncce + L <= nof_cce)
+          {
+            c[k].L = l;
+            c[k].ncce = ncce;
 
-          k++;          
-        } 
+            DEBUG("UE-specific SS Candidate %d: nCCE: %d, L: %d\n",
+                  k, c[k].ncce, c[k].L);
+
+            k++;
+          }
+        }
       }
     }
-  }    
+  }
 
   DEBUG("Initiated %d candidate(s) in the UE-specific search space for C-RNTI: 0x%x, nsubframe=%d, nof_cce=%d\n", 
          k, rnti, nsubframe, nof_cce);
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index cdaa4ad80..8c57d5ac9 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -702,6 +702,7 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
     // Pre-decoder
     if (srslte_predecoding_type(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
                                       cfg->codebook_idx, cfg->nbits[0].nof_re, cfg->mimo_type, pdsch_scaling, noise_estimate)<0) {
+      printf("Error predecoding\n");
       return -1;
     }
 
diff --git a/lib/src/phy/phch/ra.c b/lib/src/phy/phch/ra.c
index 1f6ac7a18..e1eaa5de2 100644
--- a/lib/src/phy/phch/ra.c
+++ b/lib/src/phy/phch/ra.c
@@ -240,7 +240,6 @@ int srslte_ra_ul_dci_to_grant(srslte_ra_ul_dci_t *dci, uint32_t nof_prb, uint32_
     grant->Qm = srslte_mod_bits_x_symbol(grant->mcs.mod);
 
   } else {
-    printf("Error computing UL PRB allocation\n");
     return SRSLTE_ERROR;
   }
   return SRSLTE_SUCCESS;
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index b78527ddd..a2da3195c 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -493,11 +493,7 @@ static int decode_tb(srslte_sch_t *q,
                ((uint32_t) data[cb_segm->tbs/8+1])<<8   | 
                ((uint32_t) data[cb_segm->tbs/8+2]);
 
-      if (!par_rx) {
-        INFO("Warning: Received all-zero transport block\n\n",0);      
-      }
-
-      if (par_rx == par_tx) {
+      if (par_rx == par_tx && par_rx) {
         INFO("TB decoded OK\n",0);
         return SRSLTE_SUCCESS;
       } else {
diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index 75482eaa1..0d289535e 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -50,12 +50,13 @@ typedef struct {
   double tx_rate;
   bool dynamic_rate; 
   bool has_rssi; 
-  uhd_sensor_value_handle rssi_value;
   uint32_t nof_rx_channels;
   int nof_tx_channels;
 
   srslte_rf_error_handler_t uhd_error_handler; 
-  
+
+  float current_master_clock;
+
   bool async_thread_running; 
   pthread_t async_thread; 
 } rf_uhd_handler_t;
@@ -229,7 +230,7 @@ int rf_uhd_start_rx_stream(void *h, bool now)
   };
   if (!now) {
     uhd_usrp_get_time_now(handler->usrp, 0, &stream_cmd.time_spec_full_secs, &stream_cmd.time_spec_frac_secs);
-    stream_cmd.time_spec_frac_secs += 0.1;
+    stream_cmd.time_spec_frac_secs += 0.2;
     if (stream_cmd.time_spec_frac_secs > 1) {
       stream_cmd.time_spec_frac_secs -= 1;
       stream_cmd.time_spec_full_secs += 1;
@@ -279,10 +280,15 @@ bool get_has_rssi(void *h) {
 float rf_uhd_get_rssi(void *h) {
   rf_uhd_handler_t *handler = (rf_uhd_handler_t*) h;  
   if (handler->has_rssi) {
-    double val_out; 
-    uhd_usrp_get_rx_sensor(handler->usrp, "rssi", 0, &handler->rssi_value);
-    uhd_sensor_value_to_realnum(handler->rssi_value, &val_out);
-    return val_out; 
+    double val_out;
+
+    uhd_sensor_value_handle rssi_value;
+    uhd_sensor_value_make_from_realnum(&rssi_value, "rssi", 0, "dBm", "%f");
+    uhd_usrp_get_rx_sensor(handler->usrp, "rssi", 0, &rssi_value);
+    uhd_sensor_value_to_realnum(rssi_value, &val_out);
+    uhd_sensor_value_free(&rssi_value);
+
+    return val_out;
   } else {
     return 0.0;
   }
@@ -410,23 +416,27 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
       if (find_string(devices_str, "type=b200") && !strstr(args, "recv_frame_size")) {
         // If B200 is available, use it
         args = "type=b200,master_clock_rate=30.72e6";
+        handler->current_master_clock = 30720000;
         handler->devname = DEVNAME_B200;
       } else if (find_string(devices_str, "type=x300")) {
         // Else if X300 is available, set master clock rate now (can't be changed later)
         args = "type=x300,master_clock_rate=184.32e6";
-        handler->dynamic_rate = false; 
+        handler->current_master_clock = 184320000;
+        handler->dynamic_rate = false;
         handler->devname = DEVNAME_X300;
       }
     } else {
       // If args is set and x300 type is specified, make sure master_clock_rate is defined
       if (strstr(args, "type=x300") && !strstr(args, "master_clock_rate")) {
         sprintf(args2, "%s,master_clock_rate=184.32e6",args);
-        args = args2;          
-        handler->dynamic_rate = false; 
+        args = args2;
+        handler->current_master_clock = 184320000;
+        handler->dynamic_rate = false;
         handler->devname = DEVNAME_X300;
-      } else if (strstr(args, "type=b200")) {
+      } else {
         snprintf(args2, sizeof(args2), "%s,master_clock_rate=30.72e6", args);
         args = args2;
+        handler->current_master_clock = 30720000;
         handler->devname = DEVNAME_B200;
       }
     }        
@@ -434,11 +444,7 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
     uhd_string_vector_free(&devices_str);
     
     /* Create UHD handler */
-    if (strstr(args, "silent")) {
-      rf_uhd_suppress_stdout(NULL);
-    } else {
-      printf("Opening USRP with args: %s\n", args);
-    }
+    printf("Opening USRP with args: %s\n", args);
     uhd_error error = uhd_usrp_make(&handler->usrp, args);
     if (error) {
       fprintf(stderr, "Error opening UHD: code %d\n", error);
@@ -488,10 +494,7 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
     }
 
     handler->has_rssi = get_has_rssi(handler);
-    if (handler->has_rssi) {        
-      uhd_sensor_value_make_from_realnum(&handler->rssi_value, "rssi", 0, "dBm", "%f");      
-    }
-    
+
     size_t channel[4] = {0, 1, 2, 3};
     uhd_stream_args_t stream_args = {
           .cpu_format = "fc32",
@@ -534,9 +537,17 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
     uhd_rx_metadata_make(&handler->rx_md);
     uhd_rx_metadata_make(&handler->rx_md_first);
     uhd_tx_metadata_make(&handler->tx_md, false, 0, 0, false, false);
-  
-    
-    // Start low priority thread to receive async commands 
+
+    // Set starting gain to half maximum in case of using AGC
+    uhd_meta_range_handle gain_range;
+    uhd_meta_range_make(&gain_range);
+    uhd_usrp_get_rx_gain_range(handler->usrp, "", 0, gain_range);
+    double max_gain;
+    uhd_meta_range_stop(gain_range, &max_gain);
+    rf_uhd_set_rx_gain(handler, max_gain*0.7);
+    uhd_meta_range_free(&gain_range);
+
+    // Start low priority thread to receive async commands
     handler->async_thread_running = true; 
     if (pthread_create(&handler->async_thread, NULL, async_thread, handler)) {
       perror("pthread_create");
@@ -560,10 +571,7 @@ int rf_uhd_close(void *h)
   uhd_rx_metadata_free(&handler->rx_md_first);
   uhd_rx_metadata_free(&handler->rx_md);
   uhd_meta_range_free(&handler->rx_gain_range);
-  if (handler->has_rssi) {
-    uhd_sensor_value_free(&handler->rssi_value);
-  }
-  handler->async_thread_running = false; 
+  handler->async_thread_running = false;
   pthread_join(handler->async_thread, NULL);
 
   uhd_tx_streamer_free(&handler->tx_stream);
@@ -578,8 +586,11 @@ int rf_uhd_close(void *h)
 
 void rf_uhd_set_master_clock_rate(void *h, double rate) {
   rf_uhd_handler_t *handler = (rf_uhd_handler_t*) h;
-  if (handler->dynamic_rate) {
-    uhd_usrp_set_master_clock_rate(handler->usrp, rate, 0);
+  if (rate != handler->current_master_clock) {
+    if (handler->dynamic_rate) {
+      uhd_usrp_set_master_clock_rate(handler->usrp, rate, 0);
+    }
+    handler->current_master_clock = rate;
   }
 }
 
diff --git a/lib/src/phy/rf/rf_utils.c b/lib/src/phy/rf/rf_utils.c
index 892ad817e..ea016c721 100644
--- a/lib/src/phy/rf/rf_utils.c
+++ b/lib/src/phy/rf/rf_utils.c
@@ -113,10 +113,6 @@ int rf_mib_decoder(srslte_rf_t *rf, uint32_t nof_rx_antennas,cell_search_cfg_t *
     goto clean_exit;
   }
 
-  if (config->init_agc > 0) {
-    srslte_ue_sync_start_agc(&ue_mib.ue_sync, srslte_rf_set_rx_gain_th_wrapper, config->init_agc);    
-  }
-
   int srate = srslte_sampling_freq_hz(SRSLTE_UE_MIB_NOF_PRB);
   INFO("Setting sampling frequency %.2f MHz for PSS search\n", (float) srate/1000000);
   srslte_rf_set_rx_srate(rf, (float) srate);
@@ -124,7 +120,15 @@ int rf_mib_decoder(srslte_rf_t *rf, uint32_t nof_rx_antennas,cell_search_cfg_t *
   INFO("Starting receiver...\n", 0);
   srslte_rf_start_rx_stream(rf, false);
 
-  /* Find and decody MIB */
+  // Copy CFO estimate if provided and disable CP estimation during find
+  if (cfo) {
+    ue_mib.ue_sync.cfo_current_value = *cfo/15000;
+    ue_mib.ue_sync.cfo_is_copied = true;
+    ue_mib.ue_sync.cfo_correct_enable_find = true;
+    srslte_sync_set_cfo_cp_enable(&ue_mib.ue_sync.sfind, false, 0);
+  }
+
+  /* Find and decode MIB */
   ret = srslte_ue_mib_sync_decode(&ue_mib, config->max_frames_pbch, bch_payload, &cell->nof_ports, NULL); 
   if (ret < 0) {
     fprintf(stderr, "Error decoding MIB\n");
@@ -133,12 +137,7 @@ int rf_mib_decoder(srslte_rf_t *rf, uint32_t nof_rx_antennas,cell_search_cfg_t *
   if (ret == 1) {
     srslte_pbch_mib_unpack(bch_payload, cell, NULL);
   }
-
-  // Save AGC value 
-  if (config->init_agc > 0) {
-    config->init_agc = srslte_agc_get_gain(&ue_mib.ue_sync.agc);
-  }
-
+  
   // Save CFO 
   if (cfo) {
     *cfo = srslte_ue_sync_get_cfo(&ue_mib.ue_sync);
@@ -171,10 +170,7 @@ int rf_cell_search(srslte_rf_t *rf, uint32_t nof_rx_antennas,
   if (config->nof_valid_pss_frames) {
     srslte_ue_cellsearch_set_nof_valid_frames(&cs, config->nof_valid_pss_frames);
   }
-  if (config->init_agc > 0) {
-    srslte_ue_sync_start_agc(&cs.ue_sync, srslte_rf_set_rx_gain_th_wrapper, config->init_agc);
-  }
-  
+
   INFO("Setting sampling frequency %.2f MHz for PSS search\n", SRSLTE_CS_SAMP_FREQ/1000000);
   srslte_rf_set_rx_srate(rf, SRSLTE_CS_SAMP_FREQ);
   
@@ -217,12 +213,7 @@ int rf_cell_search(srslte_rf_t *rf, uint32_t nof_rx_antennas,
 
   // Save CFO
   if (cfo) {
-    *cfo = found_cells[max_peak_cell].cfo; 
-  }
-  
-  // Save AGC value for MIB decoding
-  if (config->init_agc > 0) {
-    config->init_agc = srslte_agc_get_gain(&cs.ue_sync.agc);
+    *cfo = found_cells[max_peak_cell].cfo;
   }
   
   srslte_rf_stop_rx_stream(rf);
diff --git a/lib/src/phy/sync/cfo.c b/lib/src/phy/sync/cfo.c
index 44c795c19..806701a9a 100644
--- a/lib/src/phy/sync/cfo.c
+++ b/lib/src/phy/sync/cfo.c
@@ -45,7 +45,7 @@ int srslte_cfo_init(srslte_cfo_t *h, uint32_t nsamples) {
   if (!h->cur_cexp) {
     goto clean;
   }
-  h->tol = SRSLTE_CFO_TOLERANCE;
+  h->tol = 0;
   h->last_freq = 0;
   h->nsamples = nsamples;
   h->max_samples = nsamples;
diff --git a/lib/src/phy/sync/pss.c b/lib/src/phy/sync/pss.c
index 1f56dfda9..ac54264eb 100644
--- a/lib/src/phy/sync/pss.c
+++ b/lib/src/phy/sync/pss.c
@@ -120,6 +120,7 @@ int srslte_pss_init_fft_offset_decim(srslte_pss_t *q,
     buffer_size = fft_size + frame_size + 1;
 
     q->filter_pss_enable = false;
+    q->chest_on_filter   = false;
 
     if(q->decimate > 1) {
         int filter_order = 3;
diff --git a/lib/src/phy/sync/sync.c b/lib/src/phy/sync/sync.c
index 8b7bbd69c..bf86eef4d 100644
--- a/lib/src/phy/sync/sync.c
+++ b/lib/src/phy/sync/sync.c
@@ -80,6 +80,7 @@ int srslte_sync_init_decim(srslte_sync_t *q, uint32_t frame_size, uint32_t max_o
     q->cfo_i_initiated = false;
     q->pss_filtering_enabled = false;
 
+    q->cfo_cp_nsymbols = 3;
     q->fft_size = fft_size;
     q->frame_size = frame_size;
     q->max_offset = max_offset;
@@ -326,8 +327,9 @@ void srslte_sync_set_pss_filt_enable(srslte_sync_t *q, bool enable) {
   q->pss_filtering_enabled = enable;
 }
 
-void srslte_sync_set_cfo_cp_enable(srslte_sync_t *q, bool enable) {
-  q->cfo_cp_enable = enable;
+void srslte_sync_set_cfo_cp_enable(srslte_sync_t *q, bool enable, uint32_t nof_symbols) {
+  q->cfo_cp_enable   = enable;
+  q->cfo_cp_nsymbols = nof_symbols;
 }
 
 void srslte_sync_set_cfo_pss_enable(srslte_sync_t *q, bool enable) {
@@ -475,7 +477,7 @@ srslte_pss_t* srslte_sync_get_cur_pss_obj(srslte_sync_t *q)
 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, 7, SRSLTE_CP_LEN_NORM(1,q->fft_size));
+  cp_offset = srslte_cp_synch(&q->cp_synch, input, q->max_offset, q->cfo_cp_nsymbols, 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; 
@@ -612,16 +614,16 @@ srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, const cf_t *input, uin
         }
 
         // PSS-based CFO estimation
-        float cfo_pss = srslte_pss_cfo_compute(&q->pss, pss_ptr);
+        q->cfo_pss = srslte_pss_cfo_compute(&q->pss, pss_ptr);
         if (!q->cfo_pss_is_set) {
-          q->cfo_pss_mean   = cfo_pss;
+          q->cfo_pss_mean   = q->cfo_pss;
           q->cfo_pss_is_set = true;
-        } else if (15000*fabsf(cfo_pss) < MAX_CFO_PSS_OFFSET) {
-          q->cfo_pss_mean = SRSLTE_VEC_EMA(cfo_pss, q->cfo_pss_mean, q->cfo_ema_alpha);
+        } else if (15000*fabsf(q->cfo_pss) < MAX_CFO_PSS_OFFSET) {
+          q->cfo_pss_mean = SRSLTE_VEC_EMA(q->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);
+             q->pss_filtering_enabled?"yes":"no", q->cfo_pss, q->cfo_pss_mean);
 
       }
 
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 4183016e0..348d0bdb6 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -973,6 +973,7 @@ bool srslte_ue_dl_decode_phich(srslte_ue_dl_t *q, uint32_t sf_idx, uint32_t n_pr
     srslte_phich_ngroups(&q->phich), srslte_phich_nsf(&q->phich));
   
   if (!srslte_phich_decode(&q->phich, q->sf_symbols_m, q->ce_m, 0, ngroup, nseq, sf_idx, &ack_bit, &distance)) {
+    q->last_phich_corr = distance;
     INFO("Decoded PHICH %d with distance %f\n", ack_bit, distance);    
   } else {
     fprintf(stderr, "Error decoding PHICH\n");
diff --git a/lib/src/phy/ue/ue_sync.c b/lib/src/phy/ue/ue_sync.c
index 511cdde2f..f3360dda7 100644
--- a/lib/src/phy/ue/ue_sync.c
+++ b/lib/src/phy/ue/ue_sync.c
@@ -29,7 +29,7 @@
 #include <strings.h>
 #include <assert.h>
 #include <unistd.h>
-#include <srslte/srslte.h>
+#include "srslte/srslte.h"
 
 
 #include "srslte/phy/ue/ue_sync.h"
@@ -47,14 +47,6 @@
 #define DEFAULT_SAMPLE_OFFSET_CORRECT_PERIOD  0
 #define DEFAULT_SFO_EMA_COEFF                 0.1
 
-#define DEFAULT_CFO_BW      0.2
-#define DEFAULT_CFO_PSS_MIN 500  // typical bias of PSS estimation.
-#define DEFAULT_CFO_REF_MIN 0    // typical bias of REF estimation
-#define DEFAULT_CFO_REF_MAX 500  // Maximum detection offset of REF based estimation
-
-#define DEFAULT_PSS_STABLE_TIMEOUT     100  // Time after which the PSS is considered to be stable and we accept REF-CFO
-
-#define DEFAULT_CFO_EMA_TRACK 0.1
 
 cf_t dummy_buffer0[15*2048/2];
 cf_t dummy_buffer1[15*2048/2];
@@ -82,8 +74,9 @@ int srslte_ue_sync_init_file_multi(srslte_ue_sync_t *q, uint32_t nof_prb, char *
     q->fft_size = srslte_symbol_sz(nof_prb);
     q->nof_rx_antennas = nof_rx_ant;
 
-    q->cfo_correct_enable = true;
-    
+    q->cfo_correct_enable_find  = false;
+    q->cfo_correct_enable_track = true;
+
     if (srslte_cfo_init(&q->file_cfo_correct, 2*q->sf_len)) {
       fprintf(stderr, "Error initiating CFO\n");
       goto clean_exit; 
@@ -226,9 +219,11 @@ int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q,
     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->cfo_loop_bw_pss = DEFAULT_CFO_BW_PSS;
+    q->cfo_loop_bw_ref = DEFAULT_CFO_BW_REF;
+
+    q->cfo_correct_enable_find  = false;
+    q->cfo_correct_enable_track = true;
 
     q->pss_stable_cnt     = 0;
     q->pss_stable_timeout = DEFAULT_PSS_STABLE_TIMEOUT;
@@ -269,14 +264,12 @@ int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q,
 
     // Configure FIND and TRACK sync objects behaviour (this configuration is always the same)
     srslte_sync_set_cfo_i_enable(&q->sfind,     false);
-    srslte_sync_set_cfo_cp_enable(&q->sfind,    true);
     srslte_sync_set_cfo_pss_enable(&q->sfind,   true);
     srslte_sync_set_pss_filt_enable(&q->sfind,  true);
     srslte_sync_set_sss_eq_enable(&q->sfind,    false);
 
     // 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_eq_enable(&q->strack,   false);
@@ -285,11 +278,9 @@ int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q,
     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;
   }
 
@@ -400,8 +391,13 @@ int srslte_ue_sync_set_cell(srslte_ue_sync_t *q, srslte_cell_t cell)
 
       srslte_sync_set_em_alpha(&q->strack,  0.2);
       srslte_sync_set_threshold(&q->strack, 1.2);
+
     }
 
+    // When cell is unknown, do CP CFO correction
+    srslte_sync_set_cfo_cp_enable(&q->sfind, true, q->frame_len<10000?14:3);
+    q->cfo_correct_enable_find  = false;
+
     srslte_ue_sync_reset(q);
 
     ret = SRSLTE_SUCCESS;
@@ -703,7 +699,7 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
           return SRSLTE_ERROR;
         }
       }
-      if (q->cfo_correct_enable) {
+      if (q->cfo_correct_enable_track) {
         for (int i = 0; i < q->nof_rx_antennas; i++) {
           srslte_cfo_correct(&q->file_cfo_correct,
                              input_buffer[i],
@@ -723,10 +719,20 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
         fprintf(stderr, "Error receiving samples\n");
         return SRSLTE_ERROR;
       }
-
+      int n;
       switch (q->state) {
         case SF_FIND:
-          switch(srslte_sync_find(&q->sfind, input_buffer[0], 0, &q->peak_idx)) {
+          // Correct CFO before PSS/SSS find using the sync object corrector (initialized for 1 ms)
+          if (q->cfo_correct_enable_find) {
+            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);
+            }
+          }
+          n = srslte_sync_find(&q->sfind, input_buffer[0], 0, &q->peak_idx);
+          switch(n) {
             case SRSLTE_SYNC_ERROR: 
               ret = SRSLTE_ERROR; 
               fprintf(stderr, "Error finding correlation peak (%d)\n", ret);
@@ -758,7 +764,7 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
           q->sf_idx = (q->sf_idx + q->nof_recv_sf) % 10;
 
           // Correct CFO before PSS/SSS tracking using the sync object corrector (initialized for 1 ms)
-          if (q->cfo_correct_enable) {
+          if (q->cfo_correct_enable_track) {
             for (int i=0;i<q->nof_rx_antennas;i++) {
               srslte_cfo_correct(&q->strack.cfo_corr_frame,
                                  input_buffer[i],
@@ -780,10 +786,10 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
             /* 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)) 
+            n = 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);
+            switch(n)
             {
               case SRSLTE_SYNC_ERROR:
                 fprintf(stderr, "Error tracking correlation peak\n");
diff --git a/lib/src/phy/ue/ue_ul.c b/lib/src/phy/ue/ue_ul.c
index 736fc3470..63411fb25 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   0.0 // Hz
+#define DEFAULT_CFO_TOL   1.0 // Hz
 
 int srslte_ue_ul_init(srslte_ue_ul_t *q,
                       cf_t *out_buffer,
diff --git a/lib/src/phy/utils/bit.c b/lib/src/phy/utils/bit.c
index 809d4c392..f4e97fad6 100644
--- a/lib/src/phy/utils/bit.c
+++ b/lib/src/phy/utils/bit.c
@@ -98,14 +98,14 @@ void srslte_bit_interleaver_run(srslte_bit_interleaver_t *q, uint8_t *input, uin
     w_offset_p=8-w_offset;
   }
 
-  uint32_t i = st * 8;
+  int i = st * 8;
 
   byte_idx += i - w_offset_p;
   bit_mask += i - w_offset_p;
   output_ptr += st;
 
 #ifdef LV_HAVE_SSE
-  for(; i < q->nof_bits - 15; i += 16) {
+  for(; i < (int) q->nof_bits - 15; i += 16) {
     __m128i in128;
     in128 = _mm_insert_epi8(in128, input[*(byte_idx++)], 0x7);
     in128 = _mm_insert_epi8(in128, input[*(byte_idx++)], 0x6);
@@ -137,7 +137,7 @@ void srslte_bit_interleaver_run(srslte_bit_interleaver_t *q, uint8_t *input, uin
 
 #endif /* LV_HAVE_SSE */
 
-  for(; i < q->nof_bits; i += 8) {
+  for(; i < (int) q->nof_bits - 7; i += 8) {
     uint8_t out0  = (input[*(byte_idx++)] & *(bit_mask++))?mask[0]:(uint8_t)0;
     uint8_t out1  = (input[*(byte_idx++)] & *(bit_mask++))?mask[1]:(uint8_t)0;
     uint8_t out2  = (input[*(byte_idx++)] & *(bit_mask++))?mask[2]:(uint8_t)0;
diff --git a/lib/src/phy/utils/debug.c b/lib/src/phy/utils/debug.c
index 7cb9c0f11..4b65e8d6d 100644
--- a/lib/src/phy/utils/debug.c
+++ b/lib/src/phy/utils/debug.c
@@ -34,6 +34,7 @@
 #include "srslte/version.h"
 
 int srslte_verbose = 0;
+int handler_registered = 0;
 
 void get_time_interval(struct timeval * tdata) {
 
diff --git a/lib/src/phy/utils/vector.c b/lib/src/phy/utils/vector.c
index 35457fcb5..6f5bc48e5 100644
--- a/lib/src/phy/utils/vector.c
+++ b/lib/src/phy/utils/vector.c
@@ -394,3 +394,11 @@ void srslte_vec_quant_suc(const int16_t *in, uint8_t *out, const float gain, con
 void srs_vec_cf_cpy(const cf_t *dst, cf_t *src, int len) {
   srslte_vec_cp_simd(dst, src, len);
 }
+
+void srslte_vec_interleave(const cf_t *x, const cf_t *y, cf_t *z, const int len) {
+  srslte_vec_interleave_simd(x, y, z, len);
+}
+
+void srslte_vec_interleave_add(const cf_t *x, const cf_t *y, cf_t *z, const int len) {
+  srslte_vec_interleave_add_simd(x, y, z, len);
+}
\ No newline at end of file
diff --git a/lib/src/phy/utils/vector_simd.c b/lib/src/phy/utils/vector_simd.c
index 78286a81a..6bf8ddc1d 100644
--- a/lib/src/phy/utils/vector_simd.c
+++ b/lib/src/phy/utils/vector_simd.c
@@ -1131,3 +1131,89 @@ uint32_t srslte_vec_max_ci_simd(const cf_t *x, const int len) {
 
   return max_index;
 }
+
+void srslte_vec_interleave_simd(const cf_t *x, const cf_t *y, cf_t *z, const int len) {
+  uint32_t i = 0, k = 0;
+
+#ifdef LV_HAVE_SSE
+  if (SRSLTE_IS_ALIGNED(x) && SRSLTE_IS_ALIGNED(y) && SRSLTE_IS_ALIGNED(z)) {
+    for (; i < len - 2 + 1; i += 2) {
+      __m128i a = _mm_load_si128((__m128i *) &x[i]);
+      __m128i b = _mm_load_si128((__m128i *) &y[i]);
+
+      __m128i r1 = _mm_unpacklo_epi64(a, b);
+      _mm_store_si128((__m128i *) &z[k], r1);
+      k += 2;
+
+      __m128i r2 = _mm_unpackhi_epi64(a, b);
+      _mm_store_si128((__m128i *) &z[k], r2);
+      k += 2;
+    }
+  } else {
+    for (; i < len - 2 + 1; i += 2) {
+      __m128i a = _mm_loadu_si128((__m128i *) &x[i]);
+      __m128i b = _mm_loadu_si128((__m128i *) &y[i]);
+
+      __m128i r1 = _mm_unpacklo_epi64(a, b);
+      _mm_storeu_si128((__m128i *) &z[k], r1);
+      k += 2;
+
+      __m128i r2 = _mm_unpackhi_epi64(a, b);
+      _mm_storeu_si128((__m128i *) &z[k], r2);
+      k += 2;
+    }
+  }
+#endif /* LV_HAVE_SSE */
+
+  for (;i < len; i++) {
+    z[k++] = x[i];
+    z[k++] = y[i];
+  }
+}
+
+void srslte_vec_interleave_add_simd(const cf_t *x, const cf_t *y, cf_t *z, const int len) {
+  uint32_t i = 0, k = 0;
+
+#ifdef LV_HAVE_SSE
+  if (SRSLTE_IS_ALIGNED(x) && SRSLTE_IS_ALIGNED(y) && SRSLTE_IS_ALIGNED(z)) {
+    for (; i < len - 2 + 1; i += 2) {
+      __m128i a = _mm_load_si128((__m128i *) &x[i]);
+      __m128i b = _mm_load_si128((__m128i *) &y[i]);
+
+      __m128 r1 = (__m128) _mm_unpacklo_epi64(a, b);
+      __m128 z1 = _mm_load_ps((float *) &z[k]);
+      r1 = _mm_add_ps((__m128) r1, z1);
+      _mm_store_ps((float *) &z[k], r1);
+      k += 2;
+
+      __m128 r2 = (__m128) _mm_unpackhi_epi64(a, b);
+      __m128 z2 = _mm_load_ps((float *) &z[k]);
+      r2 = _mm_add_ps((__m128) r2, z2);
+      _mm_store_ps((float *) &z[k], r2);
+      k += 2;
+    }
+  } else {
+    for (; i < len - 2 + 1; i += 2) {
+      __m128i a = _mm_loadu_si128((__m128i *) &x[i]);
+      __m128i b = _mm_loadu_si128((__m128i *) &y[i]);
+
+      __m128 r1 = (__m128) _mm_unpacklo_epi64(a, b);
+      __m128 z1 = _mm_loadu_ps((float *) &z[k]);
+      r1 = _mm_add_ps((__m128) r1, z1);
+      _mm_storeu_ps((float *) &z[k], r1);
+      k += 2;
+
+      __m128 r2 = (__m128) _mm_unpackhi_epi64(a, b);
+      __m128 z2 = _mm_loadu_ps((float *) &z[k]);
+      r2 = _mm_add_ps((__m128) r2, z2);
+      _mm_storeu_ps((float *) &z[k], r2);
+      k += 2;
+    }
+  }
+#endif /* LV_HAVE_SSE */
+
+  for (;i < len; i++) {
+    z[k++] += x[i];
+    z[k++] += y[i];
+  }
+}
\ No newline at end of file
diff --git a/srsenb/src/mac/mac.cc b/srsenb/src/mac/mac.cc
index 80c5e1ddf..4e706cfc7 100644
--- a/srsenb/src/mac/mac.cc
+++ b/srsenb/src/mac/mac.cc
@@ -24,10 +24,10 @@
  *
  */
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 #include <string.h>
 #include <strings.h>
diff --git a/srsenb/src/mac/scheduler.cc b/srsenb/src/mac/scheduler.cc
index 73c217a74..09f006236 100644
--- a/srsenb/src/mac/scheduler.cc
+++ b/srsenb/src/mac/scheduler.cc
@@ -5,10 +5,10 @@
 #include "srslte/common/pdu.h"
 #include "mac/scheduler.h"
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 namespace srsenb {
 
diff --git a/srsenb/src/mac/scheduler_harq.cc b/srsenb/src/mac/scheduler_harq.cc
index 6e1596850..a10fb562f 100644
--- a/srsenb/src/mac/scheduler_harq.cc
+++ b/srsenb/src/mac/scheduler_harq.cc
@@ -25,16 +25,15 @@
  */
 
 #include <string.h>
-#include <boost/concept_check.hpp>
 
 #include "srslte/srslte.h"
 #include "srslte/common/pdu.h"
 #include "mac/scheduler.h"
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 namespace srsenb {
 
diff --git a/srsenb/src/mac/scheduler_metric.cc b/srsenb/src/mac/scheduler_metric.cc
index 4e03d396c..60a299c04 100644
--- a/srsenb/src/mac/scheduler_metric.cc
+++ b/srsenb/src/mac/scheduler_metric.cc
@@ -28,10 +28,10 @@
 #include "mac/scheduler_harq.h"
 #include "mac/scheduler_metric.h"
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 namespace srsenb {
     
diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index ab3fc10b9..eb8f2459f 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -31,10 +31,10 @@
 #include "mac/scheduler_ue.h"
 #include "mac/scheduler.h"
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 /****************************************************** 
  *                  UE class                          *
diff --git a/srsenb/src/mac/ue.cc b/srsenb/src/mac/ue.cc
index d41afaf73..03bf13e33 100644
--- a/srsenb/src/mac/ue.cc
+++ b/srsenb/src/mac/ue.cc
@@ -30,10 +30,10 @@
 #include "srslte/interfaces/enb_interfaces.h"
 #include "mac/ue.h"
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 
 namespace srsenb {
diff --git a/srsenb/src/main.cc b/srsenb/src/main.cc
index 75dfed363..4f0d57520 100644
--- a/srsenb/src/main.cc
+++ b/srsenb/src/main.cc
@@ -319,12 +319,18 @@ void parse_args(all_args_t *args, int argc, char* argv[]) {
   }
 }
 
+static int  sigcnt = 0;
 static bool running    = true;
 static bool do_metrics = false;
 
 void sig_int_handler(int signo)
 {
+  sigcnt++;
   running = false;
+  printf("Stopping srsENB... Press Ctrl+C %d more times to force stop\n", 10-sigcnt);
+  if (sigcnt >= 10) {
+    exit(-1);
+  }
 }
 
 void *input_loop(void *m)
diff --git a/srsenb/src/phy/phch_common.cc b/srsenb/src/phy/phch_common.cc
index 87184321c..c732f76dc 100644
--- a/srsenb/src/phy/phch_common.cc
+++ b/srsenb/src/phy/phch_common.cc
@@ -31,10 +31,10 @@
 
 #include <assert.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__)
-#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) if (SRSLTE_DEBUG_ENABLED) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug(fmt, ##__VA_ARGS__)
 
 using namespace std; 
 
diff --git a/srsenb/src/phy/phch_worker.cc b/srsenb/src/phy/phch_worker.cc
index 24d2b3ca1..cec997176 100644
--- a/srsenb/src/phy/phch_worker.cc
+++ b/srsenb/src/phy/phch_worker.cc
@@ -31,10 +31,10 @@
 
 #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__)
-#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) if (SRSLTE_DEBUG_ENABLED) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug(fmt, ##__VA_ARGS__)
 
 using namespace std; 
 
diff --git a/srsenb/src/phy/phy.cc b/srsenb/src/phy/phy.cc
index 533abb765..2d6eee4b8 100644
--- a/srsenb/src/phy/phy.cc
+++ b/srsenb/src/phy/phy.cc
@@ -36,10 +36,10 @@
 #include "srslte/common/log.h"
 #include "phy/phy.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__)
-#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) if (SRSLTE_DEBUG_ENABLED) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug(fmt, ##__VA_ARGS__)
 
 using namespace std; 
 
diff --git a/srsenb/src/phy/txrx.cc b/srsenb/src/phy/txrx.cc
index acedd36c3..396069b45 100644
--- a/srsenb/src/phy/txrx.cc
+++ b/srsenb/src/phy/txrx.cc
@@ -32,10 +32,10 @@
 #include "phy/txrx.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__)
-#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) if (SRSLTE_DEBUG_ENABLED) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug(fmt, ##__VA_ARGS__)
 
 using namespace std; 
 
diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index 46c3fb0d0..9a7f66f47 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -27,10 +27,10 @@
 #ifndef DL_HARQ_H
 #define DL_HARQ_H
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 #include "srslte/common/log.h"
 #include "srslte/common/timers.h"
diff --git a/srsue/hdr/mac/ul_harq.h b/srsue/hdr/mac/ul_harq.h
index bd38fca97..5b33be1ec 100644
--- a/srsue/hdr/mac/ul_harq.h
+++ b/srsue/hdr/mac/ul_harq.h
@@ -27,10 +27,10 @@
 #ifndef ULHARQ_H
 #define ULHARQ_H
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 #include "srslte/interfaces/ue_interfaces.h"
 #include "srslte/common/log.h"
diff --git a/srsue/hdr/phy/phch_recv.h b/srsue/hdr/phy/phch_recv.h
index 6c7ff7717..ed4bd706a 100644
--- a/srsue/hdr/phy/phch_recv.h
+++ b/srsue/hdr/phy/phch_recv.h
@@ -52,13 +52,13 @@ public:
   ~phch_recv();
   void init(srslte::radio_multi* radio_handler, mac_interface_phy *mac,rrc_interface_phy *rrc,
             prach *prach_buffer, srslte::thread_pool *_workers_pool,
-            phch_common *_worker_com, srslte::log* _log_h, uint32_t nof_rx_antennas, uint32_t prio, int sync_cpu_affinity = -1);
+            phch_common *_worker_com, srslte::log* _log_h, srslte::log *_log_phy_lib_h, uint32_t nof_rx_antennas, uint32_t prio, int sync_cpu_affinity = -1);
   void stop();
   void set_agc_enable(bool enable);
 
   void    set_earfcn(std::vector<uint32_t> earfcn);
   void    force_freq(float dl_freq, float ul_freq);
-
+  
   void    reset_sync();
   void    cell_search_start();
   void    cell_search_stop();
@@ -95,7 +95,7 @@ private:
   void   reset();
   void   radio_error();
   bool   wait_radio_reset();
-  void   set_ue_sync_opts(srslte_ue_sync_t *q);
+  void   set_ue_sync_opts(srslte_ue_sync_t *q, float cfo);
   void   run_thread();
 
   void   set_sampling_rate();
@@ -275,6 +275,7 @@ private:
   mac_interface_phy    *mac;
   rrc_interface_phy    *rrc;
   srslte::log          *log_h;
+  srslte::log          *log_phy_lib_h;
   srslte::thread_pool  *workers_pool;
   srslte::radio_multi  *radio_h;
   phch_common          *worker_com;
diff --git a/srsue/hdr/phy/phch_worker.h b/srsue/hdr/phy/phch_worker.h
index f711ade09..e1d943880 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, chest_feedback_itf *chest_loop);
+  bool  init(uint32_t max_prb, srslte::log *log, srslte::log *log_phy_lib_h, chest_feedback_itf *chest_loop);
 
   bool  set_cell(srslte_cell_t cell);
 
@@ -67,6 +67,8 @@ public:
   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 */
   void work_imp();
@@ -115,6 +117,7 @@ private:
   /* Common objects */  
   phch_common    *phy;
   srslte::log    *log_h;
+  srslte::log    *log_phy_lib_h;
   chest_feedback_itf *chest_loop;
   srslte_cell_t  cell;
   bool           mem_initiated;
diff --git a/srsue/hdr/phy/phy.h b/srsue/hdr/phy/phy.h
index adc2abdca..5a1ac14a7 100644
--- a/srsue/hdr/phy/phy.h
+++ b/srsue/hdr/phy/phy.h
@@ -64,7 +64,7 @@ public:
   void set_agc_enable(bool enabled);
 
   void get_metrics(phy_metrics_t &m);
-  
+  void srslte_phy_logger(phy_logger_level_t log_level, char *str);
   
   
   static uint32_t tti_to_SFN(uint32_t tti);
@@ -160,6 +160,7 @@ private:
   srslte::radio_multi      *radio_handler;
   std::vector<void*>        log_vec;
   srslte::log              *log_h;
+  srslte::log              *log_phy_lib_h;
   srsue::mac_interface_phy *mac;
   srsue::rrc_interface_phy *rrc;
 
diff --git a/srsue/hdr/ue_base.h b/srsue/hdr/ue_base.h
index 8e5ce22a9..0b28dfeb6 100644
--- a/srsue/hdr/ue_base.h
+++ b/srsue/hdr/ue_base.h
@@ -81,6 +81,7 @@ typedef struct {
 
 typedef struct {
   std::string   phy_level;
+  std::string   phy_lib_level;
   std::string   mac_level;
   std::string   rlc_level;
   std::string   pdcp_level;
diff --git a/srsue/src/mac/demux.cc b/srsue/src/mac/demux.cc
index 171e7bf45..71be1ffc1 100644
--- a/srsue/src/mac/demux.cc
+++ b/srsue/src/mac/demux.cc
@@ -25,10 +25,10 @@
  */
 
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 #include "mac/mac.h"
 #include "mac/demux.h"
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index 86b644b8d..daa82d1cb 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -24,10 +24,10 @@
  *
  */
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 #include <string.h>
 #include <strings.h>
diff --git a/srsue/src/mac/mux.cc b/srsue/src/mac/mux.cc
index e6e136d2e..ba2b45209 100644
--- a/srsue/src/mac/mux.cc
+++ b/srsue/src/mac/mux.cc
@@ -24,10 +24,10 @@
  *
  */
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 #include "mac/mux.h"
 #include "mac/mac.h"
diff --git a/srsue/src/mac/proc_bsr.cc b/srsue/src/mac/proc_bsr.cc
index 43694c1bc..f68b6eee8 100644
--- a/srsue/src/mac/proc_bsr.cc
+++ b/srsue/src/mac/proc_bsr.cc
@@ -24,10 +24,10 @@
  *
  */
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 #include "mac/proc_bsr.h"
 #include "mac/mac.h"
diff --git a/srsue/src/mac/proc_phr.cc b/srsue/src/mac/proc_phr.cc
index ead0bf153..54c1ec1ce 100644
--- a/srsue/src/mac/proc_phr.cc
+++ b/srsue/src/mac/proc_phr.cc
@@ -24,10 +24,10 @@
  *
  */
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 #include "mac/proc_phr.h"
 #include "mac/mac.h"
diff --git a/srsue/src/mac/proc_ra.cc b/srsue/src/mac/proc_ra.cc
index 0a1855c80..d9a3e0d4d 100644
--- a/srsue/src/mac/proc_ra.cc
+++ b/srsue/src/mac/proc_ra.cc
@@ -24,10 +24,10 @@
  *
  */
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 #include <stdlib.h>
 #include <stdint.h>
diff --git a/srsue/src/mac/proc_sr.cc b/srsue/src/mac/proc_sr.cc
index afd2b7b5a..cb31a1c6d 100644
--- a/srsue/src/mac/proc_sr.cc
+++ b/srsue/src/mac/proc_sr.cc
@@ -24,10 +24,10 @@
  *
  */
 
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug(fmt, ##__VA_ARGS__)
 
 #include "mac/proc_sr.h"
 
diff --git a/srsue/src/main.cc b/srsue/src/main.cc
index 6df1cac99..aae389f70 100644
--- a/srsue/src/main.cc
+++ b/srsue/src/main.cc
@@ -37,6 +37,7 @@
 #include <boost/program_options/parsers.hpp>
 
 #include "ue.h"
+#include "srslte/srslte.h"
 #include "metrics_stdout.h"
 #include "metrics_csv.h"
 #include "srslte/common/metrics_hub.h"
@@ -97,6 +98,7 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
     ("gui.enable", bpo::value<bool>(&args->gui.enable)->default_value(false), "Enable GUI plots")
 
     ("log.phy_level", bpo::value<string>(&args->log.phy_level), "PHY log level")
+    ("log.phy_lib_level", bpo::value<string>(&args->log.phy_lib_level), "PHY lib log level")
     ("log.phy_hex_limit", bpo::value<int>(&args->log.phy_hex_limit), "PHY log hex dump limit")
     ("log.mac_level", bpo::value<string>(&args->log.mac_level), "MAC log level")
     ("log.mac_hex_limit", bpo::value<int>(&args->log.mac_hex_limit), "MAC log hex dump limit")
@@ -201,40 +203,42 @@ 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(0.0),
+     bpo::value<float>(&args->expert.phy.cfo_correct_tol_hz)->default_value(1.0),
      "Tolerance (in Hz) for digital CFO compensation (needs to be low if average_subframe_enabled=true.")
 
     ("expert.cfo_pss_ema",
-     bpo::value<float>(&args->expert.phy.cfo_pss_ema)->default_value(0.01),
+     bpo::value<float>(&args->expert.phy.cfo_pss_ema)->default_value(DEFAULT_CFO_EMA_TRACK),
      "CFO Exponential Moving Average coefficient for PSS estimation during TRACK.")
 
+    /* REF EMA is currently not used
     ("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),
+     bpo::value<float>(&args->expert.phy.cfo_loop_bw_pss)->default_value(DEFAULT_CFO_BW_PSS),
      "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),
+     bpo::value<float>(&args->expert.phy.cfo_loop_bw_ref)->default_value(DEFAULT_CFO_BW_REF),
      "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),
+     bpo::value<float>(&args->expert.phy.cfo_loop_pss_tol)->default_value(DEFAULT_CFO_PSS_MIN),
      "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),
+     bpo::value<float>(&args->expert.phy.cfo_loop_ref_min)->default_value(DEFAULT_CFO_REF_MIN),
      "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(20),
+     bpo::value<uint32_t>(&args->expert.phy.cfo_loop_pss_conv)->default_value(DEFAULT_PSS_STABLE_TIMEOUT),
      "After the PSS estimation is below cfo_loop_pss_tol for cfo_loop_pss_timeout times consecutively, RS adjustments are allowed.")
 
     ("expert.sic_pss_enabled",
@@ -324,6 +328,9 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
     if (!vm.count("log.phy_level")) {
       args->log.phy_level = args->log.all_level;
     }
+    if (!vm.count("log.phy_lib_level")) {
+      args->log.phy_lib_level = args->log.all_level;
+    }
     if (!vm.count("log.mac_level")) {
       args->log.mac_level = args->log.all_level;
     }
@@ -377,12 +384,18 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
   }
 }
 
+static int sigcnt = 0;
 static bool running = true;
 static bool do_metrics = false;
 metrics_stdout metrics_screen;
 
 void sig_int_handler(int signo) {
+  sigcnt++;
   running = false;
+  printf("Stopping srsUE... Press Ctrl+C %d more times to force stop\n", 10-sigcnt);
+  if (sigcnt >= 10) {
+    exit(-1);
+  }
 }
 
 void *input_loop(void *m) {
diff --git a/srsue/src/phy/phch_common.cc b/srsue/src/phy/phch_common.cc
index 59db2f546..b9138100b 100644
--- a/srsue/src/phy/phch_common.cc
+++ b/srsue/src/phy/phch_common.cc
@@ -29,10 +29,10 @@
 #include "srslte/srslte.h"
 #include "phy/phch_common.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__)
-#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) if (SRSLTE_DEBUG_ENABLED) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug(fmt, ##__VA_ARGS__)
 
 namespace srsue {
 
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index 06ae1baa2..4ae969273 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -31,10 +31,10 @@
 #include "phy/phch_worker.h"
 #include "phy/phch_recv.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__)
-#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) if (SRSLTE_DEBUG_ENABLED) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug(fmt, ##__VA_ARGS__)
 
 namespace srsue {
 
@@ -47,6 +47,7 @@ double callback_set_rx_gain(void *h, double gain) {
 }
 
 
+
 phch_recv::phch_recv() {
   dl_freq = -1;
   ul_freq = -1;
@@ -57,11 +58,12 @@ phch_recv::phch_recv() {
 
 void phch_recv::init(srslte::radio_multi *_radio_handler, mac_interface_phy *_mac, rrc_interface_phy *_rrc,
                      prach *_prach_buffer, srslte::thread_pool *_workers_pool,
-                     phch_common *_worker_com, srslte::log *_log_h, uint32_t nof_rx_antennas_, uint32_t prio,
+                     phch_common *_worker_com, srslte::log *_log_h, srslte::log *_log_phy_lib_h, uint32_t nof_rx_antennas_, uint32_t prio,
                      int sync_cpu_affinity)
 {
   radio_h = _radio_handler;
   log_h   = _log_h;
+  log_phy_lib_h = _log_phy_lib_h;
   mac     = _mac;
   rrc     = _rrc;
   workers_pool    = _workers_pool;
@@ -94,7 +96,7 @@ void phch_recv::init(srslte::radio_multi *_radio_handler, mac_interface_phy *_ma
   intra_freq_meas.init(worker_com, rrc, log_h);
 
   reset();
-
+  
   // Start main thread
   if (sync_cpu_affinity < 0) {
     start(prio);
@@ -182,7 +184,7 @@ void phch_recv::set_time_adv_sec(float _time_adv_sec)
   }
 }
 
-void phch_recv::set_ue_sync_opts(srslte_ue_sync_t *q)
+void phch_recv::set_ue_sync_opts(srslte_ue_sync_t *q, float cfo)
 {
   if (worker_com->args->cfo_integer_enabled) {
     srslte_ue_sync_set_cfo_i_enable(q, true);
@@ -198,6 +200,14 @@ void phch_recv::set_ue_sync_opts(srslte_ue_sync_t *q)
 
   q->strack.pss.chest_on_filter = worker_com->args->sic_pss_enabled;
 
+  // Disable CP based CFO estimation during find
+  if (cfo != 0) {
+    q->cfo_current_value = cfo/15000;
+    q->cfo_is_copied = true;
+    q->cfo_correct_enable_find = true;
+    srslte_sync_set_cfo_cp_enable(&q->sfind, false, 0);
+  }
+
   int time_correct_period = worker_com->args->time_correct_period;
   if (time_correct_period > 0) {
     srslte_ue_sync_set_sample_offset_correct_period(q, time_correct_period);
@@ -238,7 +248,7 @@ bool phch_recv::set_cell() {
   }
 
   // Set options defined in expert section
-  set_ue_sync_opts(&ue_sync);
+  set_ue_sync_opts(&ue_sync, search_p.get_last_cfo());
 
   // Reset ue_sync and set CFO/gain from search procedure
   srslte_ue_sync_reset(&ue_sync);
@@ -539,6 +549,7 @@ double phch_recv::set_rx_gain(double gain) {
 void phch_recv::run_thread()
 {
   phch_worker *worker = NULL;
+  phch_worker *last_worker = NULL;
   cf_t *buffer[SRSLTE_MAX_PORTS] = {NULL};
   uint32_t sf_idx = 0;
   phy_state  = IDLE;
@@ -552,6 +563,8 @@ void phch_recv::run_thread()
     }
 
     log_h->step(tti);
+    log_phy_lib_h->step(tti);
+    
     sf_idx = tti%10;
 
     switch (phy_state) {
@@ -637,6 +650,15 @@ void phch_recv::run_thread()
           switch(srslte_ue_sync_zerocopy_multi(&ue_sync, buffer)) {
             case 1:
 
+              if (last_worker) {
+                Debug("SF: cfo_tot=%7.1f Hz, ref=%f Hz, pss=%f Hz\n",
+                        srslte_ue_sync_get_cfo(&ue_sync),
+                     15000*last_worker->get_ref_cfo(),
+                     15000*ue_sync.strack.cfo_pss_mean);
+              }
+
+              last_worker = worker;
+
               Debug("SYNC:  Worker %d synchronized\n", worker->get_id());
 
               metrics.sfo = srslte_ue_sync_get_sfo(&ue_sync);
@@ -764,11 +786,7 @@ void phch_recv::search::init(cf_t *buffer[SRSLTE_MAX_PORTS], srslte::log *log_h,
   srslte_ue_cellsearch_set_nof_valid_frames(&cs, 2);
 
   // Set options defined in expert section
-  p->set_ue_sync_opts(&cs.ue_sync);
-
-  if (p->do_agc) {
-    srslte_ue_sync_start_agc(&cs.ue_sync, callback_set_rx_gain, 40);
-  }
+  p->set_ue_sync_opts(&cs.ue_sync, 0);
 
   force_N_id_2 = -1;
 }
@@ -849,10 +867,11 @@ phch_recv::search::ret_code phch_recv::search::run(srslte_cell_t *cell)
   }
 
   // Set options defined in expert section
-  p->set_ue_sync_opts(&ue_mib_sync.ue_sync);
+  p->set_ue_sync_opts(&ue_mib_sync.ue_sync, cfo);
 
+  // Start AGC after initial cell search
   if (p->do_agc) {
-    srslte_ue_sync_start_agc(&ue_mib_sync.ue_sync, callback_set_rx_gain, srslte_agc_get_gain(&cs.ue_sync.agc));
+    srslte_ue_sync_start_agc(&ue_mib_sync.ue_sync, callback_set_rx_gain, p->radio_h->get_rx_gain());
   }
 
   srslte_ue_sync_reset(&ue_mib_sync.ue_sync);
@@ -1201,7 +1220,6 @@ void phch_recv::scell_recv::init(srslte::log *log_h, bool sic_pss_enabled)
   // Configure FIND object behaviour (this configuration is always the same)
   srslte_sync_set_cfo_ema_alpha(&sync_find,    1.0);
   srslte_sync_set_cfo_i_enable(&sync_find,     false);
-  srslte_sync_set_cfo_cp_enable(&sync_find,    false);
   srslte_sync_set_cfo_pss_enable(&sync_find,   true);
   srslte_sync_set_pss_filt_enable(&sync_find,  true);
   srslte_sync_set_sss_eq_enable(&sync_find,    true);
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index f8f071005..bc178b970 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -31,10 +31,10 @@
 #include "srslte/interfaces/ue_interfaces.h"
 #include "srslte/asn1/liblte_rrc.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__)
-#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) if (SRSLTE_DEBUG_ENABLED) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug(fmt, ##__VA_ARGS__)
 
 
 /* This is to visualize the channel response */
@@ -107,9 +107,10 @@ void phch_worker::set_common(phch_common* phy_)
   phy = phy_;   
 }
 
-bool phch_worker::init(uint32_t max_prb, srslte::log *log_h, chest_feedback_itf *chest_loop)
+bool phch_worker::init(uint32_t max_prb, srslte::log *log_h, srslte::log *log_phy_lib_h , chest_feedback_itf *chest_loop)
 {
   this->log_h = log_h;
+  this->log_phy_lib_h =  log_phy_lib_h;
   this->chest_loop = chest_loop;
 
   // ue_sync in phy.cc requires a buffer for 3 subframes
@@ -132,7 +133,7 @@ bool phch_worker::init(uint32_t max_prb, srslte::log *log_h, chest_feedback_itf
   }
 
   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_chest_dl_cfo_estimate_enable(&ue_dl.chest, phy->args->cfo_ref_mask!=0, phy->args->cfo_ref_mask);
   srslte_ue_ul_set_normalization(&ue_ul, true);
   srslte_ue_ul_set_cfo_enable(&ue_ul, true);
 
@@ -173,6 +174,9 @@ void phch_worker::set_tti(uint32_t tti_, uint32_t tx_tti_)
   tti    = tti_; 
   tx_tti = tx_tti_;
   log_h->step(tti);
+  log_phy_lib_h->step(tti);
+  
+  
 }
 
 void phch_worker::set_cfo(float cfo_)
@@ -195,6 +199,11 @@ void phch_worker::set_crnti(uint16_t rnti)
   rnti_is_set = true; 
 }
 
+float phch_worker::get_ref_cfo()
+{
+  return srslte_chest_dl_get_cfo(&ue_dl.chest);
+}
+
 void phch_worker::work_imp()
 {
   if (!cell_initiated) {
diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc
index 689426b2a..0763b09cb 100644
--- a/srsue/src/phy/phy.cc
+++ b/srsue/src/phy/phy.cc
@@ -38,10 +38,10 @@
 #include "srslte/common/log.h"
 #include "phy/phy.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__)
-#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) if (SRSLTE_DEBUG_ENABLED) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug(fmt, ##__VA_ARGS__)
 
 
 
@@ -56,6 +56,31 @@ phy::phy() : workers_pool(MAX_WORKERS),
 {
 }
 
+static void srslte_phy_handler(phy_logger_level_t log_level, void *ctx, char *str) {
+  phy *r = (phy *) ctx;
+  r->srslte_phy_logger(log_level, str);
+}
+
+void phy::srslte_phy_logger(phy_logger_level_t log_level, char *str) {
+  if (log_phy_lib_h) {
+    switch(log_level){
+      case LOG_LEVEL_INFO:
+        log_phy_lib_h->info(" %s", str);
+        break;
+      case LOG_LEVEL_DEBUG:
+        log_phy_lib_h->debug(" %s", str);
+        break;
+      case LOG_LEVEL_ERROR:
+        log_phy_lib_h->error(" %s", str);
+        break;
+      default:
+        break;
+    }
+  } else {
+    printf("[PHY_LIB]: %s\n", str);
+  }
+}
+
 void phy::set_default_args(phy_args_t *args)
 {
   args->nof_rx_ant          = 1;
@@ -105,7 +130,7 @@ bool phy::init(srslte::radio_multi* radio_handler, mac_interface_phy *mac, rrc_i
   this->radio_handler = radio_handler;
   this->mac           = mac;
   this->rrc           = rrc;
-
+ 
   if (!phy_args) {
     args = &default_args;
     set_default_args(args);
@@ -118,7 +143,9 @@ bool phy::init(srslte::radio_multi* radio_handler, mac_interface_phy *mac, rrc_i
   }
 
   nof_workers = args->nof_phy_threads;
-
+  this->log_phy_lib_h = (srslte::log*) log_vec[nof_workers];
+  srslte_phy_log_register_handler(this, srslte_phy_handler);
+  
   initiated = false;
   start();
   return true;
@@ -133,12 +160,12 @@ 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], &sf_recv);
+    workers[i].init(SRSLTE_MAX_PRB, (srslte::log*) log_vec[i], (srslte::log*) log_vec[nof_workers], &sf_recv);
     workers_pool.init_worker(i, &workers[i], WORKERS_THREAD_PRIO, args->worker_cpu_mask);
   }
 
   // Warning this must be initialized after all workers have been added to the pool
-  sf_recv.init(radio_handler, mac, rrc, &prach_buffer, &workers_pool, &workers_common, log_h, args->nof_rx_ant, SF_RECV_THREAD_PRIO, args->sync_cpu_affinity);
+  sf_recv.init(radio_handler, mac, rrc, &prach_buffer, &workers_pool, &workers_common, log_h, log_phy_lib_h, args->nof_rx_ant, SF_RECV_THREAD_PRIO, args->sync_cpu_affinity);
 
   // Disable UL signal pregeneration until the attachment 
   enable_pregen_signals(false);
diff --git a/srsue/src/phy/prach.cc b/srsue/src/phy/prach.cc
index 182a66aa7..498ea1c6a 100644
--- a/srsue/src/phy/prach.cc
+++ b/srsue/src/phy/prach.cc
@@ -34,10 +34,10 @@
 #include "phy/phy.h"
 #include "srslte/interfaces/ue_interfaces.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__)
-#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->error(fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) if (SRSLTE_DEBUG_ENABLED) log_h->warning(fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info(fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug(fmt, ##__VA_ARGS__)
 
 namespace srsue {
  
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index 2edf0fed5..8f10a5bfc 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -84,6 +84,16 @@ bool ue::init(all_args_t *args_)
   for (int i=0;i<args->expert.phy.nof_phy_threads;i++) {
     ((srslte::log_filter*) phy_log[i])->set_level(level(args->log.phy_level));
   }
+  
+  /* here we add a log layer to handle logging from the phy library*/
+  srslte::log_filter *lib_log = new srslte::log_filter;
+  char tmp[16];
+  sprintf(tmp, "PHY_LIB");
+  lib_log->init(tmp, logger, true);
+  phy_log.push_back((void*) lib_log);
+  ((srslte::log_filter*) phy_log[args->expert.phy.nof_phy_threads])->set_level(level(args->log.phy_lib_level));
+ 
+  
   mac_log.set_level(level(args->log.mac_level));
   rlc_log.set_level(level(args->log.rlc_level));
   pdcp_log.set_level(level(args->log.pdcp_level));
@@ -92,7 +102,7 @@ bool ue::init(all_args_t *args_)
   gw_log.set_level(level(args->log.gw_level));
   usim_log.set_level(level(args->log.usim_level));
 
-  for (int i=0;i<args->expert.phy.nof_phy_threads;i++) {
+  for (int i=0;i<args->expert.phy.nof_phy_threads + 1;i++) {
     ((srslte::log_filter*) phy_log[i])->set_hex_limit(args->log.phy_hex_limit);
   }
   mac_log.set_hex_limit(args->log.mac_hex_limit);
diff --git a/srsue/test/upper/CMakeLists.txt b/srsue/test/upper/CMakeLists.txt
index cbffe1cc8..00fb54576 100644
--- a/srsue/test/upper/CMakeLists.txt
+++ b/srsue/test/upper/CMakeLists.txt
@@ -26,7 +26,10 @@ add_executable(rrc_reconfig_test rrc_reconfig_test.cc)
 target_link_libraries(rrc_reconfig_test srsue_upper srslte_upper srslte_phy)
 add_test(rrc_reconfig_test rrc_reconfig_test)
 
-                              
+add_executable(nas_test nas_test.cc)
+target_link_libraries(nas_test srsue_upper srslte_upper srslte_phy)
+add_test(nas_test nas_test)
+
 ########################################################################
 # Option to run command after build (useful for remote builds)
 ########################################################################
diff --git a/srsue/test/upper/nas_test.cc b/srsue/test/upper/nas_test.cc
index 6aa9f29e5..f328d77df 100644
--- a/srsue/test/upper/nas_test.cc
+++ b/srsue/test/upper/nas_test.cc
@@ -25,35 +25,37 @@
  */
 
 #include <iostream>
+#include <assert.h>
 #include "upper/usim.h"
 #include "upper/nas.h"
 #include "srslte/upper/rlc.h"
 #include "upper/rrc.h"
 #include "mac/mac.h"
+#include "srslte/common/log_filter.h"
 #include "srslte/upper/pdcp_entity.h"
 #include "srslte/upper/pdcp.h"
-#include "srslte/common/log_stdout.h"
 #include "srslte/interfaces/ue_interfaces.h"
+#include "srslte/common/bcd_helpers.h"
+
 
 using namespace srsue;
 
+#define LCID 1
 
+uint8_t auth_request_pdu[] = { 0x07, 0x52, 0x01, 0x0c, 0x63, 0xa8, 0x54, 0x13, 0xe6, 0xa4,
+                               0xce, 0xd9, 0x86, 0xfb, 0xe5, 0xce, 0x9b, 0x62, 0x5e, 0x10,
+                               0x67, 0x57, 0xb3, 0xc2, 0xb9, 0x70, 0x90, 0x01, 0x0c, 0x72,
+                               0x8a, 0x67, 0x57, 0x92, 0x52, 0xb8 };
 
-uint8_t pdu1[] = {
-0x03, 0x22, 0x16, 0x15, 0xe8 , 0x00 , 0x00 , 0x03 , 0x13 , 0xb0 , 0x00 , 0x02 , 0x90 , 0x08,
-0x79, 0xf0, 0x00, 0x00, 0x40 , 0xb5 , 0x01 , 0x25 , 0x40 , 0xcc , 0x1d , 0x08 , 0x04 , 0x3c , 0x18 , 0x00,
-0x4c, 0x02, 0x20, 0x0f, 0xa8 , 0x00 , 0x65 , 0x48 , 0x07 , 0x04 , 0x04 , 0x24 , 0x1c , 0x19 , 0x05 , 0x41,
-0x39, 0x39, 0x4d, 0x38, 0x14 , 0x04 , 0x28 , 0xd1 , 0x5e , 0x6d , 0x78 , 0x13 , 0xfb , 0xf9 , 0x01 , 0xb1,
-0x40, 0x2f, 0xd8, 0x4c, 0x02 , 0x20 , 0x00 , 0x5b , 0x78 , 0x00 , 0x07 , 0xa1 , 0x25 , 0xa9 , 0xc1 , 0x3f,
-0xd9, 0x40, 0x41, 0xf5, 0x1b , 0x58 , 0x2f , 0x27 , 0x28 , 0xa0 , 0xed , 0xde , 0x54 , 0x43 , 0x48 , 0xc0,
-0x56, 0xcc, 0x00, 0x02, 0x84 , 0x00 , 0x42 , 0x0a , 0xf1 , 0x63 };
+uint8_t sec_mode_command_pdu[] = { 0x37, 0x37, 0xc7, 0x67, 0xae, 0x00, 0x07, 0x5d, 0x02, 0x01,
+                                   0x02, 0xe0, 0x60, 0xc1 };
 
-uint32_t PDU1_LEN = 104;
+uint16 mcc = 61441;
+uint16 mnc = 65281;
 
+using namespace srslte;
 
-#define LCID 3
-
-namespace srsue {
+namespace srslte {
 
 // fake classes
 class pdcp_dummy : public rrc_interface_pdcp
@@ -63,121 +65,154 @@ public:
       void write_pdu_bcch_bch(byte_buffer_t *pdu) {}
       void write_pdu_bcch_dlsch(byte_buffer_t *pdu) {}
       void write_pdu_pcch(byte_buffer_t *pdu) {}
+      std::string get_rb_name(uint32_t lcid) { return std::string("lcid"); }
 };
 
-
-
 class rrc_dummy : public rrc_interface_nas
 {
 public:
   void write_sdu(uint32_t lcid, byte_buffer_t *sdu)
   {
-
+    printf("NAS generated SDU (len=%d):\n", sdu->N_bytes);
+    last_sdu_len = sdu->N_bytes;
+    srslte_vec_fprint_byte(stdout, sdu->msg, sdu->N_bytes);
   }
+  std::string get_rb_name(uint32_t lcid) { return std::string("lcid"); }
+  uint32_t get_last_sdu_len() { return last_sdu_len; }
 
-  uint16_t get_mcc() { return 0x11; }
-  uint16_t get_mnc() { return 0xff; }
-  void enable_capabilities() {
+  void plmn_search() {};
+  void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {};
 
-  }
+  uint16_t get_mcc() { return mcc; }
+  uint16_t get_mnc() { return mnc; }
+  void enable_capabilities() {}
+
+private:
+  uint32_t last_sdu_len;
 };
 
 class gw_dummy : public gw_interface_nas, public gw_interface_pdcp
 {
-  error_t setup_if_addr(uint32_t ip_addr, char *err_str) {}
+  error_t setup_if_addr(uint32_t ip_addr, char *err_str) { return ERROR_NONE; }
   void write_pdu(uint32_t lcid, byte_buffer_t *pdu) {}
 };
 
 }
 
-class usim_dummy : public usim_interface_nas
+int security_command_test()
 {
-  void get_imsi_vec(uint8_t* imsi_, uint32_t n){
+  int ret = SRSLTE_ERROR;
+  srslte::log_filter nas_log("NAS");
+  srslte::log_filter rrc_log("RRC");
+  srslte::log_filter mac_log("MAC");
+  srslte::log_filter usim_log("USIM");
 
-  }
-  void get_imei_vec(uint8_t* imei_, uint32_t n){
+  nas_log.set_level(srslte::LOG_LEVEL_DEBUG);
+  rrc_log.set_level(srslte::LOG_LEVEL_DEBUG);
+  nas_log.set_hex_limit(100000);
+  rrc_log.set_hex_limit(100000);
 
-  }
-  void generate_authentication_response(uint8_t  *rand,
-                                                uint8_t  *autn_enb,
-                                                uint16_t  mcc,
-                                                uint16_t  mnc,
-                                                bool     *net_valid,
-                                                uint8_t  *res){
+  rrc_dummy rrc_dummy;
+  gw_dummy gw;
 
+  usim_args_t args;
+  args.algo = "xor";
+  args.amf = "9001";
+  args.imei = "353490069873319";
+  args.imsi = "001010123456789";
+  args.k = "00112233445566778899aabbccddeeff";
+  args.op = "63BFA50EE6523365FF14C1F45F88737D";
+
+
+  // init USIM
+  srsue::usim usim;
+  bool    net_valid;
+  uint8_t res[16];
+  usim.init(&args, &usim_log);
+
+  srslte::byte_buffer_pool *pool;
+  pool = byte_buffer_pool::get_instance();
+
+  srsue::nas nas;
+  srslte_nas_config_t cfg;
+  nas.init(&usim, &rrc_dummy, &gw, &nas_log, cfg);
+
+  // push auth request PDU to NAS to generate security context
+  byte_buffer_t* tmp = pool->allocate();
+  memcpy(tmp->msg, auth_request_pdu, sizeof(auth_request_pdu));
+  tmp->N_bytes = sizeof(auth_request_pdu);
+  nas.write_pdu(LCID, tmp);
+
+  // TODO: add check for authentication response
+
+  // reuse buffer for security mode command
+  memcpy(tmp->msg, sec_mode_command_pdu, sizeof(sec_mode_command_pdu));
+  tmp->N_bytes = sizeof(sec_mode_command_pdu);
+  nas.write_pdu(LCID, tmp);
+
+  // check length of generated NAS SDU
+  if (rrc_dummy.get_last_sdu_len() > 3) {
+    ret = SRSLTE_SUCCESS;
   }
 
+  pool->cleanup();
 
-  void generate_nas_keys(uint8_t *k_nas_enc,
-                                   uint8_t *k_nas_int,
-                                   CIPHERING_ALGORITHM_ID_ENUM cipher_algo,
-                                   INTEGRITY_ALGORITHM_ID_ENUM integ_algo){
+  return ret;
+}
 
+
+int mme_attach_request_test()
+{
+  int ret = SRSLTE_ERROR;
+  srslte::log_filter nas_log("NAS");
+  srslte::log_filter rrc_log("RRC");
+  srslte::log_filter mac_log("MAC");
+  srslte::log_filter usim_log("USIM");
+
+  nas_log.set_level(srslte::LOG_LEVEL_DEBUG);
+  rrc_log.set_level(srslte::LOG_LEVEL_DEBUG);
+  nas_log.set_hex_limit(100000);
+  rrc_log.set_hex_limit(100000);
+
+  rrc_dummy rrc_dummy;
+  gw_dummy gw;
+  srsue::usim usim;
+  usim_args_t args;
+  args.algo = "xor";
+  args.amf = "9001";
+  args.imei = "353490069873319";
+  args.imsi = "001010123456789";
+  args.k = "00112233445566778899aabbccddeeff";
+  args.op = "63BFA50EE6523365FF14C1F45F88737D";
+  usim.init(&args, &usim_log);
+
+  srslte_nas_config_t nas_cfg;
+  srsue::nas nas;
+  nas.init(&usim, &rrc_dummy, &gw, &nas_log, nas_cfg);
+
+  nas.attach_request();
+  nas.notify_connection_setup();
+
+  // check length of generated NAS SDU
+  if (rrc_dummy.get_last_sdu_len() > 3) {
+    ret = SRSLTE_SUCCESS;
   }
-};
-
 
+  return ret;
+}
 
 
 int main(int argc, char **argv)
 {
-  srslte::log_stdout nas_log("NAS");
-  srslte::log_stdout pdcp_entity_log("PDCP");
-  srslte::log_stdout rrc_log("RRC");
-  srslte::log_stdout mac_log("MAC");
+  if (security_command_test()) {
+    printf("Security command test failed.\n");
+    return -1;
+  }
 
+  if (mme_attach_request_test()) {
+    printf("Attach request test failed.\n");
+    return -1;
+  }
 
-  nas_log.set_level(srslte::LOG_LEVEL_DEBUG);
-  pdcp_entity_log.set_level(srslte::LOG_LEVEL_DEBUG);
-  rrc_log.set_level(srslte::LOG_LEVEL_DEBUG);
-
-  nas_log.set_hex_limit(100000);
-  rrc_log.set_hex_limit(100000);
-
-  usim_dummy usim;
-  rrc_dummy rrc_dummy;
-  gw_dummy gw;
-
-  pdcp_dummy pdcp_dummy;
-
-
-
-
-  buffer_pool        *pool;
-  pool = buffer_pool::get_instance();
-
-  srsue::nas nas;
-  nas.init(&usim, &rrc_dummy, &gw, &nas_log);
-
-
-
-
-  byte_buffer_t* tmp = pool_allocate;
-  memcpy(tmp->msg, &pdu1[0], PDU1_LEN);
-  tmp->N_bytes = PDU1_LEN;
-
-  //byte_buffer_t tmp2;
-  //memcpy(tmp2.msg, &pdu1[0], PDU1_LEN);
-  //tmp2.N_bytes = PDU1_LEN;
-
-  //srsue::mac mac;
-  //mac.init(NULL, NULL, NULL, &mac_log);
-
-  srsue::rrc rrc;
-  rrc.init(NULL, NULL, NULL, NULL, &nas, NULL, NULL, &rrc_log);
-  //rrc.init(&phy, &mac, &rlc, &pdcp, &nas, &usim, &mac, &rrc_log);
-
-
-  srsue::pdcp_entity pdcp_entity;
-  pdcp_entity.init(NULL, &rrc, &gw, &pdcp_entity_log, RB_ID_SRB1, NULL);
-
-  pdcp_entity.write_pdu(tmp);
-
-  //rrc.write_sdu(RB_ID_SRB2, tmp);
-
-
-  //nas.write_pdu(LCID, tmp);
-
-  pool->cleanup();
-
+  return 0;
 }
diff --git a/srsue/ue.conf.example b/srsue/ue.conf.example
index 444f9e156..7c096a268 100644
--- a/srsue/ue.conf.example
+++ b/srsue/ue.conf.example
@@ -73,6 +73,7 @@ nas_filename = /tmp/nas.pcap
 #####################################################################
 [log]
 all_level = info
+phy_lib_level = none
 all_hex_limit = 32
 filename = /tmp/ue.log
 
@@ -202,15 +203,14 @@ enable = false
 
 # CFO related values
 #cfo_integer_enabled = false
-#cfo_correct_tol_hz  = 0
-#cfo_pss_ema         = 0.1
-#cfo_ref_ema         = 0.01
+#cfo_correct_tol_hz  = 1.0
+#cfo_pss_ema         = 0.05
 #cfo_ref_mask        = 1023
 #cfo_loop_bw_pss     = 0.05
 #cfo_loop_bw_ref     = 0.01
-#cfo_loop_pss_tol    = 300
+#cfo_loop_pss_tol    = 400
 #cfo_loop_ref_min    = 0
-#cfo_loop_pss_conv   = 50
+#cfo_loop_pss_conv   = 20
 
 #####################################################################
 # Manual RF calibration