diff --git a/CMakeLists.txt b/CMakeLists.txt
index 40d3ef4da..28afbc0d8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -120,7 +120,7 @@ else(POLARSSL_FOUND)
     endif(BUILD_STATIC)
     add_definitions(-DHAVE_MBEDTLS)
   else(MBEDTLS_FOUND)
-    message(FATAL_ERROR "Either polarssl or mbedtls is required to compile srsLTE")
+    message(FATAL_ERROR "Either PolarSSL or mbedTLS are required to build srsLTE")
   endif (MBEDTLS_FOUND)
 endif(POLARSSL_FOUND)
 
diff --git a/cmake/modules/FindUHD.cmake b/cmake/modules/FindUHD.cmake
index a5b8834cd..0c0eb742e 100644
--- a/cmake/modules/FindUHD.cmake
+++ b/cmake/modules/FindUHD.cmake
@@ -16,6 +16,7 @@ FIND_LIBRARY(
     HINTS $ENV{UHD_DIR}/lib
     PATHS /usr/local/lib
           /usr/lib
+          /usr/lib/x86_64-linux-gnu
           /usr/local/lib64
           /usr/local/lib32
 )
diff --git a/lib/examples/cell_measurement.c b/lib/examples/cell_measurement.c
index cb6a9b78d..143ee784e 100644
--- a/lib/examples/cell_measurement.c
+++ b/lib/examples/cell_measurement.c
@@ -241,19 +241,31 @@ int main(int argc, char **argv) {
   srslte_rf_stop_rx_stream(&rf);
   srslte_rf_flush_buffer(&rf);
   
-  if (srslte_ue_sync_init_multi(&ue_sync, cell, srslte_rf_recv_wrapper, 1, (void*) &rf)) {
+  if (srslte_ue_sync_init_multi(&ue_sync, cell.nof_prb, cell.id==1000, srslte_rf_recv_wrapper, 1, (void*) &rf)) {
     fprintf(stderr, "Error initiating ue_sync\n");
     return -1; 
   }
-  if (srslte_ue_dl_init(&ue_dl, cell, 1)) {
+  if (srslte_ue_sync_set_cell(&ue_sync, cell)) {
+    fprintf(stderr, "Error initiating ue_sync\n");
+    return -1;
+  }
+  if (srslte_ue_dl_init(&ue_dl, cell.nof_prb, 1)) {
     fprintf(stderr, "Error initiating UE downlink processing module\n");
     return -1;
   }
-  if (srslte_ue_mib_init(&ue_mib, cell)) {
+  if (srslte_ue_dl_set_cell(&ue_dl, cell)) {
+    fprintf(stderr, "Error initiating UE downlink processing module\n");
+    return -1;
+  }
+  if (srslte_ue_mib_init(&ue_mib, cell.nof_prb)) {
     fprintf(stderr, "Error initaiting UE MIB decoder\n");
     return -1;
   }
-  
+  if (srslte_ue_mib_set_cell(&ue_mib, cell)) {
+    fprintf(stderr, "Error initaiting UE MIB decoder\n");
+    return -1;
+  }
+
   /* Configure downlink receiver for the SI-RNTI since will be the only one we'll use */
   srslte_ue_dl_set_rnti(&ue_dl, SRSLTE_SIRNTI); 
 
@@ -264,11 +276,15 @@ int main(int argc, char **argv) {
     fprintf(stderr, "Error initiating FFT\n");
     return -1;
   }
-  if (srslte_chest_dl_init(&chest, cell)) {
+  if (srslte_chest_dl_init(&chest, cell.nof_prb)) {
     fprintf(stderr, "Error initiating channel estimator\n");
     return -1;
   }
-  
+  if (srslte_chest_dl_set_cell(&chest, cell)) {
+    fprintf(stderr, "Error initiating channel estimator\n");
+    return -1;
+  }
+
   int sf_re = SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp);
 
   cf_t *sf_symbols = srslte_vec_malloc(sf_re * sizeof(cf_t));
diff --git a/lib/examples/cell_search.c b/lib/examples/cell_search.c
index 8ab560e0c..281e3d102 100644
--- a/lib/examples/cell_search.c
+++ b/lib/examples/cell_search.c
@@ -188,6 +188,18 @@ int main(int argc, char **argv) {
   sigprocmask(SIG_UNBLOCK, &sigset, NULL);
   signal(SIGINT, sig_int_handler);
 
+  if (srslte_ue_cellsearch_init_multi(&cs, cell_detect_config.max_frames_pss, srslte_rf_recv_wrapper, 1, (void*) &rf)) {
+    fprintf(stderr, "Error initiating UE cell detect\n");
+    exit(-1);
+  }
+
+  if (cell_detect_config.max_frames_pss) {
+    srslte_ue_cellsearch_set_nof_valid_frames(&cs, cell_detect_config.nof_valid_pss_frames);
+  }
+  if (cell_detect_config.init_agc) {
+    srslte_ue_sync_start_agc(&cs.ue_sync, srslte_rf_set_rx_gain_wrapper, cell_detect_config.init_agc);
+  }
+
   for (freq=0;freq<nof_freqs && !go_exit;freq++) {
   
     /* set rf_freq */
@@ -203,18 +215,6 @@ int main(int argc, char **argv) {
     }
       
     bzero(found_cells, 3*sizeof(srslte_ue_cellsearch_result_t));
-      
-    if (srslte_ue_cellsearch_init_multi(&cs, cell_detect_config.max_frames_pss, srslte_rf_recv_wrapper, 1, (void*) &rf)) {
-      fprintf(stderr, "Error initiating UE cell detect\n");
-      exit(-1);
-    }
-    
-    if (cell_detect_config.max_frames_pss) {
-      srslte_ue_cellsearch_set_nof_valid_frames(&cs, cell_detect_config.nof_valid_pss_frames);
-    }
-    if (cell_detect_config.init_agc) {
-      srslte_ue_sync_start_agc(&cs.ue_sync, srslte_rf_set_rx_gain_wrapper, cell_detect_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);
@@ -222,7 +222,6 @@ int main(int argc, char **argv) {
     srslte_rf_start_rx_stream(&rf);
     
     n = srslte_ue_cellsearch_scan(&cs, found_cells, NULL); 
-    srslte_ue_cellsearch_free(&cs);
     if (n < 0) {
       fprintf(stderr, "Error searching cell\n");
       exit(-1);
@@ -254,7 +253,7 @@ int main(int argc, char **argv) {
       }
     }    
   }
-  
+
   printf("\n\nFound %d cells\n", n_found_cells);
   for (int i=0;i<n_found_cells;i++) {
     printf("Found CELL %.1f MHz, EARFCN=%d, PHYID=%d, %d PRB, %d ports, PSS power=%.1f dBm\n", 
@@ -268,7 +267,8 @@ int main(int argc, char **argv) {
   }
   
   printf("\nBye\n");
-    
+
+  srslte_ue_cellsearch_free(&cs);
   srslte_rf_close(&rf);
   exit(0);
 }
diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index 35f7d3e9f..d481ff20f 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -292,7 +292,11 @@ void base_init() {
     exit(-1);
   }
   srslte_ofdm_set_normalize(&ifft, true);
-  if (srslte_pbch_init(&pbch, cell)) {
+  if (srslte_pbch_init(&pbch)) {
+    fprintf(stderr, "Error creating PBCH object\n");
+    exit(-1);
+  }
+  if (srslte_pbch_set_cell(&pbch, cell)) {
     fprintf(stderr, "Error creating PBCH object\n");
     exit(-1);
   }
@@ -302,7 +306,11 @@ void base_init() {
     exit(-1);
   }
 
-  if (srslte_pcfich_init(&pcfich, &regs, cell)) {
+  if (srslte_pcfich_init(&pcfich, 1)) {
+    fprintf(stderr, "Error creating PBCH object\n");
+    exit(-1);
+  }
+  if (srslte_pcfich_set_cell(&pcfich, &regs, cell)) {
     fprintf(stderr, "Error creating PBCH object\n");
     exit(-1);
   }
@@ -312,13 +320,20 @@ void base_init() {
     exit(-1);
   }
 
-  if (srslte_pdcch_init_tx(&pdcch, &regs, cell)) {
+  if (srslte_pdcch_init_enb(&pdcch, cell.nof_prb)) {
+    fprintf(stderr, "Error creating PDCCH object\n");
+    exit(-1);
+  }
+  if (srslte_pdcch_set_cell(&pdcch, &regs, cell)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);
   }
 
-  bzero(&pdsch, sizeof(srslte_pdsch_t));
-  if (srslte_pdsch_init_tx(&pdsch, cell)) {
+  if (srslte_pdsch_init_enb(&pdsch, cell.nof_prb)) {
+    fprintf(stderr, "Error creating PDSCH object\n");
+    exit(-1);
+  }
+  if (srslte_pdsch_set_cell(&pdsch, cell)) {
     fprintf(stderr, "Error creating PDSCH object\n");
     exit(-1);
   }
@@ -660,7 +675,11 @@ int main(int argc, char **argv) {
   srslte_sss_generate(sss_signal0, sss_signal5, cell.id);
   
   /* Generate CRS signals */
-  if (srslte_chest_dl_init(&est, cell)) {
+  if (srslte_chest_dl_init(&est, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing equalizer\n");
+    exit(-1);
+  }
+  if (srslte_chest_dl_set_cell(&est, cell)) {
     fprintf(stderr, "Error initializing equalizer\n");
     exit(-1);
   }
@@ -774,9 +793,11 @@ int main(int argc, char **argv) {
         }
       } else {
         INFO("SF: %d, Generating %d random bits\n", sf_idx, pdsch_cfg.grant.mcs[0].tbs + pdsch_cfg.grant.mcs[1].tbs);
-        for (uint32_t tb = 0; tb < pdsch_cfg.grant.nof_tb; tb++) {
-          for (i = 0; i < pdsch_cfg.grant.mcs[tb].tbs / 8; i++) {
-            data[tb][i] = (uint8_t) rand();
+        for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+          if (pdsch_cfg.grant.tb_en[tb]) {
+            for (i = 0; i < pdsch_cfg.grant.mcs[tb].tbs / 8; i++) {
+              data[tb][i] = (uint8_t) rand();
+            }
           }
         }
         /* Uncomment this to transmit on sf 0 and 5 only  */
@@ -832,10 +853,12 @@ int main(int argc, char **argv) {
         }        
         if (net_port > 0 && net_packet_ready) {
           if (null_file_sink) {
-            for (uint32_t tb = 0; tb < pdsch_cfg.grant.nof_tb; tb++) {
-              srslte_bit_pack_vector(data[tb], data_tmp, pdsch_cfg.grant.mcs[tb].tbs);
-              if (srslte_netsink_write(&net_sink, data_tmp, 1 + (pdsch_cfg.grant.mcs[tb].tbs - 1) / 8) < 0) {
-                fprintf(stderr, "Error sending data through UDP socket\n");
+            for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+              if (pdsch_cfg.grant.tb_en[tb]) {
+                srslte_bit_pack_vector(data[tb], data_tmp, pdsch_cfg.grant.mcs[tb].tbs);
+                if (srslte_netsink_write(&net_sink, data_tmp, 1 + (pdsch_cfg.grant.mcs[tb].tbs - 1) / 8) < 0) {
+                  fprintf(stderr, "Error sending data through UDP socket\n");
+                }
               }
             }
           }
diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 2ace49d6d..a4279ba1a 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -475,23 +475,42 @@ int main(int argc, char **argv) {
              //ue_sync.decimate = prog_args.decimate;
           }
       }
-    if (srslte_ue_sync_init_multi_decim(&ue_sync, cell, srslte_rf_recv_wrapper, prog_args.rf_nof_rx_ant, (void*) &rf,decimate)) {
+    if (srslte_ue_sync_init_multi_decim(&ue_sync,
+                                        cell.nof_prb,
+                                        cell.id==1000,
+                                        srslte_rf_recv_wrapper,
+                                        prog_args.rf_nof_rx_ant,
+                                        (void*) &rf,decimate))
+    {
       fprintf(stderr, "Error initiating ue_sync\n");
       exit(-1); 
     }
+    if (srslte_ue_sync_set_cell(&ue_sync, cell))
+    {
+      fprintf(stderr, "Error initiating ue_sync\n");
+      exit(-1);
+    }
 #endif
   }
 
-  if (srslte_ue_mib_init(&ue_mib, cell)) {
+  if (srslte_ue_mib_init(&ue_mib, cell.nof_prb)) {
     fprintf(stderr, "Error initaiting UE MIB decoder\n");
     exit(-1);
-  }    
+  }
+  if (srslte_ue_mib_set_cell(&ue_mib, cell)) {
+    fprintf(stderr, "Error initaiting UE MIB decoder\n");
+    exit(-1);
+  }
 
-  if (srslte_ue_dl_init(&ue_dl, cell, prog_args.rf_nof_rx_ant)) {  // This is the User RNTI
+  if (srslte_ue_dl_init(&ue_dl, cell.nof_prb, prog_args.rf_nof_rx_ant)) {
     fprintf(stderr, "Error initiating UE downlink processing module\n");
     exit(-1);
   }
-  
+  if (srslte_ue_dl_set_cell(&ue_dl, cell)) {
+    fprintf(stderr, "Error initiating UE downlink processing module\n");
+    exit(-1);
+  }
+
   for (int i=0;i<prog_args.rf_nof_rx_ant;i++) {
     sf_buffer[i] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(cell.nof_prb));
   }
@@ -518,7 +537,7 @@ int main(int argc, char **argv) {
     
   // Variables for measurements 
   uint32_t nframes=0;
-  uint32_t ri = 0, pmi = 0;
+  uint8_t ri = 0, pmi = 0;
   float rsrp0=0.0, rsrp1=0.0, rsrq=0.0, noise=0.0, enodebrate = 0.0, uerate = 0.0,
       sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS], cn = 0.0;
   bool decode_pdsch = false; 
@@ -566,6 +585,10 @@ int main(int argc, char **argv) {
       /* If a new line is detected set verbose level to Debug */
       if (fgets(input, sizeof(input), stdin)) {
         srslte_verbose = SRSLTE_VERBOSE_DEBUG;
+        ue_dl.pkt_errors = 0;
+        ue_dl.pkts_total = 0;
+        ue_dl.nof_detected = 0;
+        nof_trials = 0;
       }
     }
 
@@ -641,9 +664,10 @@ int main(int argc, char **argv) {
               /* Send data if socket active */
               if (prog_args.net_port > 0) {
                 // FIXME: UDP Data transmission does not work
-                for (uint32_t tb = 0; tb < ue_dl.pdsch_cfg.grant.nof_tb; tb++) {
-                  srslte_netsink_write(&net_sink, data[tb], 1 + (ue_dl.pdsch_cfg.grant.mcs[tb].tbs - 1) / 8);
-
+                for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+                  if (ue_dl.pdsch_cfg.grant.tb_en[tb]) {
+                    srslte_netsink_write(&net_sink, data[tb], 1 + (ue_dl.pdsch_cfg.grant.mcs[tb].tbs - 1) / 8);
+                  }
                 }
               }
               
@@ -704,7 +728,7 @@ int main(int argc, char **argv) {
 
             /* Print basic Parameters */
             PRINT_LINE("   nof layers: %d", ue_dl.pdsch_cfg.nof_layers);
-            PRINT_LINE("nof codewords: %d", ue_dl.pdsch_cfg.grant.nof_tb);
+            PRINT_LINE("nof codewords: %d", SRSLTE_RA_DL_GRANT_NOF_TB(&ue_dl.pdsch_cfg.grant));
             PRINT_LINE("          CFO: %+5.2f kHz", srslte_ue_sync_get_cfo(&ue_sync) / 1000);
             PRINT_LINE("          SNR: %+5.1f dB | %+5.1f dB", 10 * log10(rsrp0 / noise), 10 * log10(rsrp1 / noise));
             PRINT_LINE("           Rb: %6.2f / %6.2f Mbps (net/maximum)", uerate, enodebrate);
@@ -759,10 +783,12 @@ int main(int argc, char **argv) {
         if (sfn == 1024) {
           sfn = 0; 
           PRINT_LINE_ADVANCE_CURSOR();
+          /*
           ue_dl.pkt_errors = 0; 
           ue_dl.pkts_total = 0; 
           ue_dl.nof_detected = 0;           
-          nof_trials = 0; 
+          nof_trials = 0;
+          */
         } 
       }
       
diff --git a/lib/examples/usrp_capture_sync.c b/lib/examples/usrp_capture_sync.c
index 41c233be7..bac17698a 100644
--- a/lib/examples/usrp_capture_sync.c
+++ b/lib/examples/usrp_capture_sync.c
@@ -165,11 +165,15 @@ int main(int argc, char **argv) {
   cell.nof_prb = nof_prb; 
   cell.nof_ports = 1; 
   
-  if (srslte_ue_sync_init_multi(&ue_sync, cell, srslte_rf_recv_wrapper, nof_rx_antennas, (void*) &rf)) {
+  if (srslte_ue_sync_init_multi(&ue_sync, cell.nof_prb, cell.id==1000, srslte_rf_recv_wrapper, nof_rx_antennas, (void*) &rf)) {
     fprintf(stderr, "Error initiating ue_sync\n");
     exit(-1); 
   }
- 
+  if (srslte_ue_sync_set_cell(&ue_sync, cell)) {
+    fprintf(stderr, "Error initiating ue_sync\n");
+    exit(-1);
+  }
+
   uint32_t subframe_count = 0;
   bool start_capture = false; 
   bool stop_capture = false; 
diff --git a/lib/include/srslte/common/bcd_helpers.h b/lib/include/srslte/common/bcd_helpers.h
index d44a66483..55411ae33 100644
--- a/lib/include/srslte/common/bcd_helpers.h
+++ b/lib/include/srslte/common/bcd_helpers.h
@@ -30,6 +30,7 @@
 #include <ctype.h>
 #include <stdint.h>
 #include <string>
+#include <srslte/asn1/liblte_rrc.h>
 
 namespace srslte {
 
@@ -112,6 +113,12 @@ inline bool mnc_to_string(uint16_t mnc, std::string *str)
   *str += (mnc & 0x000F) + '0';
   return true;
 }
+inline std::string plmn_id_to_c_str(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {
+  std::string mcc_str, mnc_str;
+  mnc_to_string(plmn_id.mnc, &mnc_str);
+  mcc_to_string(plmn_id.mcc, &mcc_str);
+  return mcc_str + mnc_str;
+}
 
 } // namespace srslte
 
diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index 879118021..cf8a7c861 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -102,6 +102,15 @@ public:
   virtual void      write_pdu(uint32_t lcid, srslte::byte_buffer_t *pdu) = 0;
   virtual uint32_t  get_ul_count() = 0;
   virtual bool      get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi) = 0;
+  virtual void      plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) = 0;
+};
+
+// NAS interface for UE
+class nas_interface_ue
+{
+public:
+  virtual void      attach_request() = 0;
+  virtual void      deattach_request() = 0;
 };
 
 // RRC interface for MAC
@@ -123,6 +132,7 @@ class rrc_interface_phy
 public:
   virtual void in_sync() = 0;
   virtual void out_of_sync() = 0;
+  virtual void cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) = 0;
 };
 
 // RRC interface for NAS
@@ -133,18 +143,11 @@ public:
   virtual uint16_t get_mcc() = 0;
   virtual uint16_t get_mnc() = 0;
   virtual void enable_capabilities() = 0;
+  virtual void plmn_search() = 0;
+  virtual void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) = 0;
   virtual std::string get_rb_name(uint32_t lcid) = 0;
 };
 
-// RRC interface for GW
-class rrc_interface_gw
-{
-public:
-  virtual bool rrc_connected() = 0;
-  virtual void rrc_connect() = 0; 
-  virtual bool have_drb() = 0;
-};
-
 // RRC interface for PDCP
 class rrc_interface_pdcp
 {
@@ -281,6 +284,8 @@ public:
     bool        last_ndi[SRSLTE_MAX_CODEWORDS];
     uint32_t    n_bytes[SRSLTE_MAX_CODEWORDS];
     int         rv[SRSLTE_MAX_CODEWORDS];
+    bool        tb_en[SRSLTE_MAX_CODEWORDS];
+    bool        tb_cw_swap;
     uint16_t    rnti; 
     bool        is_from_rar;
     bool        is_sps_release;
@@ -421,8 +426,8 @@ typedef struct {
   int worker_cpu_mask;
   int sync_cpu_affinity;
   
-  uint32_t nof_rx_ant;   
-  std::string equalizer_mode; 
+  uint32_t nof_rx_ant;
+  std::string equalizer_mode;
   int cqi_max; 
   int cqi_fixed; 
   float snr_ema_coeff; 
@@ -442,9 +447,8 @@ class phy_interface_mac_common
 {
 public:
   /* Start synchronization with strongest cell in the current carrier frequency */
-  virtual void sync_start() = 0;
-  virtual void sync_stop() = 0;
-
+  virtual bool sync_status() = 0;
+  
   /* Sets a C-RNTI allowing the PHY to pregenerate signals if necessary */
   virtual void set_crnti(uint16_t rnti) = 0;
 
@@ -511,9 +515,15 @@ public:
   virtual void set_config_common(phy_cfg_common_t *common) = 0; 
   virtual void set_config_tdd(LIBLTE_RRC_TDD_CONFIG_STRUCT *tdd) = 0; 
   virtual void set_config_64qam_en(bool enable) = 0;
-  
+
+  /* Cell search and selection procedures */
+  virtual void cell_search_start() = 0;
+  virtual void cell_search_next() = 0;
+  virtual bool cell_select(uint32_t earfcn, srslte_cell_t cell) = 0;
+  virtual bool sync_stop() = 0;
+
   /* Is the PHY downlink synchronized? */
-  virtual bool status_is_sync() = 0;
+  virtual bool sync_status() = 0;
 
   /* Configure UL using parameters written with set_param() */
   virtual void configure_ul_params(bool pregen_disabled = false) = 0;
diff --git a/lib/include/srslte/phy/ch_estimation/chest_dl.h b/lib/include/srslte/phy/ch_estimation/chest_dl.h
index 5d0820aa4..c3fd795cf 100644
--- a/lib/include/srslte/phy/ch_estimation/chest_dl.h
+++ b/lib/include/srslte/phy/ch_estimation/chest_dl.h
@@ -92,9 +92,12 @@ typedef struct {
 
 
 SRSLTE_API int srslte_chest_dl_init(srslte_chest_dl_t *q, 
-                                    srslte_cell_t cell);
+                                    uint32_t max_prb);
 
-SRSLTE_API void srslte_chest_dl_free(srslte_chest_dl_t *q); 
+SRSLTE_API void srslte_chest_dl_free(srslte_chest_dl_t *q);
+
+SRSLTE_API int srslte_chest_dl_set_cell(srslte_chest_dl_t *q,
+                                        srslte_cell_t cell);
 
 SRSLTE_API void srslte_chest_dl_set_smooth_filter(srslte_chest_dl_t *q, 
                                                   float *filter, 
diff --git a/lib/include/srslte/phy/ch_estimation/chest_ul.h b/lib/include/srslte/phy/ch_estimation/chest_ul.h
index 1e505210c..0571fec8c 100644
--- a/lib/include/srslte/phy/ch_estimation/chest_ul.h
+++ b/lib/include/srslte/phy/ch_estimation/chest_ul.h
@@ -77,9 +77,13 @@ typedef struct {
 
 
 SRSLTE_API int srslte_chest_ul_init(srslte_chest_ul_t *q, 
-                                    srslte_cell_t cell);
+                                    uint32_t max_prb);
+
+SRSLTE_API void srslte_chest_ul_free(srslte_chest_ul_t *q);
+
+SRSLTE_API int srslte_chest_ul_set_cell(srslte_chest_ul_t *q,
+                                        srslte_cell_t cell);
 
-SRSLTE_API void srslte_chest_ul_free(srslte_chest_ul_t *q); 
 
 SRSLTE_API void srslte_chest_ul_set_cfg(srslte_chest_ul_t *q, 
                                         srslte_refsignal_dmrs_pusch_cfg_t *pusch_cfg,
diff --git a/lib/include/srslte/phy/ch_estimation/refsignal_dl.h b/lib/include/srslte/phy/ch_estimation/refsignal_dl.h
index 3cdf7901e..6b21c4fa2 100644
--- a/lib/include/srslte/phy/ch_estimation/refsignal_dl.h
+++ b/lib/include/srslte/phy/ch_estimation/refsignal_dl.h
@@ -53,7 +53,10 @@ typedef struct SRSLTE_API {
 
 
 SRSLTE_API int srslte_refsignal_cs_init(srslte_refsignal_cs_t *q, 
-                                        srslte_cell_t cell);
+                                        uint32_t max_prb);
+
+SRSLTE_API int srslte_refsignal_cs_set_cell(srslte_refsignal_cs_t * q,
+                                           srslte_cell_t cell);
 
 SRSLTE_API void srslte_refsignal_cs_free(srslte_refsignal_cs_t *q);
 
diff --git a/lib/include/srslte/phy/ch_estimation/refsignal_ul.h b/lib/include/srslte/phy/ch_estimation/refsignal_ul.h
index 72bfb5d94..9e1da9e50 100644
--- a/lib/include/srslte/phy/ch_estimation/refsignal_ul.h
+++ b/lib/include/srslte/phy/ch_estimation/refsignal_ul.h
@@ -94,7 +94,10 @@ typedef struct {
 } srslte_refsignal_srs_pregen_t;
 
 SRSLTE_API int srslte_refsignal_ul_init(srslte_refsignal_ul_t *q, 
-                                        srslte_cell_t cell);
+                                        uint32_t max_prb);
+
+SRSLTE_API int srslte_refsignal_ul_set_cell(srslte_refsignal_ul_t *q,
+                                            srslte_cell_t cell);
 
 SRSLTE_API void srslte_refsignal_ul_free(srslte_refsignal_ul_t *q);
 
@@ -115,9 +118,13 @@ SRSLTE_API uint32_t srslte_refsignal_dmrs_pucch_symbol(uint32_t m,
 
 SRSLTE_API bool srslte_refsignal_dmrs_pusch_cfg_isvalid(srslte_refsignal_ul_t *q, 
                                                         srslte_refsignal_dmrs_pusch_cfg_t *cfg, 
-                                                        uint32_t nof_prb); 
+                                                        uint32_t nof_prb);
 
-SRSLTE_API int srslte_refsignal_dmrs_pusch_pregen(srslte_refsignal_ul_t *q, 
+SRSLTE_API int srslte_refsignal_dmrs_pusch_pregen_init(srslte_refsignal_ul_t *q,
+                                                       srslte_refsignal_ul_dmrs_pregen_t *pregen,
+                                                       uint32_t max_prb);
+
+SRSLTE_API int srslte_refsignal_dmrs_pusch_pregen(srslte_refsignal_ul_t *q,
                                                   srslte_refsignal_ul_dmrs_pregen_t *pregen);
 
 SRSLTE_API void srslte_refsignal_dmrs_pusch_pregen_free(srslte_refsignal_ul_t *q, 
diff --git a/lib/include/srslte/phy/common/sequence.h b/lib/include/srslte/phy/common/sequence.h
index dbeba5ff4..4c7c2c945 100644
--- a/lib/include/srslte/phy/common/sequence.h
+++ b/lib/include/srslte/phy/common/sequence.h
@@ -44,7 +44,8 @@ typedef struct SRSLTE_API {
   uint8_t *c_bytes;
   float *c_float;
   short *c_short;
-  uint32_t len;
+  uint32_t cur_len;
+  uint32_t max_len;
 } srslte_sequence_t;
 
 SRSLTE_API int srslte_sequence_init(srslte_sequence_t *q, uint32_t len);
@@ -55,8 +56,9 @@ SRSLTE_API int srslte_sequence_LTE_pr(srslte_sequence_t *q,
                                       uint32_t len, 
                                       uint32_t seed);
 
-SRSLTE_API void srslte_sequence_set_LTE_pr(srslte_sequence_t *q, 
-                                           uint32_t seed); 
+SRSLTE_API int srslte_sequence_set_LTE_pr(srslte_sequence_t *q,
+                                           uint32_t len,
+                                           uint32_t seed);
 
 SRSLTE_API int srslte_sequence_pbch(srslte_sequence_t *seq, 
                                     srslte_cp_t cp, 
diff --git a/lib/include/srslte/phy/dft/dft.h b/lib/include/srslte/phy/dft/dft.h
index b90aab2e5..b7fc663d8 100644
--- a/lib/include/srslte/phy/dft/dft.h
+++ b/lib/include/srslte/phy/dft/dft.h
@@ -58,6 +58,7 @@ typedef enum {
 }srslte_dft_dir_t;
 
 typedef struct SRSLTE_API {
+  int init_size;      // DFT length used in the first initialization
   int size;           // DFT length
   void *in;           // Input buffer
   void *out;          // Output buffer
@@ -80,7 +81,7 @@ SRSLTE_API int srslte_dft_plan(srslte_dft_plan_t *plan,
                                srslte_dft_dir_t dir,                         
                                srslte_dft_mode_t type);
 
-SRSLTE_API int srslte_dft_plan_c(srslte_dft_plan_t *plan, 
+SRSLTE_API int srslte_dft_plan_c(srslte_dft_plan_t *plan,
                                  int dft_points, 
                                  srslte_dft_dir_t dir);
 
@@ -88,6 +89,16 @@ SRSLTE_API int srslte_dft_plan_r(srslte_dft_plan_t *plan,
                                  int dft_points, 
                                  srslte_dft_dir_t dir);
 
+SRSLTE_API int srslte_dft_replan(srslte_dft_plan_t *plan,
+                                 const int new_dft_points);
+
+SRSLTE_API int srslte_dft_replan_c(srslte_dft_plan_t *plan,
+                                   int new_dft_points);
+
+SRSLTE_API int srslte_dft_replan_r(srslte_dft_plan_t *plan,
+                                   int new_dft_points);
+
+
 SRSLTE_API void srslte_dft_plan_free(srslte_dft_plan_t *plan);
 
 /* Set options */
diff --git a/lib/include/srslte/phy/dft/dft_precoding.h b/lib/include/srslte/phy/dft/dft_precoding.h
index 6e5c5dc40..022593aa7 100644
--- a/lib/include/srslte/phy/dft/dft_precoding.h
+++ b/lib/include/srslte/phy/dft/dft_precoding.h
@@ -45,12 +45,18 @@ typedef struct SRSLTE_API {
 
   uint32_t max_prb;  
   srslte_dft_plan_t dft_plan[SRSLTE_MAX_PRB+1];
-  srslte_dft_plan_t idft_plan[SRSLTE_MAX_PRB+1];
-    
+
 }srslte_dft_precoding_t;
 
-SRSLTE_API int srslte_dft_precoding_init(srslte_dft_precoding_t *q, 
-                                         uint32_t max_prb);
+SRSLTE_API int srslte_dft_precoding_init(srslte_dft_precoding_t *q,
+                                         uint32_t max_prb,
+                                         bool is_tx);
+
+SRSLTE_API int srslte_dft_precoding_init_tx(srslte_dft_precoding_t *q,
+                                            uint32_t max_prb);
+
+SRSLTE_API int srslte_dft_precoding_init_rx(srslte_dft_precoding_t *q,
+                                            uint32_t max_prb);
 
 SRSLTE_API void srslte_dft_precoding_free(srslte_dft_precoding_t *q);
 
@@ -62,10 +68,4 @@ SRSLTE_API int srslte_dft_precoding(srslte_dft_precoding_t *q,
                                     uint32_t nof_prb, 
                                     uint32_t nof_symbols);
 
-SRSLTE_API int srslte_dft_predecoding(srslte_dft_precoding_t *q, 
-                                      cf_t *input, 
-                                      cf_t *output, 
-                                      uint32_t nof_prb, 
-                                      uint32_t nof_symbols);
-
 #endif
diff --git a/lib/include/srslte/phy/dft/ofdm.h b/lib/include/srslte/phy/dft/ofdm.h
index 990d7a8cc..175fe1ea9 100644
--- a/lib/include/srslte/phy/dft/ofdm.h
+++ b/lib/include/srslte/phy/dft/ofdm.h
@@ -47,6 +47,7 @@
 /* This is common for both directions */
 typedef struct SRSLTE_API{
   srslte_dft_plan_t fft_plan;
+  uint32_t max_prb;
   uint32_t nof_symbols;
   uint32_t symbol_sz;
   uint32_t nof_guards;
@@ -56,18 +57,27 @@ typedef struct SRSLTE_API{
   cf_t *tmp; // for removing zero padding
   
   bool freq_shift;
+  float freq_shift_f;
   cf_t *shift_buffer; 
 }srslte_ofdm_t;
 
 SRSLTE_API int srslte_ofdm_init_(srslte_ofdm_t *q, 
                                  srslte_cp_t cp, 
                                  int symbol_sz, 
-                                 int nof_prb, 
+                                 int max_prb,
                                  srslte_dft_dir_t dir); 
 
 SRSLTE_API int srslte_ofdm_rx_init(srslte_ofdm_t *q, 
                                srslte_cp_t cp_type, 
-                               uint32_t nof_prb);
+                               uint32_t max_prb);
+
+SRSLTE_API int srslte_ofdm_tx_set_prb(srslte_ofdm_t *q,
+                                      srslte_cp_t cp,
+                                      uint32_t nof_prb);
+
+SRSLTE_API int srslte_ofdm_rx_set_prb(srslte_ofdm_t *q,
+                                      srslte_cp_t cp,
+                                      uint32_t nof_prb);
 
 SRSLTE_API void srslte_ofdm_rx_free(srslte_ofdm_t *q);
 
diff --git a/lib/include/srslte/phy/enb/enb_dl.h b/lib/include/srslte/phy/enb/enb_dl.h
index d39eed1e1..f465d81e2 100644
--- a/lib/include/srslte/phy/enb/enb_dl.h
+++ b/lib/include/srslte/phy/enb/enb_dl.h
@@ -108,11 +108,14 @@ typedef struct {
 
 /* This function shall be called just after the initial synchronization */
 SRSLTE_API int srslte_enb_dl_init(srslte_enb_dl_t *q, 
-                                  srslte_cell_t cell);
+                                  uint32_t max_prb);
 
 SRSLTE_API void srslte_enb_dl_free(srslte_enb_dl_t *q);
 
-SRSLTE_API void srslte_enb_dl_set_cfi(srslte_enb_dl_t *q, 
+SRSLTE_API int srslte_enb_dl_set_cell(srslte_enb_dl_t *q,
+                                      srslte_cell_t cell);
+
+SRSLTE_API void srslte_enb_dl_set_cfi(srslte_enb_dl_t *q,
                                       uint32_t cfi);
 
 SRSLTE_API void srslte_enb_dl_set_amp(srslte_enb_dl_t *q, 
@@ -154,16 +157,18 @@ SRSLTE_API int srslte_enb_dl_put_pdsch(srslte_enb_dl_t *q,
                                        srslte_ra_dl_grant_t *grant, 
                                        srslte_softbuffer_tx_t *softbuffer[SRSLTE_MAX_CODEWORDS],
                                        uint16_t rnti,
-                                       uint32_t rv_idx, 
+                                       int rv_idx[SRSLTE_MAX_CODEWORDS],
                                        uint32_t sf_idx, 
-                                       uint8_t *data[SRSLTE_MAX_CODEWORDS]);
+                                       uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                                       srslte_mimo_type_t mimo_type,
+                                       uint32_t pmi);
 
 SRSLTE_API int srslte_enb_dl_put_pdcch_dl(srslte_enb_dl_t *q, 
                                           srslte_ra_dl_dci_t *grant, 
                                           srslte_dci_format_t format, 
                                           srslte_dci_location_t location,
                                           uint16_t rnti, 
-                                          uint32_t sf_idx); 
+                                          uint32_t sf_idx);
 
 SRSLTE_API int srslte_enb_dl_put_pdcch_ul(srslte_enb_dl_t *q, 
                                           srslte_ra_ul_dci_t *grant, 
diff --git a/lib/include/srslte/phy/enb/enb_ul.h b/lib/include/srslte/phy/enb/enb_ul.h
index 486566a1c..855b645ca 100644
--- a/lib/include/srslte/phy/enb/enb_ul.h
+++ b/lib/include/srslte/phy/enb/enb_ul.h
@@ -100,15 +100,18 @@ typedef struct {
 } srslte_enb_ul_pusch_t; 
 
 /* This function shall be called just after the initial synchronization */
-SRSLTE_API int srslte_enb_ul_init(srslte_enb_ul_t *q, 
-                                  srslte_cell_t cell, 
-                                  srslte_prach_cfg_t* prach_cfg, 
-                                  srslte_refsignal_dmrs_pusch_cfg_t *pusch_cfg,
-                                  srslte_pusch_hopping_cfg_t *hopping_cfg,
-                                  srslte_pucch_cfg_t *pucch_cfg);
+SRSLTE_API int srslte_enb_ul_init(srslte_enb_ul_t *q,
+                                  uint32_t max_prb);
 
 SRSLTE_API void srslte_enb_ul_free(srslte_enb_ul_t *q);
 
+SRSLTE_API int srslte_enb_ul_set_cell(srslte_enb_ul_t *q,
+                                      srslte_cell_t cell,
+                                      srslte_prach_cfg_t* prach_cfg,
+                                      srslte_refsignal_dmrs_pusch_cfg_t *pusch_cfg,
+                                      srslte_pusch_hopping_cfg_t *hopping_cfg,
+                                      srslte_pucch_cfg_t *pucch_cfg);
+
 SRSLTE_API int srslte_enb_ul_add_rnti(srslte_enb_ul_t *q, 
                                       uint16_t rnti); 
 
diff --git a/lib/include/srslte/phy/phch/cqi.h b/lib/include/srslte/phy/phch/cqi.h
index 593592f1c..cfc9e92e7 100644
--- a/lib/include/srslte/phy/phch/cqi.h
+++ b/lib/include/srslte/phy/phch/cqi.h
@@ -41,11 +41,14 @@
 #include "srslte/phy/common/phy_common.h"
 
 #define SRSLTE_CQI_MAX_BITS 64
+#define SRSLTE_DIF_CQI_MAX_BITS 3
+#define SRSLTE_PMI_MAX_BITS 4
 
 typedef struct {
   bool     configured; 
   uint32_t pmi_idx; 
   uint32_t ri_idx;
+  bool ri_idx_present;
   bool     simul_cqi_ack;
   bool     format_is_subband; 
   uint32_t subband_size; 
diff --git a/lib/include/srslte/phy/phch/pbch.h b/lib/include/srslte/phy/phch/pbch.h
index 8489df05a..4dc7ec727 100644
--- a/lib/include/srslte/phy/phch/pbch.h
+++ b/lib/include/srslte/phy/phch/pbch.h
@@ -53,6 +53,8 @@
 #define SRSLTE_BCH_PAYLOADCRC_LEN  (SRSLTE_BCH_PAYLOAD_LEN+16)
 #define SRSLTE_BCH_ENCODED_LEN     3*(SRSLTE_BCH_PAYLOADCRC_LEN)
 
+#define SRSLTE_PBCH_MAX_RE 256 // make it avx2-aligned
+
 /* PBCH object */
 typedef struct SRSLTE_API {
   srslte_cell_t cell;
@@ -83,12 +85,14 @@ typedef struct SRSLTE_API {
   
 } srslte_pbch_t;
 
-SRSLTE_API int srslte_pbch_init(srslte_pbch_t *q,
-                         srslte_cell_t cell);
+SRSLTE_API int srslte_pbch_init(srslte_pbch_t *q);
 
 SRSLTE_API void srslte_pbch_free(srslte_pbch_t *q);
 
-SRSLTE_API int srslte_pbch_decode(srslte_pbch_t *q, 
+SRSLTE_API int srslte_pbch_set_cell(srslte_pbch_t *q,
+                                    srslte_cell_t cell);
+
+SRSLTE_API int srslte_pbch_decode(srslte_pbch_t *q,
                            cf_t *slot1_symbols, 
                            cf_t *ce_slot1[SRSLTE_MAX_PORTS], 
                            float noise_estimate, 
diff --git a/lib/include/srslte/phy/phch/pcfich.h b/lib/include/srslte/phy/phch/pcfich.h
index 672caed2c..fbbb82c88 100644
--- a/lib/include/srslte/phy/phch/pcfich.h
+++ b/lib/include/srslte/phy/phch/pcfich.h
@@ -78,14 +78,12 @@ typedef struct SRSLTE_API {
   
 } srslte_pcfich_t;
 
-SRSLTE_API int srslte_pcfich_init(srslte_pcfich_t *q, 
-                           srslte_regs_t *regs, 
-                           srslte_cell_t cell);
+SRSLTE_API int srslte_pcfich_init(srslte_pcfich_t *q,
+                                  uint32_t nof_rx_antennas);
 
-SRSLTE_API int srslte_pcfich_init_multi(srslte_pcfich_t *q, 
-                                        srslte_regs_t *regs, 
-                                        srslte_cell_t cell, 
-                                        uint32_t nof_rx_antennas);
+SRSLTE_API int srslte_pcfich_set_cell(srslte_pcfich_t *q,
+                                      srslte_regs_t *regs,
+                                      srslte_cell_t cell);
 
 SRSLTE_API void srslte_pcfich_free(srslte_pcfich_t *q);
 
diff --git a/lib/include/srslte/phy/phch/pdcch.h b/lib/include/srslte/phy/phch/pdcch.h
index f67d29dcb..9bb896b34 100644
--- a/lib/include/srslte/phy/phch/pdcch.h
+++ b/lib/include/srslte/phy/phch/pdcch.h
@@ -64,6 +64,7 @@ typedef struct SRSLTE_API {
   uint32_t nof_cce;
   uint32_t max_bits;
   uint32_t nof_rx_antennas;
+  bool     is_ue;
   
   srslte_regs_t *regs;
 
@@ -84,29 +85,16 @@ typedef struct SRSLTE_API {
   
 } srslte_pdcch_t;
 
-SRSLTE_API int srslte_pdcch_init(srslte_pdcch_t *q, 
-                                 srslte_regs_t *regs, 
-                                 srslte_cell_t cell);
+SRSLTE_API int srslte_pdcch_init_ue(srslte_pdcch_t *q,
+                                    uint32_t max_prb,
+                                    uint32_t nof_rx_antennas);
 
-SRSLTE_API int srslte_pdcch_init_tx(srslte_pdcch_t *q,
-                         srslte_regs_t *regs,
-                         srslte_cell_t cell);
+SRSLTE_API int srslte_pdcch_init_enb(srslte_pdcch_t *q,
+                                     uint32_t max_prb);
 
-SRSLTE_API int srslte_pdcch_init_rx(srslte_pdcch_t *q,
-                         srslte_regs_t *regs,
-                         srslte_cell_t cell,
-                         uint32_t nof_rx_antennas);
-
-SRSLTE_API int srslte_pdcch_init_multi(srslte_pdcch_t *q, 
-                                       srslte_regs_t *regs, 
-                                       srslte_cell_t cell, 
-                                       uint32_t nof_rx_antennas);
-
-SRSLTE_API int srslte_pdcch_init_txrx(srslte_pdcch_t *q,
-                                      srslte_regs_t *regs,
-                                      srslte_cell_t cell,
-                                      uint32_t nof_rx_antennas,
-                                      bool isReceiver);
+SRSLTE_API int srslte_pdcch_set_cell(srslte_pdcch_t *q,
+                                     srslte_regs_t *regs,
+                                     srslte_cell_t cell);
 
 SRSLTE_API void srslte_pdcch_free(srslte_pdcch_t *q);
 
diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index 44ed161e8..7ef2d9b3d 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -49,6 +49,7 @@
 
 typedef struct {
   srslte_sequence_t seq[SRSLTE_MAX_CODEWORDS][SRSLTE_NSUBFRAMES_X_FRAME];
+  uint32_t cell_id;
   bool sequence_generated;
 } srslte_pdsch_user_t;
 
@@ -59,7 +60,10 @@ typedef struct SRSLTE_API {
   uint32_t nof_rx_antennas;
   
   uint32_t max_re;
-  
+
+  uint16_t ue_rnti;
+  bool is_ue;
+
   /* buffers */
   // void buffers are shared for tx and rx
   cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS]; /* Channel estimation (Rx only) */
@@ -73,21 +77,27 @@ typedef struct SRSLTE_API {
   
   // This is to generate the scrambling seq for multiple CRNTIs
   srslte_pdsch_user_t **users;
-  
+
+  srslte_sequence_t tmp_seq;
+
   srslte_sch_t dl_sch;
 
 } srslte_pdsch_t;
 
-SRSLTE_API int srslte_pdsch_init_tx(srslte_pdsch_t *q,
-                                    srslte_cell_t cell);
 
-SRSLTE_API int srslte_pdsch_init_rx(srslte_pdsch_t *q,
-                                    srslte_cell_t cell,
-                                    uint32_t nof_antennas);
+SRSLTE_API int srslte_pdsch_init_ue(srslte_pdsch_t *q,
+                                    uint32_t max_prb,
+                                    uint32_t nof_rx_antennas);
+
+SRSLTE_API int srslte_pdsch_init_enb(srslte_pdsch_t *q,
+                                     uint32_t max_prb);
 
 SRSLTE_API void srslte_pdsch_free(srslte_pdsch_t *q);
 
-SRSLTE_API int srslte_pdsch_set_rnti(srslte_pdsch_t *q, 
+SRSLTE_API int srslte_pdsch_set_cell(srslte_pdsch_t *q,
+                                     srslte_cell_t cell);
+
+SRSLTE_API int srslte_pdsch_set_rnti(srslte_pdsch_t *q,
                                      uint16_t rnti);
 
 SRSLTE_API void srslte_pdsch_free_rnti(srslte_pdsch_t *q, 
diff --git a/lib/include/srslte/phy/phch/phich.h b/lib/include/srslte/phy/phch/phich.h
index 18454a2d5..f560fdc80 100644
--- a/lib/include/srslte/phy/phch/phich.h
+++ b/lib/include/srslte/phy/phch/phich.h
@@ -85,18 +85,16 @@ typedef struct SRSLTE_API {
   
 } srslte_phich_t;
 
-SRSLTE_API int srslte_phich_init(srslte_phich_t *q, 
-                                 srslte_regs_t *regs, 
-                                 srslte_cell_t cell);
-
-SRSLTE_API int srslte_phich_init_multi(srslte_phich_t *q, 
-                                       srslte_regs_t *regs, 
-                                       srslte_cell_t cell, 
-                                       uint32_t nof_rx_antennas);
+SRSLTE_API int srslte_phich_init(srslte_phich_t *q,
+                                 uint32_t nof_rx_antennas);
 
 SRSLTE_API void srslte_phich_free(srslte_phich_t *q);
 
-SRSLTE_API void srslte_phich_calc(srslte_phich_t *q, 
+SRSLTE_API int srslte_phich_set_cell(srslte_phich_t *q,
+                                     srslte_regs_t *regs,
+                                     srslte_cell_t cell);
+
+SRSLTE_API void srslte_phich_calc(srslte_phich_t *q,
                                   uint32_t n_prb_lowest, 
                                   uint32_t n_dmrs, 
                                   uint32_t *ngroup, 
diff --git a/lib/include/srslte/phy/phch/prach.h b/lib/include/srslte/phy/phch/prach.h
index a08256a57..3e3df01bb 100644
--- a/lib/include/srslte/phy/phch/prach.h
+++ b/lib/include/srslte/phy/phch/prach.h
@@ -44,6 +44,8 @@
 #include "srslte/phy/common/phy_common.h"
 
 
+#define SRSLTE_PRACH_MAX_LEN  (2*24576+21024) // Maximum Tcp + Tseq
+
 /** Generation and detection of RACH signals for uplink.
  *  Currently only supports preamble formats 0-3.
  *  Does not currently support high speed flag.
@@ -60,6 +62,8 @@ typedef struct SRSLTE_API {
   uint32_t N_ifft_ul;       // IFFT size for uplink
   uint32_t N_ifft_prach;    // IFFT size for PRACH generation
 
+  uint32_t max_N_ifft_ul;
+
   // Working parameters
   uint32_t N_zc;  // PRACH sequence length
   uint32_t N_cs;  // Cyclic shift size
@@ -82,12 +86,12 @@ typedef struct SRSLTE_API {
   float *corr;
 
   // PRACH IFFT
-  srslte_dft_plan_t *fft;
-  srslte_dft_plan_t *ifft;
+  srslte_dft_plan_t fft;
+  srslte_dft_plan_t ifft;
 
   // ZC-sequence FFT and IFFT
-  srslte_dft_plan_t *zc_fft;
-  srslte_dft_plan_t *zc_ifft;
+  srslte_dft_plan_t zc_fft;
+  srslte_dft_plan_t zc_ifft;
   
   cf_t *signal_fft; 
   float detect_factor; 
@@ -129,11 +133,14 @@ SRSLTE_API void srslte_prach_sf_config(uint32_t config_idx,
                                        srslte_prach_sf_config_t *sf_config);
 
 SRSLTE_API int srslte_prach_init(srslte_prach_t *p,
-                                 uint32_t N_ifft_ul,
-                                 uint32_t config_idx,
-                                 uint32_t root_seq_index,
-                                 bool high_speed_flag,
-                                 uint32_t zero_corr_zone_config);
+                                 uint32_t max_N_ifft_ul);
+
+SRSLTE_API int srslte_prach_set_cell(srslte_prach_t *p,
+                                     uint32_t N_ifft_ul,
+                                     uint32_t config_idx,
+                                     uint32_t root_seq_index,
+                                     bool high_speed_flag,
+                                     uint32_t zero_corr_zone_config);
 
 SRSLTE_API int srslte_prach_init_cfg(srslte_prach_t* p, 
                                      srslte_prach_cfg_t* cfg, 
diff --git a/lib/include/srslte/phy/phch/pucch.h b/lib/include/srslte/phy/phch/pucch.h
index da0b5b81e..8aa1495c8 100644
--- a/lib/include/srslte/phy/phch/pucch.h
+++ b/lib/include/srslte/phy/phch/pucch.h
@@ -45,7 +45,7 @@
 #define SRSLTE_PUCCH_N_SEQ       12 
 #define SRSLTE_PUCCH2_NOF_BITS   SRSLTE_UCI_CQI_CODED_PUCCH_B
 #define SRSLTE_PUCCH_MAX_BITS    SRSLTE_CQI_MAX_BITS
-#define SRSLTE_PUCCH_MAX_SYMBOLS 120
+#define SRSLTE_PUCCH_MAX_SYMBOLS 128
 
 typedef enum SRSLTE_API {
   SRSLTE_PUCCH_FORMAT_1 = 0, 
@@ -115,12 +115,14 @@ typedef struct SRSLTE_API {
 }srslte_pucch_t;
 
 
-SRSLTE_API int srslte_pucch_init(srslte_pucch_t *q, 
-                                 srslte_cell_t cell);
+SRSLTE_API int srslte_pucch_init(srslte_pucch_t *q);
 
 SRSLTE_API void srslte_pucch_free(srslte_pucch_t *q);
 
-SRSLTE_API bool srslte_pucch_set_cfg(srslte_pucch_t* q, 
+SRSLTE_API int srslte_pucch_set_cell(srslte_pucch_t *q,
+                                     srslte_cell_t cell);
+
+SRSLTE_API bool srslte_pucch_set_cfg(srslte_pucch_t* q,
                                      srslte_pucch_cfg_t* cfg, 
                                      bool group_hopping_en); 
 
diff --git a/lib/include/srslte/phy/phch/pusch.h b/lib/include/srslte/phy/phch/pusch.h
index e5ee43995..2328c8ff3 100644
--- a/lib/include/srslte/phy/phch/pusch.h
+++ b/lib/include/srslte/phy/phch/pusch.h
@@ -62,13 +62,16 @@ typedef struct {
 
 typedef struct {
   srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];
-  bool sequences_generated;
-} srslte_pusch_user_t; 
+  uint32_t cell_id;
+  bool sequence_generated;
+} srslte_pusch_user_t;
 
 /* PUSCH object */
 typedef struct SRSLTE_API {
   srslte_cell_t cell;
 
+  bool is_ue;
+  uint16_t ue_rnti;
   uint32_t max_re;
 
   srslte_dft_precoding_t dft_precoding;  
@@ -96,12 +99,18 @@ typedef struct SRSLTE_API {
 }srslte_pusch_t;
 
 
-SRSLTE_API int srslte_pusch_init(srslte_pusch_t *q, 
-                                 srslte_cell_t cell);
+SRSLTE_API int srslte_pusch_init_ue(srslte_pusch_t *q,
+                                    uint32_t max_prb);
+
+SRSLTE_API int srslte_pusch_init_enb(srslte_pusch_t *q,
+                                    uint32_t max_prb);
 
 SRSLTE_API void srslte_pusch_free(srslte_pusch_t *q);
 
-SRSLTE_API int srslte_pusch_cfg(srslte_pusch_t             *q, 
+SRSLTE_API int srslte_pusch_set_cell(srslte_pusch_t *q,
+                                     srslte_cell_t cell);
+
+SRSLTE_API int srslte_pusch_cfg(srslte_pusch_t             *q,
                                 srslte_pusch_cfg_t         *cfg, 
                                 srslte_ra_ul_grant_t       *grant, 
                                 srslte_uci_cfg_t           *uci_cfg, 
@@ -114,7 +123,7 @@ SRSLTE_API int srslte_pusch_cfg(srslte_pusch_t             *q,
 SRSLTE_API int srslte_pusch_set_rnti(srslte_pusch_t *q, 
                                      uint16_t rnti);
 
-SRSLTE_API void srslte_pusch_clear_rnti(srslte_pusch_t *q, 
+SRSLTE_API void srslte_pusch_free_rnti(srslte_pusch_t *q,
                                        uint16_t rnti);
 
 SRSLTE_API int srslte_pusch_encode(srslte_pusch_t *q, 
diff --git a/lib/include/srslte/phy/phch/ra.h b/lib/include/srslte/phy/phch/ra.h
index 9f31a0218..94cc9a084 100644
--- a/lib/include/srslte/phy/phch/ra.h
+++ b/lib/include/srslte/phy/phch/ra.h
@@ -104,10 +104,12 @@ typedef struct SRSLTE_API {
   uint32_t nof_prb;  
   uint32_t Qm[SRSLTE_MAX_CODEWORDS];
   srslte_ra_mcs_t mcs[SRSLTE_MAX_CODEWORDS];
-  uint32_t nof_tb;
+  bool tb_en[SRSLTE_MAX_CODEWORDS];
   uint32_t pinfo;
 } srslte_ra_dl_grant_t;
 
+#define SRSLTE_RA_DL_GRANT_NOF_TB(G) ((((G)->tb_en[0])?1:0)+(((G)->tb_en[1])?1:0))
+
 /** Unpacked DCI message for DL grant */
 typedef struct SRSLTE_API {
   
diff --git a/lib/include/srslte/phy/phch/uci.h b/lib/include/srslte/phy/phch/uci.h
index 01eae8e89..ff315384e 100644
--- a/lib/include/srslte/phy/phch/uci.h
+++ b/lib/include/srslte/phy/phch/uci.h
@@ -64,6 +64,10 @@ typedef struct SRSLTE_API {
 typedef struct SRSLTE_API {
   uint8_t  uci_cqi[SRSLTE_CQI_MAX_BITS];
   uint32_t uci_cqi_len;
+  uint8_t  uci_dif_cqi[SRSLTE_DIF_CQI_MAX_BITS];
+  uint32_t uci_dif_cqi_len;
+  uint8_t  uci_pmi[SRSLTE_PMI_MAX_BITS];
+  uint8_t  uci_pmi_len;
   uint8_t  uci_ri;  // Only 1-bit supported for RI
   uint32_t uci_ri_len;
   uint8_t  uci_ack;   // 1st codeword bit for HARQ-ACK
@@ -119,8 +123,9 @@ SRSLTE_API int srslte_uci_decode_cqi_pusch(srslte_uci_cqi_pusch_t *q,
                                            bool *cqi_ack); 
 
 SRSLTE_API int srslte_uci_encode_ack(srslte_pusch_cfg_t *cfg,
-                                     uint8_t data, 
-                                     uint32_t O_cqi, 
+                                     uint8_t acks[2],
+                                     uint32_t nof_acks,
+                                     uint32_t O_cqi,
                                      float beta, 
                                      uint32_t H_prime_total, 
                                      srslte_uci_bit_t *ri_bits); 
@@ -132,7 +137,8 @@ SRSLTE_API int srslte_uci_decode_ack(srslte_pusch_cfg_t *cfg,
                                      uint32_t H_prime_total, 
                                      uint32_t O_cqi,
                                      srslte_uci_bit_t *ack_bits,
-                                     uint8_t *data); 
+                                     uint8_t acks[2],
+                                     uint32_t nof_acks);
 
 SRSLTE_API int srslte_uci_encode_ri(srslte_pusch_cfg_t *cfg,
                                     uint8_t data, 
diff --git a/lib/include/srslte/phy/resampling/interp.h b/lib/include/srslte/phy/resampling/interp.h
index 0bc2a6e39..508b6e957 100644
--- a/lib/include/srslte/phy/resampling/interp.h
+++ b/lib/include/srslte/phy/resampling/interp.h
@@ -67,7 +67,8 @@ SRSLTE_API void srslte_interp_linear_f(float *input,
 
 typedef struct {
   cf_t *diff_vec; 
-  uint32_t vector_len; 
+  uint32_t vector_len;
+  uint32_t max_vector_len;
 } srslte_interp_linsrslte_vec_t;
 
 SRSLTE_API int srslte_interp_linear_vector_init(srslte_interp_linsrslte_vec_t *q, 
@@ -75,6 +76,9 @@ SRSLTE_API int srslte_interp_linear_vector_init(srslte_interp_linsrslte_vec_t *q
 
 SRSLTE_API void srslte_interp_linear_vector_free(srslte_interp_linsrslte_vec_t *q); 
 
+SRSLTE_API int srslte_interp_linear_vector_resize(srslte_interp_linsrslte_vec_t *q,
+                                                  uint32_t vector_len);
+
 SRSLTE_API void srslte_interp_linear_vector(srslte_interp_linsrslte_vec_t *q, 
                                             cf_t *in0, 
                                             cf_t *in1, 
@@ -107,7 +111,9 @@ typedef struct {
   cf_t *diff_vec2;
   float *ramp;
   uint32_t vector_len; 
-  uint32_t M; 
+  uint32_t M;
+  uint32_t max_vector_len;
+  uint32_t max_M;
 } srslte_interp_lin_t;
 
 SRSLTE_API int srslte_interp_linear_init(srslte_interp_lin_t *q, 
@@ -116,6 +122,10 @@ SRSLTE_API int srslte_interp_linear_init(srslte_interp_lin_t *q,
 
 SRSLTE_API void srslte_interp_linear_free(srslte_interp_lin_t *q);
 
+SRSLTE_API int srslte_interp_linear_resize(srslte_interp_lin_t *q,
+                                           uint32_t vector_len,
+                                           uint32_t M);
+
 SRSLTE_API void srslte_interp_linear_offset(srslte_interp_lin_t *q, 
                                             cf_t *input, 
                                             cf_t *output, 
diff --git a/lib/include/srslte/phy/sync/cfo.h b/lib/include/srslte/phy/sync/cfo.h
index 642e81fc4..fd2ab4d83 100644
--- a/lib/include/srslte/phy/sync/cfo.h
+++ b/lib/include/srslte/phy/sync/cfo.h
@@ -49,6 +49,7 @@ typedef struct SRSLTE_API {
   float last_freq;
   float tol;
   int nsamples;
+  int max_samples;
   srslte_cexptab_t tab;
   cf_t *cur_cexp;
 }srslte_cfo_t;
@@ -58,8 +59,8 @@ SRSLTE_API int srslte_cfo_init(srslte_cfo_t *h,
 
 SRSLTE_API void srslte_cfo_free(srslte_cfo_t *h);
 
-SRSLTE_API int srslte_cfo_realloc(srslte_cfo_t *h, 
-                                  uint32_t samples);
+SRSLTE_API int srslte_cfo_resize(srslte_cfo_t *h,
+                                 uint32_t samples);
 
 SRSLTE_API void srslte_cfo_set_tol(srslte_cfo_t *h, 
                                    float tol);
diff --git a/lib/include/srslte/phy/sync/cp.h b/lib/include/srslte/phy/sync/cp.h
index 211479030..9daf7d4f6 100644
--- a/lib/include/srslte/phy/sync/cp.h
+++ b/lib/include/srslte/phy/sync/cp.h
@@ -35,6 +35,7 @@
 typedef struct {
   cf_t *corr;
   uint32_t symbol_sz;
+  uint32_t max_symbol_sz;
 } srslte_cp_synch_t;
 
 SRSLTE_API int srslte_cp_synch_init(srslte_cp_synch_t *q, 
@@ -42,6 +43,9 @@ SRSLTE_API int srslte_cp_synch_init(srslte_cp_synch_t *q,
 
 SRSLTE_API void srslte_cp_synch_free(srslte_cp_synch_t *q);
 
+SRSLTE_API int srslte_cp_synch_resize(srslte_cp_synch_t *q,
+                                      uint32_t symbol_sz);
+
 SRSLTE_API uint32_t srslte_cp_synch(srslte_cp_synch_t *q, 
                                     cf_t *input, 
                                     uint32_t max_offset, 
diff --git a/lib/include/srslte/phy/sync/pss.h b/lib/include/srslte/phy/sync/pss.h
index f18c5b6af..9760a178c 100644
--- a/lib/include/srslte/phy/sync/pss.h
+++ b/lib/include/srslte/phy/sync/pss.h
@@ -79,6 +79,10 @@ typedef struct SRSLTE_API {
 
 #endif
   int decimate;
+
+  uint32_t max_frame_size;
+  uint32_t max_fft_size;
+
   uint32_t frame_size;
   uint32_t N_id_2;
   uint32_t fft_size;
@@ -113,6 +117,10 @@ SRSLTE_API int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q,
                                                 int cfo_i,
                                                 int decimate);
 
+SRSLTE_API int srslte_pss_synch_resize(srslte_pss_synch_t *q, uint32_t frame_size,
+                                       uint32_t fft_size,
+                                       int offset);
+
 SRSLTE_API int srslte_pss_synch_init(srslte_pss_synch_t *q, 
                                      uint32_t frame_size);
 
diff --git a/lib/include/srslte/phy/sync/sss.h b/lib/include/srslte/phy/sync/sss.h
index aca57e860..35eedac8f 100644
--- a/lib/include/srslte/phy/sync/sss.h
+++ b/lib/include/srslte/phy/sync/sss.h
@@ -70,6 +70,7 @@ typedef struct SRSLTE_API {
   srslte_dft_plan_t dftp_input;
   
   uint32_t fft_size;
+  uint32_t max_fft_size;
 
   float corr_peak_threshold;
   uint32_t symbol_sz;
@@ -89,7 +90,7 @@ typedef struct SRSLTE_API {
 SRSLTE_API int srslte_sss_synch_init(srslte_sss_synch_t *q,
                                      uint32_t fft_size);
 
-SRSLTE_API int srslte_sss_synch_realloc(srslte_sss_synch_t *q, 
+SRSLTE_API int srslte_sss_synch_resize(srslte_sss_synch_t *q,
                                         uint32_t fft_size); 
 
 SRSLTE_API void srslte_sss_synch_free(srslte_sss_synch_t *q);
diff --git a/lib/include/srslte/phy/sync/sync.h b/lib/include/srslte/phy/sync/sync.h
index ab22d64fc..1c70b2449 100644
--- a/lib/include/srslte/phy/sync/sync.h
+++ b/lib/include/srslte/phy/sync/sync.h
@@ -75,6 +75,8 @@ typedef struct SRSLTE_API {
   uint32_t frame_size;
   uint32_t max_offset;
   bool enable_cfo_corr;
+  bool mean_cfo2_isunset;
+  bool mean_cfo_isunset;
   float mean_cfo;
   float mean_cfo2; 
   int cfo_i;
@@ -85,6 +87,7 @@ typedef struct SRSLTE_API {
   uint32_t cp_len;
   srslte_cfo_t cfocorr;
   srslte_cfo_t cfocorr2;
+  float current_cfo_tol;
   sss_alg_t sss_alg; 
   bool detect_cp;
   bool sss_en;
@@ -97,6 +100,8 @@ typedef struct SRSLTE_API {
   float M_ext_avg; 
   cf_t  *temp;
 
+  uint32_t max_frame_size;
+
 }srslte_sync_t;
 
 typedef enum {
@@ -121,6 +126,11 @@ SRSLTE_API int srslte_sync_init_decim(srslte_sync_t *q,
 
 SRSLTE_API void srslte_sync_free(srslte_sync_t *q);
 
+SRSLTE_API int srslte_sync_resize(srslte_sync_t *q,
+                                  uint32_t frame_size,
+                                  uint32_t max_offset,
+                                  uint32_t fft_size);
+
 SRSLTE_API void srslte_sync_reset(srslte_sync_t *q); 
 
 /* Finds a correlation peak in the input signal around position find_offset */
@@ -138,6 +148,9 @@ SRSLTE_API srslte_cp_t srslte_sync_detect_cp(srslte_sync_t *q,
 SRSLTE_API void srslte_sync_set_threshold(srslte_sync_t *q, 
                                           float threshold);
 
+SRSLTE_API void srslte_sync_set_cfo_tol(srslte_sync_t *q,
+                                        float tol);
+
 /* Gets the subframe idx (0 or 5) */
 SRSLTE_API uint32_t srslte_sync_get_sf_idx(srslte_sync_t *q);
 
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index d0723fae7..d88148e9e 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -119,13 +119,16 @@ typedef struct SRSLTE_API {
 
 /* This function shall be called just after the initial synchronization */
 SRSLTE_API int srslte_ue_dl_init(srslte_ue_dl_t *q,
-                                 srslte_cell_t cell,
+                                 uint32_t max_prb,
                                  uint32_t nof_rx_antennas);
 
 SRSLTE_API void srslte_ue_dl_free(srslte_ue_dl_t *q);
 
+SRSLTE_API int srslte_ue_dl_set_cell(srslte_ue_dl_t *q,
+                                          srslte_cell_t cell);
+
 SRSLTE_API int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q,
-                                                cf_t *input[SRSLTE_MAX_PORTS], 
+                                                cf_t *input[SRSLTE_MAX_PORTS],
                                                 uint32_t sf_idx, 
                                                 uint32_t *cfi); 
 
@@ -182,12 +185,12 @@ SRSLTE_API int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q,
                                         bool acks[SRSLTE_MAX_CODEWORDS]);
 
 SRSLTE_API int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q,
-                                          uint32_t *ri,
-                                          uint32_t *pmi,
+                                          uint8_t *ri,
+                                          uint8_t *pmi,
                                           float *current_sinr);
 
 SRSLTE_API int srslte_ue_dl_ri_select(srslte_ue_dl_t *q,
-                                      uint32_t *ri,
+                                      uint8_t *ri,
                                       float *cn);
 
 SRSLTE_API bool srslte_ue_dl_decode_phich(srslte_ue_dl_t *q, 
diff --git a/lib/include/srslte/phy/ue/ue_mib.h b/lib/include/srslte/phy/ue/ue_mib.h
index abefa014f..5d2ef14fa 100644
--- a/lib/include/srslte/phy/ue/ue_mib.h
+++ b/lib/include/srslte/phy/ue/ue_mib.h
@@ -79,10 +79,13 @@ typedef struct SRSLTE_API {
 } srslte_ue_mib_t;
 
 SRSLTE_API int srslte_ue_mib_init(srslte_ue_mib_t *q, 
-                                  srslte_cell_t cell);
+                                  uint32_t max_prb);
 
 SRSLTE_API void srslte_ue_mib_free(srslte_ue_mib_t *q);
 
+SRSLTE_API int srslte_ue_mib_set_cell(srslte_ue_mib_t * q,
+                                      srslte_cell_t cell);
+
 SRSLTE_API void srslte_ue_mib_reset(srslte_ue_mib_t * q); 
 
 SRSLTE_API int srslte_ue_mib_decode(srslte_ue_mib_t * q, 
@@ -104,21 +107,17 @@ typedef struct {
   uint32_t nof_rx_antennas;
 } srslte_ue_mib_sync_t;
 
-SRSLTE_API int srslte_ue_mib_sync_init(srslte_ue_mib_sync_t *q, 
-                                       uint32_t cell_id, 
-                                       srslte_cp_t cp,
-                                       int (recv_callback)(void*, void*, uint32_t, srslte_timestamp_t *),   
-                                       void *stream_handler);
-
-SRSLTE_API int srslte_ue_mib_sync_init_multi(srslte_ue_mib_sync_t *q, 
-                                             uint32_t cell_id, 
-                                             srslte_cp_t cp,
-                                             int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t, srslte_timestamp_t *),   
+SRSLTE_API int srslte_ue_mib_sync_init_multi(srslte_ue_mib_sync_t *q,
+                                             int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t, srslte_timestamp_t *),
                                              uint32_t nof_rx_antennas,
                                              void *stream_handler);
 
 SRSLTE_API void srslte_ue_mib_sync_free(srslte_ue_mib_sync_t *q);
 
+SRSLTE_API int srslte_ue_mib_sync_set_cell(srslte_ue_mib_sync_t *q,
+                                           uint32_t cell_id,
+                                           srslte_cp_t cp);
+
 SRSLTE_API void srslte_ue_mib_sync_reset(srslte_ue_mib_sync_t * q); 
 
 SRSLTE_API int srslte_ue_mib_sync_decode(srslte_ue_mib_sync_t * q, 
diff --git a/lib/include/srslte/phy/ue/ue_sync.h b/lib/include/srslte/phy/ue/ue_sync.h
index 3570e5a30..bd280acd0 100644
--- a/lib/include/srslte/phy/ue/ue_sync.h
+++ b/lib/include/srslte/phy/ue/ue_sync.h
@@ -69,6 +69,8 @@ typedef enum SRSLTE_API { SF_FIND, SF_TRACK} srslte_ue_sync_state_t;
 typedef struct SRSLTE_API {
   srslte_sync_t sfind;
   srslte_sync_t strack;
+
+  uint32_t max_prb;
   
   srslte_agc_t agc; 
   bool do_agc; 
@@ -125,22 +127,25 @@ typedef struct SRSLTE_API {
 } srslte_ue_sync_t;
 
 SRSLTE_API int srslte_ue_sync_init(srslte_ue_sync_t *q, 
-                                   srslte_cell_t cell,
+                                   uint32_t max_prb,
+                                   bool search_cell,
                                    int (recv_callback)(void*, void*, uint32_t, srslte_timestamp_t*), 
                                    void *stream_handler);
 
-SRSLTE_API int srslte_ue_sync_init_multi(srslte_ue_sync_t *q, 
-                                         srslte_cell_t cell,
+SRSLTE_API int srslte_ue_sync_init_multi(srslte_ue_sync_t *q,
+                                         uint32_t max_prb,
+                                         bool search_cell,
                                          int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t, srslte_timestamp_t*), 
                                          uint32_t nof_rx_antennas,
                                          void *stream_handler);
 
-SRSLTE_API int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q, 
-                                         srslte_cell_t cell,
-                                         int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t, srslte_timestamp_t*), 
-                                         uint32_t nof_rx_antennas,
-                                         void *stream_handler,
-                                         int decimate);
+SRSLTE_API int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q,
+                                               uint32_t max_prb,
+                                               bool search_cell,
+                                               int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t, srslte_timestamp_t*),
+                                               uint32_t nof_rx_antennas,
+                                               void *stream_handler,
+                                               int decimate);
 
 SRSLTE_API int srslte_ue_sync_init_file(srslte_ue_sync_t *q, 
                                         uint32_t nof_prb,
@@ -157,6 +162,9 @@ SRSLTE_API int srslte_ue_sync_init_file_multi(srslte_ue_sync_t *q,
 
 SRSLTE_API void srslte_ue_sync_free(srslte_ue_sync_t *q);
 
+SRSLTE_API int srslte_ue_sync_set_cell(srslte_ue_sync_t *q,
+                                       srslte_cell_t cell);
+
 SRSLTE_API int srslte_ue_sync_start_agc(srslte_ue_sync_t *q, 
                                         double (set_gain_callback)(void*, double), 
                                         float init_gain_value); 
@@ -173,6 +181,9 @@ SRSLTE_API int srslte_ue_sync_zerocopy(srslte_ue_sync_t *q,
 SRSLTE_API int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, 
                                              cf_t *input_buffer[SRSLTE_MAX_PORTS]);
 
+SRSLTE_API void srslte_ue_sync_set_cfo_tol(srslte_ue_sync_t *q,
+                                           float tol);
+
 SRSLTE_API void srslte_ue_sync_set_cfo(srslte_ue_sync_t *q, 
                                        float cfo); 
 
diff --git a/lib/include/srslte/phy/ue/ue_ul.h b/lib/include/srslte/phy/ue/ue_ul.h
index 2afa8068a..3492180e3 100644
--- a/lib/include/srslte/phy/ue/ue_ul.h
+++ b/lib/include/srslte/phy/ue/ue_ul.h
@@ -75,7 +75,8 @@ typedef struct SRSLTE_API {
   
   bool normalize_en; 
   bool cfo_en; 
-  
+
+  float current_cfo_tol;
   float current_cfo; 
   srslte_pucch_format_t last_pucch_format;
   
@@ -106,15 +107,21 @@ typedef struct SRSLTE_API {
 
 
 /* This function shall be called just after the initial synchronization */
-SRSLTE_API int srslte_ue_ul_init(srslte_ue_ul_t *q, 
-                                 srslte_cell_t cell);
+SRSLTE_API int srslte_ue_ul_init(srslte_ue_ul_t *q,
+                                 uint32_t max_prb);
 
 SRSLTE_API void srslte_ue_ul_free(srslte_ue_ul_t *q);
 
-SRSLTE_API void srslte_ue_ul_set_cfo(srslte_ue_ul_t *q, 
+SRSLTE_API int srslte_ue_ul_set_cell(srslte_ue_ul_t *q,
+                                     srslte_cell_t cell);
+
+SRSLTE_API void srslte_ue_ul_set_cfo_tol(srslte_ue_ul_t *q,
+                                         float tol);
+
+SRSLTE_API void srslte_ue_ul_set_cfo(srslte_ue_ul_t *q,
                                      float cur_cfo); 
 
-SRSLTE_API void srslte_ue_ul_set_cfo_enable(srslte_ue_ul_t *q, 
+SRSLTE_API void srslte_ue_ul_set_cfo_enable(srslte_ue_ul_t *q,
                                             bool enabled); 
 
 SRSLTE_API void srslte_ue_ul_set_normalization(srslte_ue_ul_t *q, 
diff --git a/lib/include/srslte/phy/utils/convolution.h b/lib/include/srslte/phy/utils/convolution.h
index 27ea7e590..fcb321ce5 100644
--- a/lib/include/srslte/phy/utils/convolution.h
+++ b/lib/include/srslte/phy/utils/convolution.h
@@ -46,6 +46,8 @@ typedef struct SRSLTE_API {
   uint32_t input_len;
   uint32_t filter_len;
   uint32_t output_len;
+  uint32_t max_input_len;
+  uint32_t max_filter_len;
   srslte_dft_plan_t input_plan;
   srslte_dft_plan_t filter_plan;
   srslte_dft_plan_t output_plan;
@@ -58,6 +60,9 @@ SRSLTE_API int srslte_conv_fft_cc_init(srslte_conv_fft_cc_t *q,
                                        uint32_t input_len, 
                                        uint32_t filter_len);
 
+SRSLTE_API int srslte_conv_fft_cc_replan(srslte_conv_fft_cc_t *q,
+                                         uint32_t input_len,
+                                         uint32_t filter_len);
 
 SRSLTE_API void srslte_conv_fft_cc_free(srslte_conv_fft_cc_t *q);
 
diff --git a/lib/include/srslte/radio/radio.h b/lib/include/srslte/radio/radio.h
index 849c1c0e9..30d274f44 100644
--- a/lib/include/srslte/radio/radio.h
+++ b/lib/include/srslte/radio/radio.h
@@ -98,15 +98,16 @@ namespace srslte {
       void set_tx_rx_gain_offset(float offset); 
       double set_rx_gain_th(float gain);
 
-      void set_tx_freq(float freq);
-      void set_rx_freq(float freq);
+      void set_freq_offset(double freq);
+      void set_tx_freq(double freq);
+      void set_rx_freq(double freq);
 
-      float get_tx_freq();
-      float get_rx_freq();
+      double get_tx_freq();
+      double get_rx_freq();
 
-      void set_master_clock_rate(float rate);
-      void set_tx_srate(float srate);
-      void set_rx_srate(float srate);
+      void set_master_clock_rate(double rate);
+      void set_tx_srate(double srate);
+      void set_rx_srate(double srate);
 
       float get_tx_gain();
       float get_rx_gain();
@@ -156,9 +157,9 @@ namespace srslte {
       
       const static double blade_default_burst_preamble_sec = 0.0;
       const static double blade_default_tx_adv_samples     = 27;
-      const static double blade_default_tx_adv_offset_sec  = 1e-6; 
-      
-      float tx_freq, rx_freq; 
+      const static double blade_default_tx_adv_offset_sec  = 1e-6;
+
+      double tx_freq, rx_freq, freq_offset;
       
       trace<uint32_t> tr_local_time;
       trace<uint32_t> tr_usrp_time;
diff --git a/lib/include/srslte/upper/gw.h b/lib/include/srslte/upper/gw.h
index ce5936b71..aa92ddc32 100644
--- a/lib/include/srslte/upper/gw.h
+++ b/lib/include/srslte/upper/gw.h
@@ -46,7 +46,7 @@ class gw
 {
 public:
   gw();
-  void init(srsue::pdcp_interface_gw *pdcp_, srsue::rrc_interface_gw *rrc_, srsue::ue_interface *ue_, log *gw_log_, uint32_t lcid_);
+  void init(srsue::pdcp_interface_gw *pdcp_, srsue::ue_interface *ue_, log *gw_log_, uint32_t lcid_);
   void stop();
 
   void get_metrics(gw_metrics_t &m);
@@ -62,7 +62,6 @@ private:
   static const int GW_THREAD_PRIO = 7;
 
   srsue::pdcp_interface_gw  *pdcp;
-  srsue::rrc_interface_gw   *rrc;
   srsue::ue_interface       *ue;
 
   byte_buffer_pool   *pool;
@@ -75,6 +74,8 @@ private:
   bool                if_up;
   uint32_t            lcid;
 
+  uint32_t            current_ip_addr;
+
   long                ul_tput_bytes;
   long                dl_tput_bytes;
   struct timeval      metrics_time[3];
diff --git a/lib/src/phy/ch_estimation/chest_dl.c b/lib/src/phy/ch_estimation/chest_dl.c
index 3c2f4210c..edb8c5ca2 100644
--- a/lib/src/phy/ch_estimation/chest_dl.c
+++ b/lib/src/phy/ch_estimation/chest_dl.c
@@ -32,6 +32,8 @@
 #include <string.h>
 #include <complex.h>
 #include <math.h>
+#include <srslte/phy/common/phy_common.h>
+#include <srslte/srslte.h>
 
 #include "srslte/config.h"
 
@@ -72,63 +74,56 @@ static void set_default_filter(srslte_chest_dl_t *q, int filter_len) {
  * 
  * This object depends on the srslte_refsignal_t object for creating the LTE CSR signal.  
 */
-
-int srslte_chest_dl_init(srslte_chest_dl_t *q, srslte_cell_t cell) 
+int srslte_chest_dl_init(srslte_chest_dl_t *q, uint32_t max_prb)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
-  if (q                != NULL &&
-      srslte_cell_isvalid(&cell)) 
+  if (q                != NULL)
   {
     bzero(q, sizeof(srslte_chest_dl_t));
     
-    ret = srslte_refsignal_cs_init(&q->csr_signal, cell); 
+    ret = srslte_refsignal_cs_init(&q->csr_signal, max_prb);
     if (ret != SRSLTE_SUCCESS) {
       fprintf(stderr, "Error initializing CSR signal (%d)\n",ret);
       goto clean_exit;
     }
     
-    q->tmp_noise = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(cell.nof_prb));
+    q->tmp_noise = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(max_prb));
     if (!q->tmp_noise) {
       perror("malloc");
       goto clean_exit;
     }
-    q->pilot_estimates = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(cell.nof_prb));
+
+    q->pilot_estimates = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(max_prb));
     if (!q->pilot_estimates) {
       perror("malloc");
       goto clean_exit;
     }      
-    q->pilot_estimates_average = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(cell.nof_prb));
+    q->pilot_estimates_average = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(max_prb));
     if (!q->pilot_estimates_average) {
       perror("malloc");
       goto clean_exit;
     }      
-    q->pilot_recv_signal = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(cell.nof_prb));
+    q->pilot_recv_signal = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(max_prb));
     if (!q->pilot_recv_signal) {
       perror("malloc");
       goto clean_exit;
     }
     
-    if (srslte_interp_linear_vector_init(&q->srslte_interp_linvec, SRSLTE_NRE*cell.nof_prb)) {
+    if (srslte_interp_linear_vector_init(&q->srslte_interp_linvec, SRSLTE_NRE*max_prb)) {
       fprintf(stderr, "Error initializing vector interpolator\n");
       goto clean_exit; 
     }
 
-    if (srslte_interp_linear_init(&q->srslte_interp_lin, 2*cell.nof_prb, SRSLTE_NRE/2)) {
+    if (srslte_interp_linear_init(&q->srslte_interp_lin, 2*max_prb, SRSLTE_NRE/2)) {
       fprintf(stderr, "Error initializing interpolator\n");
       goto clean_exit; 
     }
     
-    if (srslte_pss_generate(q->pss_signal, cell.id%3)) {
-      fprintf(stderr, "Error initializing PSS signal for noise estimation\n");
-      goto clean_exit;
-    }
-    
-    q->noise_alg = SRSLTE_NOISE_ALG_REFS; 
+    q->noise_alg = SRSLTE_NOISE_ALG_REFS;
     
     q->smooth_filter_len = 3; 
     srslte_chest_dl_set_smooth_filter3_coeff(q, 0.1);
     
-    q->cell = cell; 
   }
   
   ret = SRSLTE_SUCCESS;
@@ -162,10 +157,45 @@ void srslte_chest_dl_free(srslte_chest_dl_t *q)
   bzero(q, sizeof(srslte_chest_dl_t));
 }
 
+int srslte_chest_dl_set_cell(srslte_chest_dl_t *q, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+  if (q                != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+      ret = srslte_refsignal_cs_set_cell(&q->csr_signal, cell);
+      if (ret != SRSLTE_SUCCESS) {
+        fprintf(stderr, "Error initializing CSR signal (%d)\n",ret);
+        return SRSLTE_ERROR;
+      }
+      if (srslte_pss_generate(q->pss_signal, cell.id%3)) {
+        fprintf(stderr, "Error initializing PSS signal for noise estimation\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_interp_linear_vector_resize(&q->srslte_interp_linvec, SRSLTE_NRE*q->cell.nof_prb)) {
+        fprintf(stderr, "Error initializing vector interpolator\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_interp_linear_resize(&q->srslte_interp_lin, 2*q->cell.nof_prb, SRSLTE_NRE/2)) {
+        fprintf(stderr, "Error initializing interpolator\n");
+        return SRSLTE_ERROR;
+      }
+
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+
+  return ret;
+}
+
 /* 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) 
 {
   int nref=SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id);
+
   /* Substract noisy pilot estimates */
   srslte_vec_sub_ccc(q->pilot_estimates_average, q->pilot_estimates, q->tmp_noise, nref);  
   
@@ -318,7 +348,8 @@ int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, u
   
   /* Use the known CSR signal to compute Least-squares estimates */
   srslte_vec_prod_conj_ccc(q->pilot_recv_signal, q->csr_signal.pilots[port_id/2][sf_idx], 
-              q->pilot_estimates, SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id)); 
+              q->pilot_estimates, SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id));
+
   if (ce != NULL) {
     
     /* Smooth estimates (if applicable) and interpolate */
@@ -370,7 +401,7 @@ int srslte_chest_dl_estimate_multi(srslte_chest_dl_t *q, cf_t *input[SRSLTE_MAX_
 int srslte_chest_dl_estimate(srslte_chest_dl_t *q, cf_t *input, cf_t *ce[SRSLTE_MAX_PORTS], uint32_t sf_idx) 
 {
   uint32_t port_id; 
-  
+
   for (port_id=0;port_id<q->cell.nof_ports;port_id++) {
     if (srslte_chest_dl_estimate_port(q, input, ce[port_id], sf_idx, port_id, 0)) {
       return SRSLTE_ERROR;
diff --git a/lib/src/phy/ch_estimation/chest_ul.c b/lib/src/phy/ch_estimation/chest_ul.c
index 9996fc9c3..ac4831cc9 100644
--- a/lib/src/phy/ch_estimation/chest_ul.c
+++ b/lib/src/phy/ch_estimation/chest_ul.c
@@ -32,6 +32,8 @@
 #include <string.h>
 #include <complex.h>
 #include <math.h>
+#include <srslte/srslte.h>
+#include <srslte/phy/common/phy_common.h>
 
 #include "srslte/config.h"
 
@@ -43,6 +45,9 @@
 #define NOF_REFS_SYM    (q->cell.nof_prb*SRSLTE_NRE)
 #define NOF_REFS_SF     (NOF_REFS_SYM*2) // 2 reference symbols per subframe
 
+#define MAX_REFS_SYM    (max_prb*SRSLTE_NRE)
+#define MAX_REFS_SF     (max_prb*SRSLTE_NRE*2) // 2 reference symbols per subframe
+
 /** 3GPP LTE Downlink channel estimator and equalizer. 
  * Estimates the channel in the resource elements transmitting references and interpolates for the rest
  * of the resource grid. 
@@ -52,52 +57,49 @@
  * This object depends on the srslte_refsignal_t object for creating the LTE CSR signal.  
 */
 
-int srslte_chest_ul_init(srslte_chest_ul_t *q, srslte_cell_t cell) 
+int srslte_chest_ul_init(srslte_chest_ul_t *q, uint32_t max_prb)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
-  if (q                != NULL &&
-      srslte_cell_isvalid(&cell)) 
+  if (q                != NULL)
   {
     bzero(q, sizeof(srslte_chest_ul_t));
 
-    q->cell = cell; 
-    
-    ret = srslte_refsignal_ul_init(&q->dmrs_signal, cell); 
+    ret = srslte_refsignal_ul_init(&q->dmrs_signal, max_prb);
     if (ret != SRSLTE_SUCCESS) {
       fprintf(stderr, "Error initializing CSR signal (%d)\n",ret);
       goto clean_exit;
     }
     
-    q->tmp_noise = srslte_vec_malloc(sizeof(cf_t) * NOF_REFS_SF);
+    q->tmp_noise = srslte_vec_malloc(sizeof(cf_t) * MAX_REFS_SF);
     if (!q->tmp_noise) {
       perror("malloc");
       goto clean_exit;
     }
-    q->pilot_estimates = srslte_vec_malloc(sizeof(cf_t) * NOF_REFS_SF);
+    q->pilot_estimates = srslte_vec_malloc(sizeof(cf_t) * MAX_REFS_SF);
     if (!q->pilot_estimates) {
       perror("malloc");
       goto clean_exit;
     }      
     for (int i=0;i<4;i++) {
-      q->pilot_estimates_tmp[i] = srslte_vec_malloc(sizeof(cf_t) * NOF_REFS_SF);
+      q->pilot_estimates_tmp[i] = srslte_vec_malloc(sizeof(cf_t) * MAX_REFS_SF);
       if (!q->pilot_estimates_tmp[i]) {
         perror("malloc");
         goto clean_exit;
       }      
     }
-    q->pilot_recv_signal = srslte_vec_malloc(sizeof(cf_t) * (NOF_REFS_SF+1));
+    q->pilot_recv_signal = srslte_vec_malloc(sizeof(cf_t) * (MAX_REFS_SF+1));
     if (!q->pilot_recv_signal) {
       perror("malloc");
       goto clean_exit;
     }
     
-    q->pilot_known_signal = srslte_vec_malloc(sizeof(cf_t) * (NOF_REFS_SF+1));
+    q->pilot_known_signal = srslte_vec_malloc(sizeof(cf_t) * (MAX_REFS_SF+1));
     if (!q->pilot_known_signal) {
       perror("malloc");
       goto clean_exit;
     }
     
-    if (srslte_interp_linear_vector_init(&q->srslte_interp_linvec, NOF_REFS_SYM)) {
+    if (srslte_interp_linear_vector_init(&q->srslte_interp_linvec, MAX_REFS_SYM)) {
       fprintf(stderr, "Error initializing vector interpolator\n");
       goto clean_exit; 
     }
@@ -105,12 +107,17 @@ int srslte_chest_ul_init(srslte_chest_ul_t *q, srslte_cell_t cell)
     q->smooth_filter_len = 3; 
     srslte_chest_ul_set_smooth_filter3_coeff(q, 0.3333);
   
-    q->dmrs_signal_configured = false; 
+    q->dmrs_signal_configured = false;
+
+    if (srslte_refsignal_dmrs_pusch_pregen_init(&q->dmrs_signal, &q->dmrs_pregen, max_prb)) {
+      fprintf(stderr, "Error allocating memory for pregenerated signals\n");
+      goto clean_exit;
+    }
   
   }
     
   ret = SRSLTE_SUCCESS;
-  
+
 clean_exit:
   if (ret != SRSLTE_SUCCESS) {
       srslte_chest_ul_free(q);
@@ -120,9 +127,8 @@ clean_exit:
 
 void srslte_chest_ul_free(srslte_chest_ul_t *q) 
 {
-  if (q->dmrs_signal_configured) {
-    srslte_refsignal_dmrs_pusch_pregen_free(&q->dmrs_signal, &q->dmrs_pregen);
-  }
+  srslte_refsignal_dmrs_pusch_pregen_free(&q->dmrs_signal, &q->dmrs_pregen);
+
   srslte_refsignal_ul_free(&q->dmrs_signal);
   if (q->tmp_noise) {
     free(q->tmp_noise);
@@ -146,6 +152,30 @@ void srslte_chest_ul_free(srslte_chest_ul_t *q)
   bzero(q, sizeof(srslte_chest_ul_t));
 }
 
+int srslte_chest_ul_set_cell(srslte_chest_ul_t *q, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+  if (q                != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    if (cell.id != q->cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+      ret = srslte_refsignal_ul_set_cell(&q->dmrs_signal, cell);
+      if (ret != SRSLTE_SUCCESS) {
+        fprintf(stderr, "Error initializing CSR signal (%d)\n",ret);
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_interp_linear_vector_resize(&q->srslte_interp_linvec, NOF_REFS_SYM)) {
+        fprintf(stderr, "Error initializing vector interpolator\n");
+        return SRSLTE_ERROR;
+      }
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
 void srslte_chest_ul_set_cfg(srslte_chest_ul_t *q, 
                              srslte_refsignal_dmrs_pusch_cfg_t *pusch_cfg,
                              srslte_pucch_cfg_t *pucch_cfg, 
diff --git a/lib/src/phy/ch_estimation/refsignal_dl.c b/lib/src/phy/ch_estimation/refsignal_dl.c
index ec0f0b4e0..fa8a27968 100644
--- a/lib/src/phy/ch_estimation/refsignal_dl.c
+++ b/lib/src/phy/ch_estimation/refsignal_dl.c
@@ -99,10 +99,39 @@ inline uint32_t srslte_refsignal_cs_nsymbol(uint32_t l, srslte_cp_t cp, uint32_t
 }
 
 
-/** Allocates and precomputes the Cell-Specific Reference (CSR) signal for 
+/** Allocates memory for the 20 slots in a subframe
+ */
+int srslte_refsignal_cs_init(srslte_refsignal_cs_t * q, uint32_t max_prb)
+{
+
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q != NULL)
+  {
+    ret = SRSLTE_ERROR; 
+
+    for (int p=0;p<2;p++) {
+      for (int i=0;i<SRSLTE_NSUBFRAMES_X_FRAME;i++) {
+        q->pilots[p][i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_NUM_SF(max_prb, 2*p));
+        if (!q->pilots[p][i]) {
+          perror("malloc");
+          goto free_and_exit;
+        }
+      }      
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+free_and_exit:
+  if (ret == SRSLTE_ERROR) {
+    srslte_refsignal_cs_free(q);
+  }
+  return ret;
+}
+
+/** Allocates and precomputes the Cell-Specific Reference (CSR) signal for
  * the 20 slots in a subframe
  */
-int srslte_refsignal_cs_init(srslte_refsignal_cs_t * q, srslte_cell_t cell)
+int srslte_refsignal_cs_set_cell(srslte_refsignal_cs_t * q, srslte_cell_t cell)
 {
 
   uint32_t c_init;
@@ -112,76 +141,57 @@ int srslte_refsignal_cs_init(srslte_refsignal_cs_t * q, srslte_cell_t cell)
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
   if (q != NULL &&
-      srslte_cell_isvalid(&cell)) 
+      srslte_cell_isvalid(&cell))
   {
-    ret = SRSLTE_ERROR; 
-    
-    bzero(q, sizeof(srslte_refsignal_cs_t));
-    bzero(&seq, sizeof(srslte_sequence_t));
-    if (srslte_sequence_init(&seq, 2 * 2 * SRSLTE_MAX_PRB)) {
-      goto free_and_exit;
-    }
-    
-    if (SRSLTE_CP_ISNORM(cell.cp)) {
-      N_cp = 1;
-    } else {
-      N_cp = 0;
-    }
-    
-    q->cell = cell; 
-    
-    for (p=0;p<2;p++) {
-      for (i=0;i<SRSLTE_NSUBFRAMES_X_FRAME;i++) {
-        q->pilots[p][i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, 2*p));
-        if (!q->pilots[p][i]) {
-          perror("malloc");
-          goto free_and_exit;
-        }
-      }      
-    }
-    
-    for (ns=0;ns<SRSLTE_NSLOTS_X_FRAME;ns++) {
-      for (p=0;p<2;p++) {
-        uint32_t nsymbols = srslte_refsignal_cs_nof_symbols(2*p)/2;
-        for (l = 0; l < nsymbols; l++) {
-          /* Compute sequence init value */
-          uint32_t lp = srslte_refsignal_cs_nsymbol(l, cell.cp, 2*p);
-          c_init = 1024 * (7 * (ns + 1) + lp + 1) * (2 * cell.id + 1)
-            + 2 * cell.id + N_cp;
-          
-          /* generate sequence for this symbol and slot */
-          srslte_sequence_set_LTE_pr(&seq, c_init);
-          
-          /* Compute signal */
-          for (i = 0; i < 2*q->cell.nof_prb; i++) {
-            mp = i + SRSLTE_MAX_PRB - cell.nof_prb;
-            /* save signal */
-            q->pilots[p][ns/2][SRSLTE_REFSIGNAL_PILOT_IDX(i,(ns%2)*nsymbols+l,q->cell)] = 
-                                                  (1 - 2 * (float) seq.c[2 * mp]) / sqrt(2) +
-                                    _Complex_I * (1 - 2 * (float) seq.c[2 * mp + 1]) / sqrt(2);
-          }        
-        }
-        
+    if (cell.id != q->cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+
+      bzero(&seq, sizeof(srslte_sequence_t));
+      if (srslte_sequence_init(&seq, 2*2*SRSLTE_MAX_PRB)) {
+        return SRSLTE_ERROR;
       }
+
+      if (SRSLTE_CP_ISNORM(cell.cp)) {
+        N_cp = 1;
+      } else {
+        N_cp = 0;
+      }
+
+      for (ns=0;ns<SRSLTE_NSLOTS_X_FRAME;ns++) {
+        for (p=0;p<2;p++) {
+          uint32_t nsymbols = srslte_refsignal_cs_nof_symbols(2*p)/2;
+          for (l = 0; l < nsymbols; l++) {
+            /* Compute sequence init value */
+            uint32_t lp = srslte_refsignal_cs_nsymbol(l, cell.cp, 2*p);
+            c_init = 1024 * (7 * (ns + 1) + lp + 1) * (2 * cell.id + 1)
+                     + 2 * cell.id + N_cp;
+
+            /* generate sequence for this symbol and slot */
+            srslte_sequence_set_LTE_pr(&seq, 2*2*SRSLTE_MAX_PRB, c_init);
+
+            /* Compute signal */
+            for (i = 0; i < 2*q->cell.nof_prb; i++) {
+              mp = i + SRSLTE_MAX_PRB - cell.nof_prb;
+              /* save signal */
+              q->pilots[p][ns/2][SRSLTE_REFSIGNAL_PILOT_IDX(i,(ns%2)*nsymbols+l,q->cell)] =
+                (1 - 2 * (float) seq.c[2 * mp]) / sqrt(2) +
+                _Complex_I * (1 - 2 * (float) seq.c[2 * mp + 1]) / sqrt(2);
+            }
+          }
+        }
+      }
+      srslte_sequence_free(&seq);
     }
-    srslte_sequence_free(&seq);
     ret = SRSLTE_SUCCESS;
   }
-free_and_exit:
-  if (ret == SRSLTE_ERROR) {
-    srslte_sequence_free(&seq);
-    srslte_refsignal_cs_free(q);
-  }
   return ret;
 }
 
 /** Deallocates a srslte_refsignal_cs_t object allocated with srslte_refsignal_cs_init */
 void srslte_refsignal_cs_free(srslte_refsignal_cs_t * q)
 {
-  int i, p; 
-  
-  for (p=0;p<2;p++) {
-    for (i=0;i<SRSLTE_NSUBFRAMES_X_FRAME;i++) {
+  for (int p=0;p<2;p++) {
+    for (int i=0;i<SRSLTE_NSUBFRAMES_X_FRAME;i++) {
       if (q->pilots[p][i]) {
         free(q->pilots[p][i]);
       }
diff --git a/lib/src/phy/ch_estimation/refsignal_ul.c b/lib/src/phy/ch_estimation/refsignal_ul.c
index 609ef52d0..a287f8d48 100644
--- a/lib/src/phy/ch_estimation/refsignal_ul.c
+++ b/lib/src/phy/ch_estimation/refsignal_ul.c
@@ -29,6 +29,7 @@
 #include <strings.h>
 #include <stdlib.h>
 #include <complex.h>
+#include <srslte/phy/common/phy_common.h>
 
 #include "srslte/phy/common/phy_common.h"
 #include "srslte/phy/ch_estimation/refsignal_ul.h"
@@ -138,8 +139,8 @@ static int generate_n_prs(srslte_refsignal_ul_t * q) {
       }
       q->n_prs_pusch[delta_ss][ns] = n_prs;
     }
-    srslte_sequence_free(&seq);
   }
+  srslte_sequence_free(&seq);
 
   return SRSLTE_SUCCESS; 
 }
@@ -160,9 +161,9 @@ static int generate_srslte_sequence_hopping_v(srslte_refsignal_ul_t *q) {
         return SRSLTE_ERROR;
       }
       q->v_pusch[ns][delta_ss] = seq.c[ns];    
-      srslte_sequence_free(&seq);
     }
   }
+  srslte_sequence_free(&seq);
   return SRSLTE_SUCCESS;
 }
 
@@ -170,46 +171,24 @@ static int generate_srslte_sequence_hopping_v(srslte_refsignal_ul_t *q) {
 /** Initializes srslte_refsignal_ul_t object according to 3GPP 36.211 5.5
  *
  */
-int srslte_refsignal_ul_init(srslte_refsignal_ul_t * q, srslte_cell_t cell)
+int srslte_refsignal_ul_init(srslte_refsignal_ul_t * q, uint32_t max_prb)
 {
 
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
-  if (q != NULL && srslte_cell_isvalid(&cell)) {
+  if (q != NULL) {
 
     ret = SRSLTE_ERROR; 
     
     bzero(q, sizeof(srslte_refsignal_ul_t));
-    q->cell = cell; 
-    
+
     // Allocate temporal buffer for computing signal argument
-    q->tmp_arg = srslte_vec_malloc(SRSLTE_NRE * q->cell.nof_prb * sizeof(cf_t)); 
+    q->tmp_arg = srslte_vec_malloc(SRSLTE_NRE * max_prb * sizeof(cf_t));
     if (!q->tmp_arg) {
       perror("malloc");
       goto free_and_exit;
     }
     
-    srslte_pucch_cfg_default(&q->pucch_cfg);
-    
-    // Precompute n_prs
-    if (generate_n_prs(q)) {
-      goto free_and_exit;
-    }
-    
-    // Precompute group hopping values u. 
-    if (srslte_group_hopping_f_gh(q->f_gh, q->cell.id)) {
-      goto free_and_exit;
-    }
-    
-    // Precompute sequence hopping values v. Uses f_ss_pusch
-    if (generate_srslte_sequence_hopping_v(q)) {
-      goto free_and_exit;
-    }
-
-    if (srslte_pucch_n_cs_cell(q->cell, q->n_cs_cell)) {
-      goto free_and_exit;
-    }
-
     ret = SRSLTE_SUCCESS;
   }
 free_and_exit:
@@ -226,6 +205,45 @@ void srslte_refsignal_ul_free(srslte_refsignal_ul_t * q) {
   bzero(q, sizeof(srslte_refsignal_ul_t));
 }
 
+/** Initializes srslte_refsignal_ul_t object according to 3GPP 36.211 5.5
+ *
+ */
+int srslte_refsignal_ul_set_cell(srslte_refsignal_ul_t * q, srslte_cell_t cell)
+{
+
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q != NULL && srslte_cell_isvalid(&cell)) {
+
+    if (cell.id != q->cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+
+      srslte_pucch_cfg_default(&q->pucch_cfg);
+
+      // Precompute n_prs
+      if (generate_n_prs(q)) {
+        return SRSLTE_ERROR;
+      }
+
+      // Precompute group hopping values u.
+      if (srslte_group_hopping_f_gh(q->f_gh, q->cell.id)) {
+        return SRSLTE_ERROR;
+      }
+
+      // Precompute sequence hopping values v. Uses f_ss_pusch
+      if (generate_srslte_sequence_hopping_v(q)) {
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pucch_n_cs_cell(q->cell, q->n_cs_cell)) {
+        return SRSLTE_ERROR;
+      }
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
 void srslte_refsignal_ul_set_cfg(srslte_refsignal_ul_t *q, 
                                   srslte_refsignal_dmrs_pusch_cfg_t *pusch_cfg,
                                   srslte_pucch_cfg_t *pucch_cfg, 
@@ -363,15 +381,38 @@ void compute_r(srslte_refsignal_ul_t *q, uint32_t nof_prb, uint32_t ns, uint32_t
 
 }
 
+int srslte_refsignal_dmrs_pusch_pregen_init(srslte_refsignal_ul_t *q, srslte_refsignal_ul_dmrs_pregen_t *pregen,
+                                            uint32_t max_prb)
+{
+  for (uint32_t sf_idx=0;sf_idx<SRSLTE_NSUBFRAMES_X_FRAME;sf_idx++) {
+    for (uint32_t cs=0;cs<SRSLTE_NOF_CSHIFT;cs++) {
+      pregen->r[cs][sf_idx] = (cf_t**) calloc(sizeof(cf_t*), max_prb + 1);
+      if (pregen->r[cs][sf_idx]) {
+        for (uint32_t n=0;n<=max_prb;n++) {
+          if (srslte_dft_precoding_valid_prb(n)) {
+            pregen->r[cs][sf_idx][n] = (cf_t*) srslte_vec_malloc(sizeof(cf_t)*n*2*SRSLTE_NRE);
+            if (pregen->r[cs][sf_idx][n]) {
+            } else {
+              return SRSLTE_ERROR;
+            }
+          }
+        }
+      } else {
+        return SRSLTE_ERROR;
+      }
+    }
+  }
+  return SRSLTE_SUCCESS;
+}
+
+
 int srslte_refsignal_dmrs_pusch_pregen(srslte_refsignal_ul_t *q, srslte_refsignal_ul_dmrs_pregen_t *pregen)
 {
   for (uint32_t sf_idx=0;sf_idx<SRSLTE_NSUBFRAMES_X_FRAME;sf_idx++) {
     for (uint32_t cs=0;cs<SRSLTE_NOF_CSHIFT;cs++) {
-      pregen->r[cs][sf_idx] = (cf_t**) calloc(sizeof(cf_t*), q->cell.nof_prb + 1);
       if (pregen->r[cs][sf_idx]) {
         for (uint32_t n=0;n<=q->cell.nof_prb;n++) {
           if (srslte_dft_precoding_valid_prb(n)) {
-            pregen->r[cs][sf_idx][n] = (cf_t*) srslte_vec_malloc(sizeof(cf_t)*n*2*SRSLTE_NRE);
             if (pregen->r[cs][sf_idx][n]) {
               if (srslte_refsignal_dmrs_pusch_gen(q, n, sf_idx, cs, pregen->r[cs][sf_idx][n])) {
                 return SRSLTE_ERROR; 
diff --git a/lib/src/phy/ch_estimation/test/chest_test_dl.c b/lib/src/phy/ch_estimation/test/chest_test_dl.c
index c60e94d96..f91600872 100644
--- a/lib/src/phy/ch_estimation/test/chest_test_dl.c
+++ b/lib/src/phy/ch_estimation/test/chest_test_dl.c
@@ -29,6 +29,7 @@
 #include <strings.h>
 #include <unistd.h>
 #include <complex.h>
+#include <srslte/phy/common/phy_common.h>
 
 #include "srslte/srslte.h"
 
@@ -128,10 +129,13 @@ int main(int argc, char **argv) {
     cid = cell.id;
     max_cid = cell.id;
   }
-
+  if (srslte_chest_dl_init(&est, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing equalizer\n");
+    goto do_exit;
+  }
   while(cid <= max_cid) {
     cell.id = cid; 
-    if (srslte_chest_dl_init(&est, cell)) {
+    if (srslte_chest_dl_set_cell(&est, cell)) {
       fprintf(stderr, "Error initializing equalizer\n");
       goto do_exit;
     }
@@ -214,10 +218,10 @@ int main(int argc, char **argv) {
         }
       }
     }
-    srslte_chest_dl_free(&est);
     cid+=10;
     INFO("cid=%d\n", cid);
   }
+  srslte_chest_dl_free(&est);
 
 
   ret = 0;
diff --git a/lib/src/phy/ch_estimation/test/chest_test_ul.c b/lib/src/phy/ch_estimation/test/chest_test_ul.c
index b3b2331da..7b9fbacc9 100644
--- a/lib/src/phy/ch_estimation/test/chest_test_ul.c
+++ b/lib/src/phy/ch_estimation/test/chest_test_ul.c
@@ -125,13 +125,17 @@ int main(int argc, char **argv) {
     max_cid = cell.id;
   }
   printf("max_cid=%d, cid=%d, cell.id=%d\n", max_cid, cid, cell.id);
+  if (srslte_chest_ul_init(&est, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing equalizer\n");
+    goto do_exit;
+  }
   while(cid <= max_cid) {
     cell.id = cid; 
-    if (srslte_chest_ul_init(&est, cell)) {
+    if (srslte_chest_ul_set_cell(&est, cell)) {
       fprintf(stderr, "Error initializing equalizer\n");
       goto do_exit;
     }
-    
+
     for (int n=6;n<=cell.nof_prb;n+=5) {
       if (srslte_dft_precoding_valid_prb(n)) {
         for (int delta_ss=29;delta_ss<SRSLTE_NOF_DELTA_SS;delta_ss++) {
@@ -213,9 +217,9 @@ int main(int argc, char **argv) {
     }
     cid+=10;
     printf("cid=%d\n", cid);
-    srslte_chest_ul_free(&est);
   }
 
+  srslte_chest_ul_free(&est);
 
   if (fmatlab) {
     fprintf(fmatlab, "input=");
diff --git a/lib/src/phy/ch_estimation/test/refsignal_ul_test.c b/lib/src/phy/ch_estimation/test/refsignal_ul_test.c
index 40e940b49..c1323bf0d 100644
--- a/lib/src/phy/ch_estimation/test/refsignal_ul_test.c
+++ b/lib/src/phy/ch_estimation/test/refsignal_ul_test.c
@@ -81,7 +81,12 @@ int main(int argc, char **argv) {
   
   parse_args(argc,argv);
 
-  if (srslte_refsignal_ul_init(&refs, cell)) {
+  if (srslte_refsignal_ul_init(&refs, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing UL reference signal\n");
+    goto do_exit;
+  }
+
+  if (srslte_refsignal_ul_set_cell(&refs, cell)) {
     fprintf(stderr, "Error initializing UL reference signal\n");
     goto do_exit;
   }
diff --git a/lib/src/phy/common/phy_common.c b/lib/src/phy/common/phy_common.c
index 977ebc5b1..b502fbd75 100644
--- a/lib/src/phy/common/phy_common.c
+++ b/lib/src/phy/common/phy_common.c
@@ -235,7 +235,7 @@ int srslte_symbol_sz_power2(uint32_t nof_prb) {
     return 1024;
   } else if (nof_prb<=75) {
     return 1536;
-  } else if (nof_prb<=100) {
+  } else if (nof_prb<=110) {
     return 2048;
   } else {
     return -1;
@@ -257,7 +257,7 @@ int srslte_symbol_sz(uint32_t nof_prb) {
       return 768;
     } else if (nof_prb<=75) {
       return 1024;
-    } else if (nof_prb<=100) {
+    } else if (nof_prb<=110) {
       return 1536;
     } else {
       return SRSLTE_ERROR;
diff --git a/lib/src/phy/common/sequence.c b/lib/src/phy/common/sequence.c
index 2a863bcee..0ecf9f159 100644
--- a/lib/src/phy/common/sequence.c
+++ b/lib/src/phy/common/sequence.c
@@ -27,7 +27,8 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <strings.h>
-#include <assert.h>
+#include <pthread.h>
+#include <srslte/phy/common/sequence.h>
 
 #include "srslte/phy/common/sequence.h"
 #include "srslte/phy/utils/vector.h"
@@ -35,26 +36,75 @@
 
 #define Nc 1600
 
+#define MAX_SEQ_LEN  (128*1024)
+
+#define static_memory
 
 /*
  * Pseudo Random Sequence generation.
  * It follows the 3GPP Release 8 (LTE) 36.211
  * Section 7.2
  */
-void srslte_sequence_set_LTE_pr(srslte_sequence_t *q, uint32_t seed) {
+#ifdef static_memory
+static uint8_t x1[Nc+MAX_SEQ_LEN+31];
+static uint8_t x2[Nc+MAX_SEQ_LEN+31];
+
+static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
+int srslte_sequence_set_LTE_pr(srslte_sequence_t *q, uint32_t len, uint32_t seed) {
+  int n;
+
+  if (len > q->max_len) {
+    fprintf(stderr, "Error generating pseudo-random sequence: len %d exceeds maximum len %d\n",
+            len, MAX_SEQ_LEN);
+    return -1;
+  }
+
+  if (len > q->max_len) {
+    fprintf(stderr, "Error generating pseudo-random sequence: len %d is greater than allocated len %d\n",
+            len, q->max_len);
+    return -1;
+  }
+  pthread_mutex_lock(&mutex);
+
+  for (n = 0; n < 31; n++) {
+    x2[n] = (seed >> n) & 0x1;
+  }
+  x1[0] = 1;
+
+  for (n = 0; n < Nc + len; n++) {
+    x1[n + 31] = (x1[n + 3] + x1[n]) & 0x1;
+    x2[n + 31] = (x2[n + 3] + x2[n + 2] + x2[n+1] + x2[n]) & 0x1;
+  }
+
+  for (n = 0; n < len; n++) {
+    q->c[n] = (x1[n + Nc] + x2[n + Nc]) & 0x1;
+  }
+  pthread_mutex_unlock(&mutex);
+
+  return 0;
+}
+
+#else
+int srslte_sequence_set_LTE_pr(srslte_sequence_t *q, uint32_t len, uint32_t seed) {
   int n;
   uint32_t *x1, *x2;
 
-  x1 = calloc(Nc + q->len + 31, sizeof(uint32_t));
+  if (len > q->max_len) {
+    fprintf(stderr, "Error generating pseudo-random sequence: len %d is greater than allocated len %d\n",
+            len, q->max_len);
+    return -1;
+  }
+
+  x1 = calloc(Nc + len + 31, sizeof(uint32_t));
   if (!x1) {
     perror("calloc");
-    return;
+    return -1;
   }
-  x2 = calloc(Nc + q->len + 31, sizeof(uint32_t));
+  x2 = calloc(Nc + len + 31, sizeof(uint32_t));
   if (!x2) {
     free(x1);
     perror("calloc");
-    return;
+    return -1;
   }
 
   for (n = 0; n < 31; n++) {
@@ -62,25 +112,29 @@ void srslte_sequence_set_LTE_pr(srslte_sequence_t *q, uint32_t seed) {
   }
   x1[0] = 1;
 
-  for (n = 0; n < Nc + q->len; n++) {
+  for (n = 0; n < Nc + len; n++) {
     x1[n + 31] = (x1[n + 3] + x1[n]) & 0x1;
     x2[n + 31] = (x2[n + 3] + x2[n + 2] + +x2[n+1] + x2[n]) & 0x1;
   }
 
-  for (n = 0; n < q->len; n++) {
+  for (n = 0; n < len; n++) {
     q->c[n] = (x1[n + Nc] + x2[n + Nc]) & 0x1;
   }
 
   free(x1);
   free(x2);
+
+  return 0;
 }
 
+#endif
+
 int srslte_sequence_LTE_pr(srslte_sequence_t *q, uint32_t len, uint32_t seed) {
   if (srslte_sequence_init(q, len)) {
     return SRSLTE_ERROR;
   }
-  q->len = len;
-  srslte_sequence_set_LTE_pr(q, seed);
+  q->cur_len = len;
+  srslte_sequence_set_LTE_pr(q, len, seed);
   srslte_bit_pack_vector(q->c, q->c_bytes, len);
   for (int i=0;i<len;i++) {
     q->c_float[i] = (1-2*q->c[i]);
@@ -90,17 +144,8 @@ int srslte_sequence_LTE_pr(srslte_sequence_t *q, uint32_t len, uint32_t seed) {
 }
 
 int srslte_sequence_init(srslte_sequence_t *q, uint32_t len) {
-  if (q->c && (q->len != len)) {
-    free(q->c);
-    if (q->c_bytes) {
-      free(q->c_bytes);
-    }
-    if (q->c_float) {
-      free(q->c_float);
-    }
-    if (q->c_short) {
-      free(q->c_short);
-    }
+  if (q->c && len > q->max_len) {
+    srslte_sequence_free(q);
   }
   if (!q->c) {
     q->c = srslte_vec_malloc(len * sizeof(uint8_t));
@@ -119,7 +164,7 @@ int srslte_sequence_init(srslte_sequence_t *q, uint32_t len) {
     if (!q->c_short) {
       return SRSLTE_ERROR;
     }
-    q->len = len;
+    q->max_len = len;
   }
   return SRSLTE_SUCCESS;
 }
diff --git a/lib/src/phy/dft/dft_fftw.c b/lib/src/phy/dft/dft_fftw.c
index 5fb34bb3c..b4a627742 100644
--- a/lib/src/phy/dft/dft_fftw.c
+++ b/lib/src/phy/dft/dft_fftw.c
@@ -29,6 +29,7 @@
 #include <complex.h>
 #include <fftw3.h>
 #include <string.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/dft/dft.h"
 #include "srslte/phy/utils/vector.h"
@@ -38,14 +39,27 @@
 
 #define FFTW_WISDOM_FILE ".fftw_wisdom"
 
+#ifdef FFTW_WISDOM_FILE
+#define FFTW_TYPE FFTW_MEASURE
+#else
+#define FFTW_TYPE 0
+#endif
+
+
 void srslte_dft_load() {
+#ifdef FFTW_WISDOM_FILE
   fftwf_import_wisdom_from_filename(FFTW_WISDOM_FILE);
+#else
+  printf("Warning: FFTW Wisdom file not defined\n");
+#endif
 }
 
 void srslte_dft_exit() {
+#ifdef FFTW_WISDOM_FILE
   if (!fftwf_export_wisdom_to_filename(FFTW_WISDOM_FILE)) {
     fprintf(stderr, "Error saving FFTW wisdom to file %s\n", FFTW_WISDOM_FILE);
   }
+#endif
 }
 
 int srslte_dft_plan(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir,
@@ -58,19 +72,50 @@ int srslte_dft_plan(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_di
   return 0;
 }
 
+int srslte_dft_replan(srslte_dft_plan_t *plan, const int new_dft_points) {
+  if (new_dft_points <= plan->init_size) {
+    if(plan->mode == SRSLTE_DFT_COMPLEX){
+      return srslte_dft_replan_c(plan,new_dft_points);
+    } else {
+      return srslte_dft_replan_r(plan,new_dft_points);
+    }
+  } else {
+    fprintf(stderr, "DFT: Error calling replan: new_dft_points (%d) must be lower or equal "
+      "dft_size passed initially (%d)\n", new_dft_points, plan->init_size);
+    return -1;
+  }
+}
+
+
+
 static void allocate(srslte_dft_plan_t *plan, int size_in, int size_out, int len) {
   plan->in = fftwf_malloc(size_in*len);
   plan->out = fftwf_malloc(size_out*len);
 }
 
+int srslte_dft_replan_c(srslte_dft_plan_t *plan, const int new_dft_points) {
+  int sign = (plan->dir == SRSLTE_DFT_FORWARD) ? FFTW_FORWARD : FFTW_BACKWARD;
+  if (plan->p) {
+    fftwf_destroy_plan(plan->p);
+    plan->p = NULL;
+  }
+  plan->p = fftwf_plan_dft_1d(new_dft_points, plan->in, plan->out, sign, FFTW_TYPE);
+  if (!plan->p) {
+    return -1;
+  }
+  plan->size = new_dft_points;
+  return 0;
+}
+
 int srslte_dft_plan_c(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir) {
   allocate(plan,sizeof(fftwf_complex),sizeof(fftwf_complex), dft_points);
   int sign = (dir == SRSLTE_DFT_FORWARD) ? FFTW_FORWARD : FFTW_BACKWARD;
-  plan->p = fftwf_plan_dft_1d(dft_points, plan->in, plan->out, sign, FFTW_MEASURE);
+  plan->p = fftwf_plan_dft_1d(dft_points, plan->in, plan->out, sign, FFTW_TYPE);
   if (!plan->p) {
     return -1;
   }
   plan->size = dft_points;
+  plan->init_size = plan->size;
   plan->mode = SRSLTE_DFT_COMPLEX;
   plan->dir = dir;
   plan->forward = (dir==SRSLTE_DFT_FORWARD)?true:false;
@@ -82,14 +127,29 @@ int srslte_dft_plan_c(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_
   return 0;
 }
 
+int srslte_dft_replan_r(srslte_dft_plan_t *plan, const int new_dft_points) {
+  int sign = (plan->dir == SRSLTE_DFT_FORWARD) ? FFTW_R2HC : FFTW_HC2R;
+  if (plan->p) {
+    fftwf_destroy_plan(plan->p);
+    plan->p = NULL;
+  }
+  plan->p = fftwf_plan_r2r_1d(new_dft_points, plan->in, plan->out, sign, FFTW_TYPE);
+  if (!plan->p) {
+    return -1;
+  }
+  plan->size = new_dft_points;
+  return 0;
+}
+
 int srslte_dft_plan_r(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir) {
   allocate(plan,sizeof(float),sizeof(float), dft_points);
   int sign = (dir == SRSLTE_DFT_FORWARD) ? FFTW_R2HC : FFTW_HC2R;
-  plan->p = fftwf_plan_r2r_1d(dft_points, plan->in, plan->out, sign, FFTW_MEASURE);
+  plan->p = fftwf_plan_r2r_1d(dft_points, plan->in, plan->out, sign, FFTW_TYPE);
   if (!plan->p) {
     return -1;
   }
   plan->size = dft_points;
+  plan->init_size = plan->size;
   plan->mode = SRSLTE_REAL;
   plan->dir = dir;
   plan->forward = (dir==SRSLTE_DFT_FORWARD)?true:false;
diff --git a/lib/src/phy/dft/dft_precoding.c b/lib/src/phy/dft/dft_precoding.c
index 3bec5b96e..a43d79406 100644
--- a/lib/src/phy/dft/dft_precoding.c
+++ b/lib/src/phy/dft/dft_precoding.c
@@ -40,37 +40,42 @@
 #include "srslte/phy/dft/dft_precoding.h"
 
 /* Create DFT plans for transform precoding */
-int srslte_dft_precoding_init(srslte_dft_precoding_t *q, uint32_t max_prb) 
+
+int srslte_dft_precoding_init(srslte_dft_precoding_t *q, uint32_t max_prb, bool is_tx)
 {
-  int ret = SRSLTE_ERROR_INVALID_INPUTS; 
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
   bzero(q, sizeof(srslte_dft_precoding_t));
-  
+
   if (max_prb <= SRSLTE_MAX_PRB) {
-    ret = SRSLTE_ERROR; 
+    ret = SRSLTE_ERROR;
     for (uint32_t i=1;i<=max_prb;i++) {
-      if(srslte_dft_precoding_valid_prb(i)) {        
+      if(srslte_dft_precoding_valid_prb(i)) {
         DEBUG("Initiating DFT precoding plan for %d PRBs\n", i);
-        if (srslte_dft_plan_c(&q->dft_plan[i], i*SRSLTE_NRE, SRSLTE_DFT_FORWARD)) {
+        if (srslte_dft_plan_c(&q->dft_plan[i], i*SRSLTE_NRE, is_tx?SRSLTE_DFT_FORWARD:SRSLTE_DFT_BACKWARD)) {
           fprintf(stderr, "Error: Creating DFT plan %d\n",i);
           goto clean_exit;
         }
         srslte_dft_plan_set_norm(&q->dft_plan[i], true);
-        if (srslte_dft_plan_c(&q->idft_plan[i], i*SRSLTE_NRE, SRSLTE_DFT_BACKWARD)) {
-          fprintf(stderr, "Error: Creating DFT plan %d\n",i);
-          goto clean_exit;
-        }
-        srslte_dft_plan_set_norm(&q->idft_plan[i], true);
       }
     }
     q->max_prb = max_prb;
     ret = SRSLTE_SUCCESS;
-  } 
+  }
 
-clean_exit:
+  clean_exit:
   if (ret == SRSLTE_ERROR) {
     srslte_dft_precoding_free(q);
   }
-  return ret; 
+  return ret;
+}
+
+int srslte_dft_precoding_init_rx(srslte_dft_precoding_t *q, uint32_t max_prb)
+{
+  return srslte_dft_precoding_init(q, max_prb, false);
+}
+
+int srslte_dft_precoding_init_tx(srslte_dft_precoding_t *q, uint32_t max_prb) {
+  return srslte_dft_precoding_init(q, max_prb, true);
 }
 
 /* Free DFT plans for transform precoding */
@@ -79,7 +84,6 @@ void srslte_dft_precoding_free(srslte_dft_precoding_t  *q)
   for (uint32_t i=1;i<=q->max_prb;i++) {
     if(srslte_dft_precoding_valid_prb(i)) {      
       srslte_dft_plan_free(&q->dft_plan[i]);
-      srslte_dft_plan_free(&q->idft_plan[i]);        
     }
   }  
   bzero(q, sizeof(srslte_dft_precoding_t));
@@ -98,7 +102,7 @@ bool srslte_dft_precoding_valid_prb(uint32_t nof_prb) {
 }
 
 int srslte_dft_precoding(srslte_dft_precoding_t *q, cf_t *input, cf_t *output, 
-                  uint32_t nof_prb, uint32_t nof_symbols) 
+                         uint32_t nof_prb, uint32_t nof_symbols)
 {
   
   if (!srslte_dft_precoding_valid_prb(nof_prb) && nof_prb <= q->max_prb) {
@@ -112,19 +116,3 @@ int srslte_dft_precoding(srslte_dft_precoding_t *q, cf_t *input, cf_t *output,
   
   return SRSLTE_SUCCESS;
 }
-
-int srslte_dft_predecoding(srslte_dft_precoding_t *q, cf_t *input, cf_t *output, 
-                    uint32_t nof_prb, uint32_t nof_symbols)
-{
-  if (!srslte_dft_precoding_valid_prb(nof_prb) && nof_prb <= q->max_prb) {
-    fprintf(stderr, "Error invalid number of PRB (%d)\n", nof_prb);
-    return SRSLTE_ERROR; 
-  }
-
-  for (uint32_t i=0;i<nof_symbols;i++) {
-    srslte_dft_run_c(&q->idft_plan[nof_prb], &input[i*SRSLTE_NRE*nof_prb], &output[i*SRSLTE_NRE*nof_prb]);
-  }
-  
-  return SRSLTE_SUCCESS;
-  
-}
diff --git a/lib/src/phy/dft/ofdm.c b/lib/src/phy/dft/ofdm.c
index 69b7e9161..825aff1e0 100644
--- a/lib/src/phy/dft/ofdm.c
+++ b/lib/src/phy/dft/ofdm.c
@@ -29,6 +29,7 @@
 #include <stdlib.h>
 #include <complex.h>
 #include <math.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/common/phy_common.h"
 #include "srslte/phy/dft/dft.h"
@@ -48,6 +49,12 @@ int srslte_ofdm_init_(srslte_ofdm_t *q, srslte_cp_t cp, int symbol_sz, int nof_p
     return -1;
   }
 
+  q->shift_buffer = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN(symbol_sz));
+  if (!q->shift_buffer) {
+    perror("malloc");
+    return -1;
+  }
+
   srslte_dft_plan_set_mirror(&q->fft_plan, true);
   srslte_dft_plan_set_dc(&q->fft_plan, true);
 
@@ -55,7 +62,6 @@ int srslte_ofdm_init_(srslte_ofdm_t *q, srslte_cp_t cp, int symbol_sz, int nof_p
   q->nof_symbols = SRSLTE_CP_NSYMB(cp);
   q->cp = cp;
   q->freq_shift = false;
-  q->shift_buffer = NULL; 
   q->nof_re = nof_prb * SRSLTE_NRE;
   q->nof_guards = ((symbol_sz - q->nof_re) / 2);
   q->slot_sz = SRSLTE_SLOT_LEN(symbol_sz);
@@ -67,6 +73,32 @@ int srslte_ofdm_init_(srslte_ofdm_t *q, srslte_cp_t cp, int symbol_sz, int nof_p
   return SRSLTE_SUCCESS;
 }
 
+
+int srslte_ofdm_replan_(srslte_ofdm_t *q, srslte_cp_t cp, int symbol_sz, int nof_prb) {
+
+  if (srslte_dft_replan_c(&q->fft_plan, symbol_sz)) {
+    fprintf(stderr, "Error: Creating DFT plan\n");
+    return -1;
+  }
+
+  q->symbol_sz = (uint32_t) symbol_sz;
+  q->nof_symbols = SRSLTE_CP_NSYMB(cp);
+  q->cp = cp;
+  q->nof_re = nof_prb * SRSLTE_NRE;
+  q->nof_guards = ((symbol_sz - q->nof_re) / 2);
+  q->slot_sz = SRSLTE_SLOT_LEN(symbol_sz);
+
+  if (q->freq_shift) {
+    srslte_ofdm_set_freq_shift(q, q->freq_shift_f);
+  }
+
+  DEBUG("Replan symbol_sz=%d, nof_symbols=%d, cp=%s, nof_re=%d, nof_guards=%d\n",
+        q->symbol_sz, q->nof_symbols,
+        q->cp==SRSLTE_CP_NORM?"Normal":"Extended", q->nof_re, q->nof_guards);
+
+  return SRSLTE_SUCCESS;
+}
+
 void srslte_ofdm_free_(srslte_ofdm_t *q) {
   srslte_dft_plan_free(&q->fft_plan);
   if (q->tmp) {
@@ -78,30 +110,28 @@ void srslte_ofdm_free_(srslte_ofdm_t *q) {
   bzero(q, sizeof(srslte_ofdm_t));
 }
 
-int srslte_ofdm_rx_init(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t nof_prb) {
-  int symbol_sz = srslte_symbol_sz(nof_prb);
+int srslte_ofdm_rx_init(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t max_prb) {
+  int symbol_sz = srslte_symbol_sz(max_prb);
   if (symbol_sz < 0) {
-    fprintf(stderr, "Error: Invalid nof_prb=%d\n", nof_prb);
+    fprintf(stderr, "Error: Invalid nof_prb=%d\n", max_prb);
     return -1;
   }
-  return srslte_ofdm_init_(q, cp, symbol_sz, nof_prb, SRSLTE_DFT_FORWARD);
+  q->max_prb = max_prb;
+  return srslte_ofdm_init_(q, cp, symbol_sz, max_prb, SRSLTE_DFT_FORWARD);
 }
 
-void srslte_ofdm_rx_free(srslte_ofdm_t *q) {
-  srslte_ofdm_free_(q);
-}
-
-int srslte_ofdm_tx_init(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t nof_prb) {
+int srslte_ofdm_tx_init(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t max_prb) {
   uint32_t i;
   int ret;
   
-  int symbol_sz = srslte_symbol_sz(nof_prb);
+  int symbol_sz = srslte_symbol_sz(max_prb);
   if (symbol_sz < 0) {
-    fprintf(stderr, "Error: Invalid nof_prb=%d\n", nof_prb);
+    fprintf(stderr, "Error: Invalid nof_prb=%d\n", max_prb);
     return -1;
   }
+  q->max_prb = max_prb;
 
-  ret = srslte_ofdm_init_(q, cp, symbol_sz, nof_prb, SRSLTE_DFT_BACKWARD); 
+  ret = srslte_ofdm_init_(q, cp, symbol_sz, max_prb, SRSLTE_DFT_BACKWARD);
   
   if (ret == SRSLTE_SUCCESS) {
     srslte_dft_plan_set_norm(&q->fft_plan, false);
@@ -115,16 +145,57 @@ int srslte_ofdm_tx_init(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t nof_prb) {
   return ret;
 }
 
+int srslte_ofdm_rx_set_prb(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t nof_prb) {
+  if (nof_prb <= q->max_prb) {
+    int symbol_sz = srslte_symbol_sz(nof_prb);
+    if (symbol_sz < 0) {
+      fprintf(stderr, "Error: Invalid nof_prb=%d\n", nof_prb);
+      return -1;
+    }
+    return srslte_ofdm_replan_(q, cp, symbol_sz, nof_prb);
+  } else {
+    fprintf(stderr, "OFDM: Error calling set_prb: nof_prb must be equal or lower initialized max_prb\n");
+    return -1;
+  }
+}
+
+int srslte_ofdm_tx_set_prb(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t nof_prb) {
+  uint32_t i;
+  int ret;
+
+  if (nof_prb <= q->max_prb) {
+    int symbol_sz = srslte_symbol_sz(nof_prb);
+    if (symbol_sz < 0) {
+      fprintf(stderr, "Error: Invalid nof_prb=%d\n", nof_prb);
+      return -1;
+    }
+
+    ret = srslte_ofdm_replan_(q, cp, symbol_sz, nof_prb);
+
+    if (ret == SRSLTE_SUCCESS) {
+      /* set now zeros at CP */
+      for (i=0;i<q->nof_symbols;i++) {
+        bzero(q->tmp, q->nof_guards * sizeof(cf_t));
+        bzero(&q->tmp[q->nof_re + q->nof_guards], q->nof_guards * sizeof(cf_t));
+      }
+    }
+    return ret;
+  } else {
+    fprintf(stderr, "OFDM: Error calling set_prb: nof_prb must be equal or lower initialized max_prb\n");
+    return -1;
+  }
+}
+
+
+void srslte_ofdm_rx_free(srslte_ofdm_t *q) {
+  srslte_ofdm_free_(q);
+}
+
 /* Shifts the signal after the iFFT or before the FFT. 
  * Freq_shift is relative to inter-carrier spacing.
  * Caution: This function shall not be called during run-time 
  */
 int srslte_ofdm_set_freq_shift(srslte_ofdm_t *q, float freq_shift) {
-  q->shift_buffer = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN(q->symbol_sz));
-  if (!q->shift_buffer) {
-    perror("malloc");
-    return -1; 
-  }
   cf_t *ptr = q->shift_buffer;
   for (uint32_t n=0;n<2;n++) {
     for (uint32_t i=0;i<q->nof_symbols;i++) {
@@ -140,7 +211,7 @@ int srslte_ofdm_set_freq_shift(srslte_ofdm_t *q, float freq_shift) {
   srslte_dft_plan_set_dc(&q->fft_plan, false);  
  
   q->freq_shift = true;
-  
+  q->freq_shift_f = freq_shift;
   return SRSLTE_SUCCESS;
 }
 
diff --git a/lib/src/phy/enb/enb_dl.c b/lib/src/phy/enb/enb_dl.c
index 39606ff7f..fb9fe7d87 100644
--- a/lib/src/phy/enb/enb_dl.c
+++ b/lib/src/phy/enb/enb_dl.c
@@ -29,6 +29,8 @@
 #include <complex.h>
 #include <math.h>
 #include <string.h>
+#include <srslte/phy/common/phy_common.h>
+#include <srslte/srslte.h>
 
 
 #define CURRENT_FFTSIZE   srslte_symbol_sz(q->cell.nof_prb)
@@ -39,78 +41,67 @@
 
 #define SRSLTE_ENB_RF_AMP 0.1
 
-int srslte_enb_dl_init(srslte_enb_dl_t *q, srslte_cell_t cell)
+int srslte_enb_dl_init(srslte_enb_dl_t *q, uint32_t max_prb)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS; 
   
-  if (q                 != NULL &&
-      srslte_cell_isvalid(&cell))   
+  if (q != NULL)
   {
     ret = SRSLTE_ERROR;
     
     bzero(q, sizeof(srslte_enb_dl_t));
     
-    q->cell = cell;
-    q->cfi  = 3; 
+    q->cfi  = 3;
     q->tx_amp = SRSLTE_ENB_RF_AMP;
     
-    if (srslte_ofdm_tx_init(&q->ifft, q->cell.cp, q->cell.nof_prb)) {
+    if (srslte_ofdm_tx_init(&q->ifft, SRSLTE_CP_NORM, max_prb)) {
       fprintf(stderr, "Error initiating FFT\n");
       goto clean_exit;
     }
 
     srslte_ofdm_set_normalize(&q->ifft, true);
 
-    if (srslte_regs_init(&q->regs, q->cell)) {
-      fprintf(stderr, "Error initiating REGs\n");
-      goto clean_exit;
-    }
-    if (srslte_pbch_init(&q->pbch, q->cell)) {
+    if (srslte_pbch_init(&q->pbch)) {
       fprintf(stderr, "Error creating PBCH object\n");
       goto clean_exit;
     }
-    if (srslte_pcfich_init(&q->pcfich, &q->regs, q->cell)) {
+    if (srslte_pcfich_init(&q->pcfich, 0)) {
       fprintf(stderr, "Error creating PCFICH object\n");
       goto clean_exit;
     }
-    if (srslte_phich_init(&q->phich, &q->regs, q->cell)) {
+    if (srslte_phich_init(&q->phich, 0)) {
       fprintf(stderr, "Error creating PHICH object\n");
       goto clean_exit;
     }
 
-    if (srslte_pdcch_init(&q->pdcch, &q->regs, q->cell)) {
+    if (srslte_pdcch_init_enb(&q->pdcch, max_prb)) {
       fprintf(stderr, "Error creating PDCCH object\n");
       goto clean_exit;
     }
 
-    if (srslte_pdsch_init_tx(&q->pdsch, q->cell)) {
+    if (srslte_pdsch_init_enb(&q->pdsch, max_prb)) {
       fprintf(stderr, "Error creating PDSCH object\n");
       goto clean_exit;
     }
     
-    if (srslte_refsignal_cs_init(&q->csr_signal, q->cell)) {
+    if (srslte_refsignal_cs_init(&q->csr_signal, max_prb)) {
       fprintf(stderr, "Error initializing CSR signal (%d)\n",ret);
       goto clean_exit;
     }
     
-    for (int i=0;i<q->cell.nof_ports;i++) {
-      q->sf_symbols[i] = srslte_vec_malloc(CURRENT_SFLEN_RE * sizeof(cf_t));
+    for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
+      q->sf_symbols[i] = srslte_vec_malloc(SRSLTE_SF_LEN_RE(max_prb, SRSLTE_CP_NORM) * sizeof(cf_t));
       if (!q->sf_symbols[i]) {
         perror("malloc");
         goto clean_exit; 
       }
-      q->slot1_symbols[i] = &q->sf_symbols[i][CURRENT_SLOTLEN_RE];
+      q->slot1_symbols[i] = &q->sf_symbols[i][SRSLTE_SLOT_LEN_RE(max_prb, SRSLTE_CP_NORM)];
     }
     
-    /* Generate PSS/SSS signals */
-    srslte_pss_generate(q->pss_signal, cell.id%3);
-    srslte_sss_generate(q->sss_signal0, q->sss_signal5, cell.id);
-    
     ret = SRSLTE_SUCCESS;
     
   } else {
-    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
-            cell.id, cell.nof_ports, cell.nof_prb);      
+    fprintf(stderr, "Invalid parameters\n");
   }
 
 clean_exit: 
@@ -125,6 +116,7 @@ void srslte_enb_dl_free(srslte_enb_dl_t *q)
   if (q) {
     srslte_ofdm_tx_free(&q->ifft);
     srslte_regs_free(&q->regs);
+    srslte_pbch_free(&q->pbch);
     srslte_pcfich_free(&q->pcfich);
     srslte_phich_free(&q->phich);
     srslte_pdcch_free(&q->pdcch);
@@ -141,6 +133,71 @@ void srslte_enb_dl_free(srslte_enb_dl_t *q)
   }  
 }
 
+int srslte_enb_dl_set_cell(srslte_enb_dl_t *q, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q                 != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    srslte_enb_dl_set_cfi(q, 3);
+    q->tx_amp = SRSLTE_ENB_RF_AMP;
+
+    if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
+      if (q->cell.nof_prb != 0) {
+        srslte_regs_free(&q->regs);
+      }
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+      if (srslte_regs_init(&q->regs, q->cell)) {
+        fprintf(stderr, "Error resizing REGs\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_ofdm_rx_set_prb(&q->ifft, q->cell.cp, q->cell.nof_prb)) {
+        fprintf(stderr, "Error initiating FFT\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_pbch_set_cell(&q->pbch, q->cell)) {
+        fprintf(stderr, "Error creating PBCH object\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_pcfich_set_cell(&q->pcfich, &q->regs, q->cell)) {
+        fprintf(stderr, "Error creating PCFICH object\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_phich_set_cell(&q->phich, &q->regs, q->cell)) {
+        fprintf(stderr, "Error creating PHICH object\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pdcch_set_cell(&q->pdcch, &q->regs, q->cell)) {
+        fprintf(stderr, "Error creating PDCCH object\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pdsch_set_cell(&q->pdsch, q->cell)) {
+        fprintf(stderr, "Error creating PDSCH object\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_refsignal_cs_set_cell(&q->csr_signal, q->cell)) {
+        fprintf(stderr, "Error initializing CSR signal (%d)\n",ret);
+        return SRSLTE_ERROR;
+      }
+      /* Generate PSS/SSS signals */
+      srslte_pss_generate(q->pss_signal, cell.id%3);
+      srslte_sss_generate(q->sss_signal0, q->sss_signal5, cell.id);
+    }
+    ret = SRSLTE_SUCCESS;
+
+  } else {
+    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
+            cell.id, cell.nof_ports, cell.nof_prb);
+  }
+  return ret;
+}
+
+
+
 void srslte_enb_dl_set_amp(srslte_enb_dl_t *q, float amp)
 {
   q->tx_amp = amp; 
@@ -263,11 +320,11 @@ int srslte_enb_dl_put_pdcch_ul(srslte_enb_dl_t *q, srslte_ra_ul_dci_t *grant,
 }
 
 int srslte_enb_dl_put_pdsch(srslte_enb_dl_t *q, srslte_ra_dl_grant_t *grant, srslte_softbuffer_tx_t *softbuffer[SRSLTE_MAX_CODEWORDS],
-                            uint16_t rnti, uint32_t rv_idx, uint32_t sf_idx, 
-                            uint8_t *data[SRSLTE_MAX_CODEWORDS])
+                            uint16_t rnti, int rv_idx[SRSLTE_MAX_CODEWORDS], uint32_t sf_idx,
+                            uint8_t *data[SRSLTE_MAX_CODEWORDS], srslte_mimo_type_t mimo_type, uint32_t pmi)
 {  
   /* Configure pdsch_cfg parameters */
-  if (srslte_pdsch_cfg(&q->pdsch_cfg, q->cell, grant, q->cfi, sf_idx, rv_idx)) {
+  if (srslte_pdsch_cfg_mimo(&q->pdsch_cfg, q->cell, grant, q->cfi, sf_idx, rv_idx, mimo_type, pmi)) {
     fprintf(stderr, "Error configuring PDSCH\n");
     return SRSLTE_ERROR;
   }
diff --git a/lib/src/phy/enb/enb_ul.c b/lib/src/phy/enb/enb_ul.c
index c399d715c..db05d44ea 100644
--- a/lib/src/phy/enb/enb_ul.c
+++ b/lib/src/phy/enb/enb_ul.c
@@ -39,78 +39,54 @@
 
 #define MAX_CANDIDATES  16
 
-int srslte_enb_ul_init(srslte_enb_ul_t *q, srslte_cell_t cell, 
-                       srslte_prach_cfg_t *prach_cfg, 
-                       srslte_refsignal_dmrs_pusch_cfg_t *pusch_cfg, 
-                       srslte_pusch_hopping_cfg_t *hopping_cfg, 
-                       srslte_pucch_cfg_t *pucch_cfg)
+int srslte_enb_ul_init(srslte_enb_ul_t *q,
+                       uint32_t max_prb)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS; 
   
-  if (q                 != NULL &&
-      srslte_cell_isvalid(&cell))   
+  if (q != NULL)
   {
     ret = SRSLTE_ERROR;
     
     bzero(q, sizeof(srslte_enb_ul_t));
     
-    q->cell = cell;
-    
-    if (hopping_cfg) {
-      memcpy(&q->hopping_cfg, hopping_cfg, sizeof(srslte_pusch_hopping_cfg_t));
-    } 
-    
     q->users = calloc(sizeof(srslte_enb_ul_user_t*), (1+SRSLTE_SIRNTI));
     if (!q->users) {
       perror("malloc");
       goto clean_exit;
     }
     
-    if (srslte_ofdm_rx_init(&q->fft, q->cell.cp, q->cell.nof_prb)) {
+    if (srslte_ofdm_rx_init(&q->fft, SRSLTE_CP_NORM, max_prb)) {
       fprintf(stderr, "Error initiating FFT\n");
       goto clean_exit;
     }
     srslte_ofdm_set_normalize(&q->fft, false);
     srslte_ofdm_set_freq_shift(&q->fft, -0.5);
 
-    if (srslte_pucch_init(&q->pucch, q->cell)) {
+    if (srslte_pucch_init(&q->pucch)) {
       fprintf(stderr, "Error creating PUCCH object\n");
       goto clean_exit;
     }
 
-    if (srslte_pusch_init(&q->pusch, q->cell)) {
+    if (srslte_pusch_init_enb(&q->pusch, max_prb)) {
       fprintf(stderr, "Error creating PUSCH object\n");
       goto clean_exit;
     }
-    
-    if (prach_cfg) {
-      if (srslte_prach_init_cfg(&q->prach, prach_cfg, q->cell.nof_prb)) {
-        fprintf(stderr, "Error initiating PRACH\n");
-        goto clean_exit; 
-      }
-      srslte_prach_set_detect_factor(&q->prach, 60);    
-    }
-    
+
     srslte_pucch_set_threshold(&q->pucch, 0.8);
-    
-    if (srslte_chest_ul_init(&q->chest, cell)) {
+
+    if (srslte_chest_ul_init(&q->chest, max_prb)) {
       fprintf(stderr, "Error initiating channel estimator\n");
       goto clean_exit; 
     }
-    
-    // Configure common PUCCH configuration 
-    srslte_pucch_set_cfg(&q->pucch, pucch_cfg, pusch_cfg->group_hopping_en);
-    
-    // SRS is a dedicated configuration
-    srslte_chest_ul_set_cfg(&q->chest, pusch_cfg, pucch_cfg, NULL);
-        
-    q->sf_symbols = srslte_vec_malloc(CURRENT_SFLEN_RE * sizeof(cf_t));
+
+    q->sf_symbols = srslte_vec_malloc(SRSLTE_SF_LEN_RE(max_prb, SRSLTE_CP_NORM) * sizeof(cf_t));
     if (!q->sf_symbols) {
       perror("malloc");
       goto clean_exit; 
     }
     
-    q->ce = srslte_vec_malloc(CURRENT_SFLEN_RE * sizeof(cf_t));
+    q->ce = srslte_vec_malloc(SRSLTE_SF_LEN_RE(max_prb, SRSLTE_CP_NORM) * sizeof(cf_t));
     if (!q->ce) {
       perror("malloc");
       goto clean_exit; 
@@ -119,8 +95,7 @@ int srslte_enb_ul_init(srslte_enb_ul_t *q, srslte_cell_t cell,
     ret = SRSLTE_SUCCESS;
     
   } else {
-    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
-            cell.id, cell.nof_ports, cell.nof_prb);      
+    fprintf(stderr, "Invalid parameters\n");
   }
 
 clean_exit: 
@@ -135,7 +110,7 @@ void srslte_enb_ul_free(srslte_enb_ul_t *q)
   if (q) {
     
     if (q->users) {
-      for (int i=0;i<SRSLTE_SIRNTI;i++) {
+      for (int i=0;i<=SRSLTE_SIRNTI;i++) {
         if (q->users[i]) {
           free(q->users[i]);
         }
@@ -158,11 +133,73 @@ void srslte_enb_ul_free(srslte_enb_ul_t *q)
   }  
 }
 
+int srslte_enb_ul_set_cell(srslte_enb_ul_t *q, srslte_cell_t cell,
+                           srslte_prach_cfg_t *prach_cfg,
+                           srslte_refsignal_dmrs_pusch_cfg_t *pusch_cfg,
+                           srslte_pusch_hopping_cfg_t *hopping_cfg,
+                           srslte_pucch_cfg_t *pucch_cfg)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q                 != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    if (cell.id != q->cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+
+      if (hopping_cfg) {
+        memcpy(&q->hopping_cfg, hopping_cfg, sizeof(srslte_pusch_hopping_cfg_t));
+      }
+
+      if (srslte_ofdm_rx_set_prb(&q->fft, q->cell.cp, q->cell.nof_prb)) {
+        fprintf(stderr, "Error initiating FFT\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pucch_set_cell(&q->pucch, q->cell)) {
+        fprintf(stderr, "Error creating PUCCH object\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pusch_set_cell(&q->pusch, q->cell)) {
+        fprintf(stderr, "Error creating PUSCH object\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (prach_cfg) {
+        if (srslte_prach_init_cfg(&q->prach, prach_cfg, q->cell.nof_prb)) {
+          fprintf(stderr, "Error initiating PRACH\n");
+          return SRSLTE_ERROR;
+        }
+        srslte_prach_set_detect_factor(&q->prach, 60);
+      }
+
+      if (srslte_chest_ul_set_cell(&q->chest, cell)) {
+        fprintf(stderr, "Error initiating channel estimator\n");
+        return SRSLTE_ERROR;
+      }
+
+      // Configure common PUCCH configuration
+      srslte_pucch_set_cfg(&q->pucch, pucch_cfg, pusch_cfg->group_hopping_en);
+
+      // SRS is a dedicated configuration
+      srslte_chest_ul_set_cfg(&q->chest, pusch_cfg, pucch_cfg, NULL);
+
+      ret = SRSLTE_SUCCESS;
+    }
+  } else {
+    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
+            cell.id, cell.nof_ports, cell.nof_prb);
+  }
+  return ret;
+}
+
+
 int srslte_enb_ul_add_rnti(srslte_enb_ul_t *q, uint16_t rnti)
 {
   if (!q->users[rnti]) {
     q->users[rnti] = calloc(1, sizeof(srslte_enb_ul_user_t));
-    
+
     if (srslte_pucch_set_crnti(&q->pucch, rnti)) {
       fprintf(stderr, "Error setting PUCCH rnti\n");
       return -1;
@@ -183,7 +220,7 @@ void srslte_enb_ul_rem_rnti(srslte_enb_ul_t *q, uint16_t rnti)
   if (q->users[rnti]) {
     free(q->users[rnti]); 
     q->users[rnti] = NULL; 
-    srslte_pusch_clear_rnti(&q->pusch, rnti);
+    srslte_pusch_free_rnti(&q->pusch, rnti);
   }
 }
 
diff --git a/lib/src/phy/phch/pbch.c b/lib/src/phy/phch/pbch.c
index 783ae1e04..2eaa20501 100644
--- a/lib/src/phy/phch/pbch.c
+++ b/lib/src/phy/phch/pbch.c
@@ -32,6 +32,8 @@
 #include <stdbool.h>
 #include <assert.h>
 #include <math.h>
+#include <srslte/srslte.h>
+#include <srslte/phy/common/phy_common.h>
 
 #include "prb_dl.h"
 #include "srslte/phy/phch/pbch.h"
@@ -43,7 +45,6 @@
 #define PBCH_RE_CP_NORM    240
 #define PBCH_RE_CP_EXT     216
 
-
 const uint8_t srslte_crc_mask[4][16] = {
     { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, 
     { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }, 
@@ -139,33 +140,18 @@ int srslte_pbch_get(cf_t *slot1_data, cf_t *pbch, srslte_cell_t cell) {
  * At the receiver, the field nof_ports in the cell structure indicates the 
  * maximum number of BS transmitter ports to look for.  
  */
-int srslte_pbch_init(srslte_pbch_t *q, srslte_cell_t cell) {
+int srslte_pbch_init(srslte_pbch_t *q) {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
-  if (q                       != NULL &&
-      srslte_cell_isvalid(&cell))
+  if (q != NULL)
   {
     ret = SRSLTE_ERROR;
 
     bzero(q, sizeof(srslte_pbch_t));
-    
-    if (cell.nof_ports == 0) {
-      q->search_all_ports = true; 
-      cell.nof_ports = SRSLTE_MAX_PORTS; 
-    } else {
-      q->search_all_ports = false; 
-    }
-    
-    q->cell = cell;
-    q->nof_symbols = (SRSLTE_CP_ISNORM(q->cell.cp)) ? PBCH_RE_CP_NORM : PBCH_RE_CP_EXT;
-    
+
     if (srslte_modem_table_lte(&q->mod, SRSLTE_MOD_QPSK)) {
       goto clean;
     }
-    if (srslte_sequence_pbch(&q->seq, q->cell.cp, q->cell.id)) {
-      goto clean;
-    }
-
     int poly[3] = { 0x6D, 0x4F, 0x57 };
     if (srslte_viterbi_init(&q->decoder, SRSLTE_VITERBI_37, poly, 40, true)) {
       goto clean;
@@ -178,12 +164,14 @@ int srslte_pbch_init(srslte_pbch_t *q, srslte_cell_t cell) {
     q->encoder.tail_biting = true;
     memcpy(q->encoder.poly, poly, 3 * sizeof(int));
 
+    q->nof_symbols = PBCH_RE_CP_NORM;
+
     q->d = srslte_vec_malloc(sizeof(cf_t) * q->nof_symbols);
     if (!q->d) {
       goto clean;
     }
     int i;
-    for (i = 0; i < q->cell.nof_ports; i++) {
+    for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
       q->ce[i] = srslte_vec_malloc(sizeof(cf_t) * q->nof_symbols);
       if (!q->ce[i]) {
         goto clean;
@@ -209,6 +197,7 @@ int srslte_pbch_init(srslte_pbch_t *q, srslte_cell_t cell) {
     if (!q->rm_b) {
       goto clean;
     }
+
     ret = SRSLTE_SUCCESS;
   }
 clean: 
@@ -219,11 +208,11 @@ clean:
 }
 
 void srslte_pbch_free(srslte_pbch_t *q) {
-  if (q->d) {
-    free(q->d);
-  }
+  srslte_sequence_free(&q->seq);
+  srslte_modem_table_free(&q->mod);
+  srslte_viterbi_free(&q->decoder);
   int i;
-  for (i = 0; i < q->cell.nof_ports; i++) {
+  for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
     if (q->ce[i]) {
       free(q->ce[i]);
     }
@@ -243,12 +232,36 @@ void srslte_pbch_free(srslte_pbch_t *q) {
   if (q->rm_b) {
     free(q->rm_b);
   }
-  srslte_sequence_free(&q->seq);
-  srslte_modem_table_free(&q->mod);
-  srslte_viterbi_free(&q->decoder);
-
+  if (q->d) {
+    free(q->d);
+  }
   bzero(q, sizeof(srslte_pbch_t));
+}
 
+int srslte_pbch_set_cell(srslte_pbch_t *q, srslte_cell_t cell) {
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q                       != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    if (cell.nof_ports == 0) {
+      q->search_all_ports = true;
+      cell.nof_ports = SRSLTE_MAX_PORTS;
+    } else {
+      q->search_all_ports = false;
+    }
+
+    if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+      if (srslte_sequence_pbch(&q->seq, q->cell.cp, q->cell.id)) {
+        return SRSLTE_ERROR;
+      }
+    }
+    q->nof_symbols = (SRSLTE_CP_ISNORM(q->cell.cp)) ? PBCH_RE_CP_NORM : PBCH_RE_CP_EXT;
+
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
 }
 
 
@@ -476,6 +489,7 @@ int srslte_pbch_decode(srslte_pbch_t *q, cf_t *slot1_symbols, cf_t *ce_slot1[SRS
     } else {
       nant = q->cell.nof_ports; 
     }
+
     do {
       if (nant != 3) {
         DEBUG("Trying %d TX antennas with %d frames\n", nant, q->frame_idx);
@@ -486,7 +500,7 @@ int srslte_pbch_decode(srslte_pbch_t *q, cf_t *slot1_symbols, cf_t *ce_slot1[SRS
           srslte_predecoding_single(q->symbols[0], q->ce[0], q->d, q->nof_symbols, noise_estimate);
         } else {
           srslte_predecoding_diversity(q->symbols[0], q->ce, x, nant,
-              q->nof_symbols);
+                                       q->nof_symbols);
           srslte_layerdemap_diversity(x, q->d, nant, q->nof_symbols / nant);
         }
 
@@ -577,7 +591,7 @@ int srslte_pbch_encode(srslte_pbch_t *q, uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_
     if (q->cell.nof_ports > 1) {
       srslte_layermap_diversity(q->d, x, q->cell.nof_ports, q->nof_symbols);
       srslte_precoding_diversity(x, q->symbols, q->cell.nof_ports,
-          q->nof_symbols / q->cell.nof_ports);
+                                 q->nof_symbols / q->cell.nof_ports);
     } else {
       memcpy(q->symbols[0], q->d, q->nof_symbols * sizeof(cf_t));
     }
diff --git a/lib/src/phy/phch/pcfich.c b/lib/src/phy/phch/pcfich.c
index ec1e13abc..d960504fe 100644
--- a/lib/src/phy/phch/pcfich.c
+++ b/lib/src/phy/phch/pcfich.c
@@ -57,38 +57,24 @@ bool srslte_pcfich_exists(int nframe, int nslot) {
   return true;
 }
 
-int srslte_pcfich_init(srslte_pcfich_t *q, srslte_regs_t *regs, srslte_cell_t cell) {
-  return srslte_pcfich_init_multi(q, regs, cell, 1);  
-}
-
 /** Initializes the pcfich channel receiver. 
  * On error, returns -1 and frees the structrure 
  */
-int srslte_pcfich_init_multi(srslte_pcfich_t *q, srslte_regs_t *regs, srslte_cell_t cell, uint32_t nof_rx_antennas) {
+int srslte_pcfich_init(srslte_pcfich_t *q, uint32_t nof_rx_antennas) {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   
-  if (q                         != NULL &&
-      regs                      != NULL &&
-      srslte_cell_isvalid(&cell)) 
+  if (q != NULL)
   {   
     ret = SRSLTE_ERROR;
     
     bzero(q, sizeof(srslte_pcfich_t));
-    q->cell = cell;
-    q->regs = regs;
+    q->nof_rx_antennas = nof_rx_antennas;
     q->nof_symbols = PCFICH_RE;
-    q->nof_rx_antennas = nof_rx_antennas; 
-    
+
     if (srslte_modem_table_lte(&q->mod, SRSLTE_MOD_QPSK)) {
       goto clean;
     }
 
-    for (int nsf = 0; nsf < SRSLTE_NSUBFRAMES_X_FRAME; nsf++) {
-      if (srslte_sequence_pcfich(&q->seq[nsf], 2 * nsf, q->cell.id)) {
-        goto clean;
-      }
-    }
-    
     /* convert cfi bit tables to floats for demodulation */
     for (int i=0;i<3;i++) {
       for (int j=0;j<PCFICH_CFI_LEN;j++) {
@@ -115,6 +101,28 @@ void srslte_pcfich_free(srslte_pcfich_t *q) {
   bzero(q, sizeof(srslte_pcfich_t));
 }
 
+int srslte_pcfich_set_cell(srslte_pcfich_t *q, srslte_regs_t *regs, srslte_cell_t cell) {
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q                         != NULL &&
+      regs                      != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    q->regs = regs;
+    if (cell.id != q->cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+      for (int nsf = 0; nsf < SRSLTE_NSUBFRAMES_X_FRAME; nsf++) {
+        if (srslte_sequence_pcfich(&q->seq[nsf], 2 * nsf, q->cell.id)) {
+          return SRSLTE_ERROR;
+        }
+      }
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
+
 /** Finds the CFI with minimum distance with the vector of received 32 bits.
  * Saves the CFI value in the cfi pointer and returns the distance.
  */
@@ -197,7 +205,7 @@ int srslte_pcfich_decode_multi(srslte_pcfich_t *q, cf_t *sf_symbols[SRSLTE_MAX_P
       }
 
       q_symbols[j] = q->symbols[j];
-      
+
       /* extract channel estimates */
       for (i = 0; i < q->cell.nof_ports; i++) {
         if (q->nof_symbols != srslte_regs_pcfich_get(q->regs, ce[i][j], q->ce[i][j])) {
diff --git a/lib/src/phy/phch/pdcch.c b/lib/src/phy/phch/pdcch.c
index f4c59b321..215324e6b 100644
--- a/lib/src/phy/phch/pdcch.c
+++ b/lib/src/phy/phch/pdcch.c
@@ -61,41 +61,20 @@ float srslte_pdcch_coderate(uint32_t nof_bits, uint32_t l) {
 }
 
 /** Initializes the PDCCH transmitter and receiver */
-int srslte_pdcch_init(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t cell) {
-  return srslte_pdcch_init_multi(q, regs, cell, 1);
-}
-
-int srslte_pdcch_init_tx(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t cell) {
-  return srslte_pdcch_init_txrx(q, regs, cell, 1, false);
-}
-
-int srslte_pdcch_init_rx(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t cell, uint32_t nof_rx_antennas) {
-  return srslte_pdcch_init_txrx(q, regs, cell, nof_rx_antennas, true);
-}
-
-int srslte_pdcch_init_multi(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t cell, uint32_t nof_rx_antennas) {
-  return srslte_pdcch_init_txrx(q, regs, cell, nof_rx_antennas, true);
-}
-
-int srslte_pdcch_init_txrx(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t cell, uint32_t nof_rx_antennas, bool isReceiver)
+static int pdcch_init(srslte_pdcch_t *q, uint32_t max_prb, uint32_t nof_rx_antennas, bool is_ue)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
-  if (q                         != NULL &&
-      regs                      != NULL &&
-      srslte_cell_isvalid(&cell)) 
+  if (q != NULL)
   {   
     ret = SRSLTE_ERROR;
     bzero(q, sizeof(srslte_pdcch_t));
-    q->cell = cell;
-    q->regs = regs;
-    q->nof_rx_antennas = nof_rx_antennas; 
-    
+    q->nof_rx_antennas = nof_rx_antennas;
+    q->is_ue           = is_ue;
     /* Allocate memory for the maximum number of PDCCH bits (CFI=3) */
-    q->max_bits = (srslte_regs_pdcch_nregs(q->regs, 3) / 9) * 72;
+    q->max_bits = max_prb*3*12*2;
 
-    INFO("Init PDCCH: Max bits: %d, %d ports.\n", 
-         q->max_bits, q->cell.nof_ports);
+    INFO("Init PDCCH: Max bits: %d\n", q->max_bits);
 
     if (srslte_modem_table_lte(&q->mod, SRSLTE_MOD_QPSK)) {
       goto clean;
@@ -104,14 +83,6 @@ int srslte_pdcch_init_txrx(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t
       goto clean;
     }
 
-    for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-      // we need to pregenerate the sequence for the maximum number of bits, which is 8 times 
-      // the maximum number of REGs (for CFI=3)
-      if (srslte_sequence_pdcch(&q->seq[i], 2 * i, q->cell.id, 8*srslte_regs_pdcch_nregs(q->regs, 3))) {
-        goto clean;
-      }
-    }
-
     int poly[3] = { 0x6D, 0x4F, 0x57 };
     if (srslte_viterbi_init(&q->decoder, SRSLTE_VITERBI_37, poly, SRSLTE_DCI_MAX_BITS + 16, true)) {
       goto clean;
@@ -134,8 +105,16 @@ int srslte_pdcch_init_txrx(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t
       goto clean;
     }
 
-    if (isReceiver) {
-      for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
+    for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
+      q->x[i] = srslte_vec_malloc(sizeof(cf_t) * q->max_bits / 2);
+      if (!q->x[i]) {
+        goto clean;
+      }
+      q->symbols[i] = srslte_vec_malloc(sizeof(cf_t) * q->max_bits / 2);
+      if (!q->symbols[i]) {
+        goto clean;
+      }
+      if (q->is_ue) {
         for (int j = 0; j < q->nof_rx_antennas; j++) {
           q->ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * q->max_bits / 2);
           if (!q->ce[i][j]) {
@@ -145,20 +124,6 @@ int srslte_pdcch_init_txrx(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t
       }
     }
 
-    for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
-      q->x[i] = srslte_vec_malloc(sizeof(cf_t) * q->max_bits / 2);
-      if (!q->x[i]) {
-        goto clean;
-      }
-    }
-
-    for (int j = 0; j < ((isReceiver) ? q->nof_rx_antennas : cell.nof_ports); j++) {
-      q->symbols[j] = srslte_vec_malloc(sizeof(cf_t) * q->max_bits / 2);
-      if (!q->symbols[j]) {
-        goto clean;
-      }
-    }
-
     ret = SRSLTE_SUCCESS;
   }
   clean: 
@@ -168,6 +133,14 @@ int srslte_pdcch_init_txrx(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t
   return ret;
 }
 
+int srslte_pdcch_init_enb(srslte_pdcch_t *q, uint32_t max_prb) {
+  return pdcch_init(q, max_prb, 0, false);
+}
+
+int srslte_pdcch_init_ue(srslte_pdcch_t *q, uint32_t max_prb, uint32_t nof_rx_antennas) {
+  return pdcch_init(q, max_prb, nof_rx_antennas, true);
+}
+
 void srslte_pdcch_free(srslte_pdcch_t *q) {
 
   if (q->e) {
@@ -180,18 +153,18 @@ void srslte_pdcch_free(srslte_pdcch_t *q) {
     free(q->d);
   }
   for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
-    for (int j=0;j<q->nof_rx_antennas;j++) {
-      if (q->ce[i][j]) {
-        free(q->ce[i][j]);
-      }      
-    }
     if (q->x[i]) {
       free(q->x[i]);
     }
-  }
-  for (int j=0;j<q->nof_rx_antennas;j++) {
-    if (q->symbols[j]) {
-      free(q->symbols[j]);
+    if (q->symbols[i]) {
+      free(q->symbols[i]);
+    }
+    if (q->is_ue) {
+      for (int j=0;j<q->nof_rx_antennas;j++) {
+        if (q->ce[i][j]) {
+          free(q->ce[i][j]);
+        }
+      }
     }
   }
   for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
@@ -205,6 +178,39 @@ void srslte_pdcch_free(srslte_pdcch_t *q) {
 
 }
 
+int srslte_pdcch_set_cell(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q                         != NULL &&
+      regs                      != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    q->regs = regs;
+
+    /* Allocate memory for the maximum number of PDCCH bits (CFI=3) */
+    q->max_bits = (srslte_regs_pdcch_nregs(q->regs, 3) / 9) * 72;
+
+    INFO("PDCCH: Cell config PCI=%d, %d ports.\n",
+         q->cell.id, q->cell.nof_ports);
+
+    if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+
+      for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
+        // we need to pregenerate the sequence for the maximum number of bits, which is 8 times
+        // the maximum number of REGs (for CFI=3)
+        if (srslte_sequence_pdcch(&q->seq[i], 2 * i, q->cell.id, 8*srslte_regs_pdcch_nregs(q->regs, 3))) {
+          return SRSLTE_ERROR;
+        }
+      }
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
+
 uint32_t srslte_pdcch_ue_locations(srslte_pdcch_t *q, srslte_dci_location_t *c, uint32_t max_candidates,
                         uint32_t nsubframe, uint32_t cfi, uint16_t rnti) 
 {
@@ -490,7 +496,7 @@ int srslte_pdcch_extract_llr_multi(srslte_pdcch_t *q, cf_t *sf_symbols[SRSLTE_MA
 
     /* descramble */
     srslte_scrambling_f_offset(&q->seq[nsubframe], q->llr, 0, e_bits);
-    
+
     ret = SRSLTE_SUCCESS;
   } 
   return ret;  
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index f761dbc3d..0908793db 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -27,25 +27,22 @@
 #include <stdint.h>
 #include <stdio.h>
 #include <string.h>
-#include <strings.h>
 #include <stdlib.h>
 #include <stdbool.h>
-#include <assert.h>
-#include <math.h>
+#include <srslte/phy/phch/pdsch_cfg.h>
+#include <srslte/phy/common/sequence.h>
+#include <srslte/phy/phch/pdsch.h>
 
 #include "prb_dl.h"
 #include "srslte/phy/phch/pdsch.h"
-#include "srslte/phy/phch/sch.h"
-#include "srslte/phy/common/phy_common.h"
-#include "srslte/phy/utils/bit.h"
 #include "srslte/phy/utils/debug.h"
 #include "srslte/phy/utils/vector.h"
+#include "srslte/phy/utils/bit.h"
 
 
 #define MAX_PDSCH_RE(cp) (2 * SRSLTE_CP_NSYMB(cp) * 12)
 
 
-
 const static srslte_mod_t modulations[4] =
     { SRSLTE_MOD_BPSK, SRSLTE_MOD_QPSK, SRSLTE_MOD_16QAM, SRSLTE_MOD_64QAM };
     
@@ -197,30 +194,24 @@ int srslte_pdsch_get(srslte_pdsch_t *q, cf_t *sf_symbols, cf_t *symbols,
   return srslte_pdsch_cp(q, sf_symbols, symbols, grant, lstart, subframe, false);
 }
 
-/** Initializes the PDCCH transmitter or receiver */
-int srslte_pdsch_init(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_antennas, bool is_receiver)
+/** Initializes the PDSCH transmitter and receiver */
+static int pdsch_init(srslte_pdsch_t *q, uint32_t max_prb, bool is_ue, uint32_t nof_antennas)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
-  int i;
 
- if (q != NULL                  &&
-     srslte_cell_isvalid(&cell) && 
-     nof_antennas <= SRSLTE_MAX_PORTS) 
-  {   
+  if (q != NULL)
+  {
     
     bzero(q, sizeof(srslte_pdsch_t));
     ret = SRSLTE_ERROR;
     
-    q->cell = cell;
-    q->max_re = q->cell.nof_prb * MAX_PDSCH_RE(q->cell.cp);
-    if (is_receiver) {
-      q->nof_rx_antennas = nof_antennas;
-    }
+    q->max_re          = max_prb * MAX_PDSCH_RE(q->cell.cp);
+    q->is_ue           = is_ue;
+    q->nof_rx_antennas = nof_antennas;
 
-    INFO("Init PDSCH: %d ports %d PRBs, max_symbols: %d\n", q->cell.nof_ports,
-        q->cell.nof_prb, q->max_re);
+    INFO("Init PDSCH: %d PRBs, max_symbols: %d\n", max_prb, q->max_re);
 
-    for (i = 0; i < 4; i++) {
+    for (int i = 0; i < 4; i++) {
       if (srslte_modem_table_lte(&q->mod[i], modulations[i])) {
         goto clean;
       }
@@ -232,7 +223,7 @@ int srslte_pdsch_init(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_antenn
       goto clean;
     }
 
-    for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
       // Allocate int16_t for reception (LLRs)
       q->e[i] = srslte_vec_malloc(sizeof(int16_t) * q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM));
       if (!q->e[i]) {
@@ -245,18 +236,17 @@ int srslte_pdsch_init(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_antenn
       }
     }
 
-    /* Layer mapped symbols memory allocation */
-    for (i = 0; i < q->cell.nof_ports; i++) {
+    for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
       q->x[i] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
       if (!q->x[i]) {
         goto clean;
       }
-    }
-
-    /* If it is the receiver side, allocate estimated channel */
-    if (is_receiver) {
-      for (i = 0; i < q->cell.nof_ports; i++) {
-        for (int j = 0; j < q->nof_rx_antennas; j++) {
+      q->symbols[i] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
+      if (!q->symbols[i]) {
+        goto clean;
+      }
+      if (q->is_ue) {
+        for (int j=0;j<q->nof_rx_antennas;j++) {
           q->ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
           if (!q->ce[i][j]) {
             goto clean;
@@ -264,42 +254,40 @@ int srslte_pdsch_init(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_antenn
         }
       }
     }
-    for (int j=0;j<SRSLTE_MAX(q->nof_rx_antennas, q->cell.nof_ports);j++) {
-      q->symbols[j] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
-      if (!q->symbols[j]) {
-        goto clean;
-      }              
-    }
-    
-    /* Allocate User memory (all zeros) */
-    q->users = calloc(sizeof(srslte_pdsch_user_t*), 1+SRSLTE_SIRNTI);
+
+    q->users = calloc(sizeof(srslte_pdsch_user_t*), q->is_ue?1:(1+SRSLTE_SIRNTI));
     if (!q->users) {
       perror("malloc");
       goto clean;
     }
- }
 
-  ret = SRSLTE_SUCCESS;
+    if (srslte_sequence_init(&q->tmp_seq, q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
+      goto clean;
+    }
 
-  clean:
+    ret = SRSLTE_SUCCESS;
+  }
+
+  clean: 
   if (ret == SRSLTE_ERROR) {
     srslte_pdsch_free(q);
   }
   return ret;
 }
 
-int srslte_pdsch_init_tx(srslte_pdsch_t *q, srslte_cell_t cell) {
-  return srslte_pdsch_init(q, cell, 0, false);
+int srslte_pdsch_init_ue(srslte_pdsch_t *q, uint32_t max_prb, uint32_t nof_antennas)
+{
+  return pdsch_init(q, max_prb, true, nof_antennas);
 }
 
-int srslte_pdsch_init_rx(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_antennas) {
-  return srslte_pdsch_init(q, cell, nof_antennas, true);
+int srslte_pdsch_init_enb(srslte_pdsch_t *q, uint32_t max_prb)
+{
+  return pdsch_init(q, max_prb, false, 0);
 }
 
 void srslte_pdsch_free(srslte_pdsch_t *q) {
-  int i;
 
-  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
 
     if (q->e[i]) {
       free(q->e[i]);
@@ -313,69 +301,107 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
   /* Free sch objects */
   srslte_sch_free(&q->dl_sch);
 
-  for (i = 0; i < q->cell.nof_ports; i++) {
+  for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
     if (q->x[i]) {
       free(q->x[i]);
     }
-    for (int j=0;j<q->nof_rx_antennas;j++) {
-      if (q->ce[i][j]) {
-        free(q->ce[i][j]);
+    if (q->symbols[i]) {
+      free(q->symbols[i]);
+    }
+    if (q->is_ue) {
+      for (int j=0;j<q->nof_rx_antennas;j++) {
+        if (q->ce[i][j]) {
+          free(q->ce[i][j]);
+        }
       }
     }
   }
-  for (int j=0;j<SRSLTE_MAX_PORTS;j++) {
-    if (q->symbols[j]) {
-      free(q->symbols[j]);
-    }          
-  }
   if (q->users) {
-    for (uint16_t u=0;u<SRSLTE_SIRNTI;u++) {
-      if (q->users[u]) {
-        srslte_pdsch_free_rnti(q, u);
+    if (q->is_ue) {
+      srslte_pdsch_free_rnti(q, 0);
+    } else {
+      for (int u=0;u<=SRSLTE_SIRNTI;u++) {
+        if (q->users[u]) {
+          srslte_pdsch_free_rnti(q, u);
+        }
       }
-    }      
+    }
     free(q->users);
   }
-  for (i = 0; i < 4; i++) {
+
+  srslte_sequence_free(&q->tmp_seq);
+
+  for (int i = 0; i < 4; i++) {
     srslte_modem_table_free(&q->mod[i]);
   }
 
   bzero(q, sizeof(srslte_pdsch_t));
 }
 
-/* Precalculate the PDSCH scramble sequences for a given RNTI. This function takes a while
+int srslte_pdsch_set_cell(srslte_pdsch_t *q, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q != NULL                  &&
+      srslte_cell_isvalid(&cell))
+  {
+    memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+    q->max_re = q->cell.nof_prb * MAX_PDSCH_RE(q->cell.cp);
+
+    INFO("PDSCH: Cell config PCI=%d, %d ports, %d PRBs, max_symbols: %d\n", q->cell.nof_ports,
+         q->cell.id, q->cell.nof_prb, q->max_re);
+
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
+/* Precalculate the PDSCH scramble sequences for a given RNTI. This function takes a while 
  * to execute, so shall be called once the final C-RNTI has been allocated for the session.
  */
 int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
-  uint32_t i, j;
-  if (!q->users[rnti]) {
-    q->users[rnti] = calloc(1, sizeof(srslte_pdsch_user_t));
-    if (q->users[rnti]) {
-      for (i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-        for (j = 0; j < SRSLTE_MAX_CODEWORDS; j++) {
-          if (srslte_sequence_pdsch(&q->users[rnti]->seq[j][i], rnti, j, 2 * i, q->cell.id,
-                                    q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
-            ERROR("Generating scrambling sequence");
-            return SRSLTE_ERROR;
-          }
+  uint32_t rnti_idx = q->is_ue?0:rnti;
+
+  if (!q->users[rnti_idx] || q->is_ue) {
+    if (!q->users[rnti_idx]) {
+      q->users[rnti_idx] = calloc(1, sizeof(srslte_pdsch_user_t));
+      if(!q->users[rnti_idx]) {
+        perror("calloc");
+        return -1;
+      }
+    }
+    for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
+      for (int j = 0; j < SRSLTE_MAX_CODEWORDS; j++) {
+        if (srslte_sequence_pdsch(&q->users[rnti_idx]->seq[j][i], rnti, j, 2 * i, q->cell.id,
+                                  q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM)))
+        {
+          fprintf(stderr, "Error initializing PDSCH scrambling sequence\n");
+          srslte_pdsch_free_rnti(q, rnti);
+          return SRSLTE_ERROR;
         }
       }
-      q->users[rnti]->sequence_generated = true;
     }
+    q->ue_rnti = rnti; 
+    q->users[rnti_idx]->cell_id = q->cell.id;
+    q->users[rnti_idx]->sequence_generated = true;
+  } else {
+    fprintf(stderr, "Error generating PDSCH sequence: rnti=0x%x already generated\n", rnti);
   }
   return SRSLTE_SUCCESS;
 }
 
 void srslte_pdsch_free_rnti(srslte_pdsch_t* q, uint16_t rnti)
 {
-  if (q->users[rnti]) {
+  uint32_t rnti_idx = q->is_ue?0:rnti;
+  if (q->users[rnti_idx]) {
     for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
       for (int j = 0; j < SRSLTE_MAX_CODEWORDS; j++) {
-        srslte_sequence_free(&q->users[rnti]->seq[j][i]);
+        srslte_sequence_free(&q->users[rnti_idx]->seq[j][i]);
       }
     }
-    free(q->users[rnti]);
-    q->users[rnti] = NULL;
+    free(q->users[rnti_idx]);
+    q->users[rnti_idx] = NULL;
+    q->ue_rnti = 0;
   }
 }
 
@@ -390,7 +416,7 @@ static void pdsch_decode_debug(srslte_pdsch_t *q, srslte_pdsch_cfg_t *cfg,
         break;
       }
       DEBUG("SAVED FILE %s: received subframe symbols\n", filename);
-      srslte_vec_save_file(filename, sf_symbols, SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+      srslte_vec_save_file(filename, sf_symbols[j], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
 
       for (int i = 0; i < q->cell.nof_ports; i++) {
         if (snprintf(filename, FILENAME_MAX, "hest_%d%d.dat", i, j) < 0) {
@@ -401,11 +427,21 @@ static void pdsch_decode_debug(srslte_pdsch_t *q, srslte_pdsch_cfg_t *cfg,
         srslte_vec_save_file(filename, ce[i][j], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
       }
     }
-    DEBUG("SAVED FILE pdsch_symbols.dat: symbols after equalization\n",0);
-    srslte_vec_save_file("pdsch_symbols.dat", q->d, cfg->nbits[0].nof_re*sizeof(cf_t));
+    for (int i=0;i<cfg->nof_layers;i++) {
+      if (snprintf(filename, FILENAME_MAX, "pdsch_symbols_%d.dat", i) < 0) {
+        ERROR("Generating file name");
+        break;
+      }
+      DEBUG("SAVED FILE %s: symbols after equalization\n", filename);
+      srslte_vec_save_file(filename, q->d[i], cfg->nbits[0].nof_re*sizeof(cf_t));
 
-    DEBUG("SAVED FILE llr.dat: LLR estimates after demodulation and descrambling\n",0);
-    srslte_vec_save_file("llr.dat", q->e, cfg->nbits[0].nof_bits*sizeof(int16_t));
+      if (snprintf(filename, FILENAME_MAX, "llr_%d.dat", i) < 0) {
+        ERROR("Generating file name");
+        break;
+      }
+      DEBUG("SAVED FILE %s: LLR estimates after demodulation and descrambling\n", filename);
+      srslte_vec_save_file(filename, q->e[i], cfg->nbits[0].nof_bits*sizeof(int16_t));
+    }
   }
 }
 
@@ -427,15 +463,17 @@ int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_g
 int srslte_pdsch_cfg_mimo(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_grant_t *grant, uint32_t cfi,
                            uint32_t sf_idx, int rvidx[SRSLTE_MAX_CODEWORDS], srslte_mimo_type_t mimo_type,
                            uint32_t pmi) {
-  if (cfg) {
-    if (grant) {
-      memcpy(&cfg->grant, grant, sizeof(srslte_ra_dl_grant_t));
-    }
+  if (cfg && grant) {
+    uint32_t nof_tb = SRSLTE_RA_DL_GRANT_NOF_TB(grant);
+    memcpy(&cfg->grant, grant, sizeof(srslte_ra_dl_grant_t));
 
-    for (int i = 0; i < grant->nof_tb; i++) {
-      if (srslte_cbsegm(&cfg->cb_segm[i], (uint32_t) cfg->grant.mcs[i].tbs)) {
-        fprintf(stderr, "Error computing Codeblock (1) segmentation for TBS=%d\n", cfg->grant.mcs[i].tbs);
-        return SRSLTE_ERROR;
+
+    for (int cw = 0; cw < SRSLTE_MAX_CODEWORDS; cw++) {
+      if (grant->tb_en[cw]) {
+        if (srslte_cbsegm(&cfg->cb_segm[cw], (uint32_t) cfg->grant.mcs[cw].tbs)) {
+          fprintf(stderr, "Error computing Codeblock (1) segmentation for TBS=%d\n", cfg->grant.mcs[cw].tbs);
+          return SRSLTE_ERROR;
+        }
       }
     }
     srslte_ra_dl_grant_to_nbits(&cfg->grant, cfi, cell, sf_idx, cfg->nbits);
@@ -447,33 +485,36 @@ int srslte_pdsch_cfg_mimo(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra
     /* Check and configure PDSCH transmission modes */
     switch(mimo_type) {
       case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
-        if (grant->nof_tb != 1) {
-          ERROR("Number of transport blocks is not supported for single transmission mode.");
+        if (nof_tb != 1) {
+          ERROR("Wrong number of transport blocks (%d) for single antenna.", nof_tb);
           return SRSLTE_ERROR;
         }
         cfg->nof_layers = 1;
         break;
       case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
-        if (grant->nof_tb != 1) {
-          ERROR("Number of transport blocks is not supported for transmit diversity mode.");
+        if (nof_tb != 1) {
+          ERROR("Wrong number of transport blocks (%d) for transmit diversity.", nof_tb);
           return SRSLTE_ERROR;
         }
         cfg->nof_layers = 2;
         break;
       case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
-        if (grant->nof_tb == 1) {
+        if (nof_tb == 1) {
           cfg->codebook_idx = pmi;
           cfg->nof_layers = 1;
-        } else {
+        } else if (nof_tb == 2) {
           cfg->codebook_idx = pmi + 1;
           cfg->nof_layers = 2;
+        } else {
+          ERROR("Wrong number of transport blocks (%d) for spatial multiplexing.", nof_tb);
+          return SRSLTE_ERROR;
         }
         INFO("PDSCH configured for Spatial Multiplex; nof_codewords=%d; nof_layers=%d; codebook_idx=%d;\n",
-             grant->nof_tb, cfg->nof_layers, cfg->codebook_idx);
+             nof_tb, cfg->nof_layers, cfg->codebook_idx);
         break;
       case SRSLTE_MIMO_TYPE_CDD:
-        if (grant->nof_tb != 2) {
-          ERROR("Number of transport blocks (%d) is not supported for CDD transmission mode.", grant->nof_tb);
+        if (nof_tb != 2) {
+          ERROR("Wrong number of transport blocks (%d) for CDD.", nof_tb);
           return SRSLTE_ERROR;
         }
         cfg->nof_layers = 2;
@@ -486,7 +527,25 @@ int srslte_pdsch_cfg_mimo(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra
   }
 }
 
-static int srslte_pdsch_codeword_encode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_t *cfg,
+static srslte_sequence_t *get_user_sequence(srslte_pdsch_t *q, uint16_t rnti,
+                                            uint32_t codeword_idx, uint32_t sf_idx, uint32_t len)
+{
+  uint32_t rnti_idx = q->is_ue?0:rnti;
+
+  // The scrambling sequence is pregenerated for all RNTIs in the eNodeB but only for C-RNTI in the UE
+  if (q->users[rnti_idx] && q->users[rnti_idx]->sequence_generated &&
+      q->users[rnti_idx]->cell_id == q->cell.id                    &&
+      q->ue_rnti == rnti                                           &&
+      ((rnti >= SRSLTE_CRNTI_START && rnti < SRSLTE_CRNTI_END) || !q->is_ue))
+  {
+    return &q->users[rnti_idx]->seq[codeword_idx][sf_idx];
+  } else {
+    srslte_sequence_pdsch(&q->tmp_seq, rnti, codeword_idx, 2 * sf_idx, q->cell.id, len);
+    return &q->tmp_seq;
+  }
+}
+
+static int srslte_pdsch_codeword_encode(srslte_pdsch_t *q, srslte_pdsch_cfg_t *cfg,
                                                srslte_softbuffer_tx_t *softbuffer, uint16_t rnti, uint8_t *data,
                                                uint32_t codeword_idx) {
   srslte_ra_nbits_t *nbits = &cfg->nbits[codeword_idx];
@@ -499,45 +558,36 @@ static int srslte_pdsch_codeword_encode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_
          nbits->nof_re, nbits->nof_bits, rv);
 
     /* Channel coding */
-    if (srslte_dlsch_encode2(&pdsch->dl_sch, cfg, softbuffer, data, pdsch->e[codeword_idx], codeword_idx)) {
+    if (srslte_dlsch_encode2(&q->dl_sch, cfg, softbuffer, data, q->e[codeword_idx], codeword_idx)) {
       ERROR("Error encoding TB %d", codeword_idx);
       return SRSLTE_ERROR;
     }
 
+    /* Select scrambling sequence */
+    srslte_sequence_t *seq = get_user_sequence(q, rnti, codeword_idx, cfg->sf_idx, nbits->nof_bits);
+
     /* Bit scrambling */
-    if (!pdsch->users[rnti]) {
-      srslte_sequence_t seq;
-
-      if (srslte_sequence_pdsch(&seq, rnti, codeword_idx, 2 * cfg->sf_idx, pdsch->cell.id, nbits->nof_bits)) {
-        ERROR("Initialising scrambling sequence");
-        return SRSLTE_ERROR;
-      }
-      srslte_scrambling_bytes(&seq, (uint8_t *) pdsch->e[codeword_idx], nbits->nof_bits);
-      srslte_sequence_free(&seq);
-
-    } else {
-      srslte_scrambling_bytes(&pdsch->users[rnti]->seq[codeword_idx][cfg->sf_idx],
-                              (uint8_t *) pdsch->e[codeword_idx],
-                              nbits->nof_bits);
-    }
+    srslte_scrambling_bytes(seq, (uint8_t *) q->e[codeword_idx], nbits->nof_bits);
 
     /* Bit mapping */
-    srslte_mod_modulate_bytes(&pdsch->mod[mcs->mod],
-                              (uint8_t *) pdsch->e[codeword_idx],
-                              pdsch->d[codeword_idx], nbits->nof_bits);
+    srslte_mod_modulate_bytes(&q->mod[mcs->mod],
+                              (uint8_t *) q->e[codeword_idx],
+                              q->d[codeword_idx], nbits->nof_bits);
+
   }
 
   return SRSLTE_SUCCESS;
 }
 
-static int srslte_pdsch_codeword_decode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_t *cfg,
+static int srslte_pdsch_codeword_decode(srslte_pdsch_t *q, srslte_pdsch_cfg_t *cfg,
                                                srslte_softbuffer_rx_t *softbuffer, uint16_t rnti, uint8_t *data,
-                                               uint32_t codeword_idx) {
+                                               uint32_t codeword_idx, bool *ack) {
   srslte_ra_nbits_t *nbits = &cfg->nbits[codeword_idx];
   srslte_ra_mcs_t *mcs = &cfg->grant.mcs[codeword_idx];
   uint32_t rv = cfg->rv[codeword_idx];
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
-  if (nbits->nof_bits) {
+  if (softbuffer && data && ack) {
     INFO("Decoding PDSCH SF: %d (TB %d), Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
          cfg->sf_idx, codeword_idx, srslte_mod_string(mcs->mod), mcs->tbs,
          nbits->nof_re, nbits->nof_bits, rv);
@@ -546,25 +596,27 @@ static int srslte_pdsch_codeword_decode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_
      * The MAX-log-MAP algorithm used in turbo decoding is unsensitive to SNR estimation,
      * thus we don't need tot set it in the LLRs normalization
      */
-    srslte_demod_soft_demodulate_s(mcs->mod, pdsch->d[codeword_idx], pdsch->e[codeword_idx], nbits->nof_re);
+    srslte_demod_soft_demodulate_s(mcs->mod, q->d[codeword_idx], q->e[codeword_idx], nbits->nof_re);
 
-    if (pdsch->users[rnti] && pdsch->users[rnti]->sequence_generated) {
-      srslte_scrambling_s_offset(&pdsch->users[rnti]->seq[codeword_idx][cfg->sf_idx], pdsch->e[codeword_idx],
-                                 0, nbits->nof_bits);
-    } else {
-      srslte_sequence_t seq;
-      if (srslte_sequence_pdsch(&seq, rnti, codeword_idx, 2 * cfg->sf_idx, pdsch->cell.id, nbits->nof_bits)) {
-        ERROR("Initialising scrambling sequence");
-        return SRSLTE_ERROR;
-      }
-      srslte_scrambling_s_offset(&seq, pdsch->e[codeword_idx], 0, nbits->nof_bits);
-      srslte_sequence_free(&seq);
+    /* Select scrambling sequence */
+    srslte_sequence_t *seq = get_user_sequence(q, rnti, codeword_idx, cfg->sf_idx, nbits->nof_bits);
+
+    /* Bit scrambling */
+    srslte_scrambling_s_offset(seq, q->e[codeword_idx], 0, nbits->nof_bits);
+
+    /* Return  */
+    ret = srslte_dlsch_decode2(&q->dl_sch, cfg, softbuffer, q->e[codeword_idx], data, codeword_idx);
+    if (ret == SRSLTE_SUCCESS) {
+      *ack = true;
+    } else if (ret == SRSLTE_ERROR) {
+      *ack = false;
+      ret = SRSLTE_SUCCESS;
     }
-
-    return srslte_dlsch_decode2(&pdsch->dl_sch, cfg, softbuffer, pdsch->e[codeword_idx], data, codeword_idx);
+  } else {
+    ERROR("Detected NULL pointer");
   }
 
-  return SRSLTE_SUCCESS;
+  return ret;
 }
 
 /** Decodes the PDSCH from the received symbols
@@ -585,9 +637,10 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
       data         != NULL &&
       cfg          != NULL)
   {
+    uint32_t nof_tb = SRSLTE_RA_DL_GRANT_NOF_TB(&cfg->grant);
 
     INFO("Decoding PDSCH SF: %d, RNTI: 0x%x, NofSymbols: %d, C_prb=%d, mimo_type=%d, nof_layers=%d, nof_tb=%d\n",
-        cfg->sf_idx, rnti, cfg->nbits[0].nof_re, cfg->grant.nof_prb, cfg->nof_layers, cfg->grant.nof_tb);
+        cfg->sf_idx, rnti, cfg->nbits[0].nof_re, cfg->grant.nof_prb, cfg->nof_layers, nof_tb);
 
     // Extract Symbols and Channel Estimates
     for (int j=0;j<q->nof_rx_antennas;j++) {
@@ -608,10 +661,10 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
 
     // Prepare layers
     int nof_symbols [SRSLTE_MAX_CODEWORDS];
-    nof_symbols[0] = cfg->nbits[0].nof_re * cfg->grant.nof_tb / cfg->nof_layers;
-    nof_symbols[1] = cfg->nbits[1].nof_re * cfg->grant.nof_tb / cfg->nof_layers;
+    nof_symbols[0] = cfg->nbits[0].nof_re * nof_tb / cfg->nof_layers;
+    nof_symbols[1] = cfg->nbits[1].nof_re * nof_tb / cfg->nof_layers;
 
-    if (cfg->nof_layers == cfg->grant.nof_tb) {
+    if (cfg->nof_layers == nof_tb) {
       /* Skip layer demap */
       for (i = 0; i < cfg->nof_layers; i++) {
         x[i] = q->d[i];
@@ -629,21 +682,26 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
                                     cfg->codebook_idx, cfg->nbits[0].nof_re, cfg->mimo_type, noise_estimate);
 
     // Layer demapping only if necessary
-    if (cfg->nof_layers != cfg->grant.nof_tb) {
-        srslte_layerdemap_type(x, q->d, cfg->nof_layers, cfg->grant.nof_tb,
+    if (cfg->nof_layers != nof_tb) {
+        srslte_layerdemap_type(x, q->d, cfg->nof_layers, nof_tb,
                              nof_symbols[0], nof_symbols, cfg->mimo_type);
     }
 
     // Codeword decoding
-    for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
-      int ret = srslte_pdsch_codeword_decode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
-      acks[tb] = (ret == SRSLTE_SUCCESS);
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb ++) {
+      if (cfg->grant.tb_en[tb]) {
+        int ret = srslte_pdsch_codeword_decode(q, cfg, softbuffers[tb], rnti, data[tb], tb, &acks[tb]);
+
+        /* Check if there has been any execution error */
+        if (ret) {
+          return ret;
+        }
+      }
     }
 
     pdsch_decode_debug(q, cfg, sf_symbols, ce);
 
     return SRSLTE_SUCCESS;
-
   } else {
     return SRSLTE_ERROR_INVALID_INPUTS;
   }
@@ -689,6 +747,8 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
 
   if (q != NULL &&
       cfg != NULL) {
+    uint32_t nof_tb = SRSLTE_RA_DL_GRANT_NOF_TB(&cfg->grant);
+
 
     for (i = 0; i < q->cell.nof_ports; i++) {
       if (sf_symbols[i] == NULL) {
@@ -708,15 +768,17 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
       return SRSLTE_ERROR_INVALID_INPUTS;
     }
 
-    for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
-      ret |= srslte_pdsch_codeword_encode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb ++) {
+      if (cfg->grant.tb_en[tb]) {
+        ret |= srslte_pdsch_codeword_encode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
+      }
     }
 
     // Layer mapping & precode if necessary
     if (q->cell.nof_ports > 1) {
       int nof_symbols;
       /* If number of layers is equal to transport blocks (codewords) skip layer mapping */
-      if (cfg->nof_layers == cfg->grant.nof_tb) {
+      if (cfg->nof_layers == nof_tb) {
         for (i = 0; i < cfg->nof_layers; i++) {
           x[i] = q->d[i];
         }
@@ -728,7 +790,7 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
         }
         memset(&x[cfg->nof_layers], 0, sizeof(cf_t *) * (SRSLTE_MAX_LAYERS - cfg->nof_layers));
 
-        nof_symbols = srslte_layermap_type(q->d, x, cfg->grant.nof_tb, cfg->nof_layers,
+        nof_symbols = srslte_layermap_type(q->d, x, nof_tb, cfg->nof_layers,
                                            (int[SRSLTE_MAX_CODEWORDS]) {cfg->nbits[0].nof_re, cfg->nbits[1].nof_re},
                                            cfg->mimo_type);
       }
diff --git a/lib/src/phy/phch/phich.c b/lib/src/phy/phch/phich.c
index 7de2f5d37..a7cb282fd 100644
--- a/lib/src/phy/phch/phich.c
+++ b/lib/src/phy/phch/phich.c
@@ -32,6 +32,7 @@
 #include <stdbool.h>
 #include <assert.h>
 #include <math.h>
+#include <srslte/phy/common/phy_common.h>
 
 #include "srslte/phy/phch/regs.h"
 #include "srslte/phy/phch/phich.h"
@@ -67,37 +68,22 @@ void srslte_phich_reset(srslte_phich_t *q, cf_t *slot_symbols[SRSLTE_MAX_PORTS])
   }
 }
 
-int srslte_phich_init(srslte_phich_t *q, srslte_regs_t *regs, srslte_cell_t cell) 
-{
-  return srslte_phich_init_multi(q, regs, cell, 1);
-}
-
 /** Initializes the phich channel receiver */
-int srslte_phich_init_multi(srslte_phich_t *q, srslte_regs_t *regs, srslte_cell_t cell, uint32_t nof_rx_antennas) 
+int srslte_phich_init(srslte_phich_t *q, uint32_t nof_rx_antennas)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   
-  if (q         != NULL &&
-      regs      != NULL &&
-      srslte_cell_isvalid(&cell)) 
+  if (q != NULL)
   {
 
     bzero(q, sizeof(srslte_phich_t));
     ret = SRSLTE_ERROR;
     
-    q->cell = cell;
-    q->regs = regs;
-    q->nof_rx_antennas = nof_rx_antennas; 
+    q->nof_rx_antennas = nof_rx_antennas;
     
     if (srslte_modem_table_lte(&q->mod, SRSLTE_MOD_BPSK)) {
       goto clean;
     }
-
-    for (int nsf = 0; nsf < SRSLTE_NSUBFRAMES_X_FRAME; nsf++) {
-      if (srslte_sequence_phich(&q->seq[nsf], 2 * nsf, q->cell.id)) {
-        goto clean;
-      }
-    }
     ret = SRSLTE_SUCCESS;
   }
   clean: 
@@ -114,9 +100,34 @@ void srslte_phich_free(srslte_phich_t *q) {
   srslte_modem_table_free(&q->mod);
 
   bzero(q, sizeof(srslte_phich_t));
-
 }
 
+int srslte_phich_set_cell(srslte_phich_t *q, srslte_regs_t *regs, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q         != NULL &&
+      regs      != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+
+    q->regs = regs;
+
+    if (cell.id != q->cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+      for (int nsf = 0; nsf < SRSLTE_NSUBFRAMES_X_FRAME; nsf++) {
+        if (srslte_sequence_phich(&q->seq[nsf], 2 * nsf, q->cell.id)) {
+          return SRSLTE_ERROR;
+        }
+      }
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
+
+
 /* Computes n_group and n_seq according to Section 9.1.2 in 36.213 */
 void srslte_phich_calc(srslte_phich_t *q, uint32_t n_prb_lowest, uint32_t n_dmrs, 
                        uint32_t *ngroup, uint32_t *nseq) 
diff --git a/lib/src/phy/phch/prach.c b/lib/src/phy/phch/prach.c
index 5a2169fd9..ffd8b51e7 100644
--- a/lib/src/phy/phch/prach.c
+++ b/lib/src/phy/phch/prach.c
@@ -26,6 +26,7 @@
 
 #include <math.h>
 #include <string.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/common/phy_common.h"
 #include "srslte/phy/phch/prach.h"
@@ -61,6 +62,8 @@ uint32_t prach_Tseq[5] = {24576, 24576, 2*24576, 2*24576, 4096};
 // Table 5.7.2-2 - N_cs values for unrestricted sets
 uint32_t prach_Ncs_unrestricted[16] = {0,13,15,18,22,26,32,38,46,59,76,93,119,167,279,419};
 
+#define MAX_N_zc 839
+
 // Table 5.7.2-2 - N_cs values for restricted sets
 uint32_t prach_Ncs_restricted[15] = {15,18,22,26,32,38,46,55,68,82,100,128,158,202,237};
 
@@ -328,7 +331,10 @@ int srslte_prach_gen_seqs(srslte_prach_t *p)
 
 int srslte_prach_init_cfg(srslte_prach_t *p, srslte_prach_cfg_t *cfg, uint32_t nof_prb)
 {
-  return srslte_prach_init(p, 
+  if (srslte_prach_init(p, srslte_symbol_sz(nof_prb))) {
+    return -1;
+  }
+  return srslte_prach_set_cell(p,
                            srslte_symbol_sz(nof_prb), 
                            cfg->config_idx, 
                            cfg->root_seq_idx,
@@ -336,28 +342,95 @@ int srslte_prach_init_cfg(srslte_prach_t *p, srslte_prach_cfg_t *cfg, uint32_t n
                            cfg->zero_corr_zone);
 }
 
-int srslte_prach_init(srslte_prach_t *p,
-               uint32_t N_ifft_ul,
-               uint32_t config_idx,
-               uint32_t root_seq_index,
-               bool high_speed_flag,
-               uint32_t zero_corr_zone_config)
+int srslte_prach_init(srslte_prach_t *p, uint32_t max_N_ifft_ul)
+{
+  int ret = SRSLTE_ERROR;
+  if(p                      != NULL      &&
+     max_N_ifft_ul          <  2049)
+  {
+    bzero(p, sizeof(srslte_prach_t));
+
+    p->max_N_ifft_ul = max_N_ifft_ul;
+
+    // Set up containers
+    p->prach_bins = srslte_vec_malloc(sizeof(cf_t)*MAX_N_zc);
+    p->corr_spec = srslte_vec_malloc(sizeof(cf_t)*MAX_N_zc);
+    p->corr = srslte_vec_malloc(sizeof(float)*MAX_N_zc);
+
+    // Set up ZC FFTS
+    if(srslte_dft_plan(&p->zc_fft, MAX_N_zc, SRSLTE_DFT_FORWARD, SRSLTE_DFT_COMPLEX)){
+      return SRSLTE_ERROR;
+    }
+    srslte_dft_plan_set_mirror(&p->zc_fft, false);
+    srslte_dft_plan_set_norm(&p->zc_fft, true);
+
+    if(srslte_dft_plan(&p->zc_ifft, MAX_N_zc, SRSLTE_DFT_BACKWARD, SRSLTE_DFT_COMPLEX)){
+      return SRSLTE_ERROR;
+    }
+    srslte_dft_plan_set_mirror(&p->zc_ifft, false);
+    srslte_dft_plan_set_norm(&p->zc_ifft, false);
+
+    uint32_t fft_size_alloc = max_N_ifft_ul * DELTA_F/DELTA_F_RA;
+
+    p->ifft_in = (cf_t*)srslte_vec_malloc(fft_size_alloc*sizeof(cf_t));
+    p->ifft_out = (cf_t*)srslte_vec_malloc(fft_size_alloc*sizeof(cf_t));
+    if(srslte_dft_plan(&p->ifft, fft_size_alloc, SRSLTE_DFT_BACKWARD, SRSLTE_DFT_COMPLEX)) {
+      fprintf(stderr, "Error creating DFT plan\n");
+      return -1;
+    }
+    srslte_dft_plan_set_mirror(&p->ifft, true);
+    srslte_dft_plan_set_norm(&p->ifft, true);
+
+    if(srslte_dft_plan(&p->fft, fft_size_alloc, SRSLTE_DFT_FORWARD, SRSLTE_DFT_COMPLEX)){
+      fprintf(stderr, "Error creating DFT plan\n");
+      return -1;
+    }
+    
+    p->signal_fft = srslte_vec_malloc(sizeof(cf_t)*fft_size_alloc);
+    if (!p->signal_fft) {
+      fprintf(stderr, "Error allocating memory\n");
+      return -1; 
+    }
+    
+    srslte_dft_plan_set_mirror(&p->fft, true);
+    srslte_dft_plan_set_norm(&p->fft, false);
+
+    ret = SRSLTE_SUCCESS;
+  } else {
+    fprintf(stderr, "Invalid parameters\n");
+  }
+
+  return ret;
+}
+
+
+int srslte_prach_set_cell(srslte_prach_t *p,
+                      uint32_t N_ifft_ul,
+                      uint32_t config_idx,
+                      uint32_t root_seq_index,
+                      bool high_speed_flag,
+                      uint32_t zero_corr_zone_config)
 {
   int ret = SRSLTE_ERROR;
   if(p                      != NULL      &&
      N_ifft_ul              <  2049      &&
-     config_idx             <  64        && 
+     config_idx             <  64        &&
      root_seq_index         <  MAX_ROOTS)
   {
+    if (N_ifft_ul > p->max_N_ifft_ul) {
+      fprintf(stderr, "PRACH: Error in set_cell(): N_ifft_ul must be lower or equal max_N_ifft_ul in init()\n");
+      return -1;
+    }
+
     uint32_t preamble_format = srslte_prach_get_preamble_format(config_idx);
-    p->config_idx = config_idx; 
+    p->config_idx = config_idx;
     p->f = preamble_format;
     p->rsi = root_seq_index;
     p->hs = high_speed_flag;
     p->zczc = zero_corr_zone_config;
-    p->detect_factor = PRACH_DETECT_FACTOR; 
-    
-    
+    p->detect_factor = PRACH_DETECT_FACTOR;
+
+
     // Determine N_zc and N_cs
     if(4 == preamble_format){
       if (p->zczc < 7) {
@@ -368,43 +441,33 @@ int srslte_prach_init(srslte_prach_t *p,
         return SRSLTE_ERROR;
       }
     }else{
-      p->N_zc = 839;
+      p->N_zc = MAX_N_zc;
       if(p->hs){
         if (p->zczc < 15) {
           p->N_cs = prach_Ncs_restricted[p->zczc];
         } else {
           fprintf(stderr, "Invalid zeroCorrelationZoneConfig=%d for restricted set\n", p->zczc);
           return SRSLTE_ERROR;
-        }   
+        }
       }else{
         if (p->zczc < 16) {
           p->N_cs = prach_Ncs_unrestricted[p->zczc];
         } else {
           fprintf(stderr, "Invalid zeroCorrelationZoneConfig=%d\n", p->zczc);
           return SRSLTE_ERROR;
-        }   
+        }
       }
     }
-    
-    // Set up containers
-    p->prach_bins = srslte_vec_malloc(sizeof(cf_t)*p->N_zc);
-    p->corr_spec = srslte_vec_malloc(sizeof(cf_t)*p->N_zc);
-    p->corr = srslte_vec_malloc(sizeof(float)*p->N_zc);
 
     // Set up ZC FFTS
-    p->zc_fft = (srslte_dft_plan_t*)srslte_vec_malloc(sizeof(srslte_dft_plan_t));
-    if(srslte_dft_plan(p->zc_fft, p->N_zc, SRSLTE_DFT_FORWARD, SRSLTE_DFT_COMPLEX)){
-      return SRSLTE_ERROR;
+    if (p->N_zc != MAX_N_zc) {
+      if(srslte_dft_replan(&p->zc_fft, p->N_zc)){
+        return SRSLTE_ERROR;
+      }
+      if(srslte_dft_replan(&p->zc_ifft, p->N_zc)){
+        return SRSLTE_ERROR;
+      }
     }
-    srslte_dft_plan_set_mirror(p->zc_fft, false);
-    srslte_dft_plan_set_norm(p->zc_fft, true);
-
-    p->zc_ifft = (srslte_dft_plan_t*)srslte_vec_malloc(sizeof(srslte_dft_plan_t));
-    if(srslte_dft_plan(p->zc_ifft, p->N_zc, SRSLTE_DFT_BACKWARD, SRSLTE_DFT_COMPLEX)){
-      return SRSLTE_ERROR;
-    }
-    srslte_dft_plan_set_mirror(p->zc_ifft, false);
-    srslte_dft_plan_set_norm(p->zc_ifft, false);
 
     // Generate our 64 sequences
     p->N_roots = 0;
@@ -412,9 +475,9 @@ int srslte_prach_init(srslte_prach_t *p,
 
     // Generate sequence FFTs
     for(int i=0;i<N_SEQS;i++){
-      srslte_dft_run(p->zc_fft, p->seqs[i], p->dft_seqs[i]);
+      srslte_dft_run(&p->zc_fft, p->seqs[i], p->dft_seqs[i]);
     }
-    
+
     // Create our FFT objects and buffers
     p->N_ifft_ul = N_ifft_ul;
     if(4 == preamble_format){
@@ -422,47 +485,31 @@ int srslte_prach_init(srslte_prach_t *p,
     }else{
       p->N_ifft_prach = p->N_ifft_ul * DELTA_F/DELTA_F_RA;
     }
-    
+
     /* The deadzone specifies the number of samples at the end of the correlation window
      * that will be considered as belonging to the next preamble
      */
-    p->deadzone = 0; 
+    p->deadzone = 0;
     /*
     if(p->N_cs != 0) {
       float samp_rate=15000*p->N_ifft_ul;
       p->deadzone = (uint32_t) ceil((float) samp_rate/((float) p->N_zc*subcarrier_spacing));
     }*/
 
-    p->ifft_in = (cf_t*)srslte_vec_malloc(p->N_ifft_prach*sizeof(cf_t));
-    p->ifft_out = (cf_t*)srslte_vec_malloc(p->N_ifft_prach*sizeof(cf_t));
-    p->ifft = (srslte_dft_plan_t*)srslte_vec_malloc(sizeof(srslte_dft_plan_t));
-    if(srslte_dft_plan(p->ifft, p->N_ifft_prach, SRSLTE_DFT_BACKWARD, SRSLTE_DFT_COMPLEX)) {
+    if(srslte_dft_replan(&p->ifft, p->N_ifft_prach)) {
       fprintf(stderr, "Error creating DFT plan\n");
       return -1;
     }
-    srslte_dft_plan_set_mirror(p->ifft, true);
-    srslte_dft_plan_set_norm(p->ifft, true);
-
-    p->fft = (srslte_dft_plan_t*)srslte_vec_malloc(sizeof(srslte_dft_plan_t));
-    if(srslte_dft_plan(p->fft, p->N_ifft_prach, SRSLTE_DFT_FORWARD, SRSLTE_DFT_COMPLEX)){
+    if(srslte_dft_replan(&p->fft, p->N_ifft_prach)){
       fprintf(stderr, "Error creating DFT plan\n");
       return -1;
     }
-    
-    p->signal_fft = srslte_vec_malloc(sizeof(cf_t)*p->N_ifft_prach);
-    if (!p->signal_fft) {
-      fprintf(stderr, "Error allocating memory\n");
-      return -1; 
-    }
-    
-    srslte_dft_plan_set_mirror(p->fft, true);
-    srslte_dft_plan_set_norm(p->fft, false);
 
     p->N_seq = prach_Tseq[p->f]*p->N_ifft_ul/2048;
     p->N_cp  = prach_Tcp[p->f]*p->N_ifft_ul/2048;
     p->T_seq = prach_Tseq[p->f]*SRSLTE_LTE_TS;
     p->T_tot = (prach_Tseq[p->f]+prach_Tcp[p->f])*SRSLTE_LTE_TS;
-    
+
     ret = SRSLTE_SUCCESS;
   } else {
     fprintf(stderr, "Invalid parameters\n");
@@ -500,7 +547,7 @@ int srslte_prach_gen(srslte_prach_t *p,
     memcpy(&p->ifft_in[begin], p->dft_seqs[seq_index], p->N_zc * sizeof(cf_t));
     memset(&p->ifft_in[begin+p->N_zc], 0, (p->N_ifft_prach - begin - p->N_zc) * sizeof(cf_t));
 
-    srslte_dft_run(p->ifft, p->ifft_in, p->ifft_out);
+    srslte_dft_run(&p->ifft, p->ifft_in, p->ifft_out);
 
     // Copy CP into buffer
     memcpy(signal, &p->ifft_out[p->N_ifft_prach-p->N_cp], p->N_cp*sizeof(cf_t));
@@ -552,7 +599,7 @@ int srslte_prach_detect_offset(srslte_prach_t *p,
     }
     
     // FFT incoming signal
-    srslte_dft_run(p->fft, signal, p->signal_fft);
+    srslte_dft_run(&p->fft, signal, p->signal_fft);
 
     *n_indices = 0;
 
@@ -569,7 +616,7 @@ int srslte_prach_detect_offset(srslte_prach_t *p,
      
       srslte_vec_prod_conj_ccc(p->prach_bins, root_spec, p->corr_spec, p->N_zc);
 
-      srslte_dft_run(p->zc_ifft, p->corr_spec, p->corr_spec);
+      srslte_dft_run(&p->zc_ifft, p->corr_spec, p->corr_spec);
       
       srslte_vec_abs_square_cf(p->corr_spec, p->corr, p->N_zc);
 
@@ -632,16 +679,12 @@ int srslte_prach_free(srslte_prach_t *p) {
   free(p->prach_bins);
   free(p->corr_spec);
   free(p->corr);
-  srslte_dft_plan_free(p->ifft);
-  free(p->ifft);
+  srslte_dft_plan_free(&p->ifft);
   free(p->ifft_in);
   free(p->ifft_out);
-  srslte_dft_plan_free(p->fft);
-  free(p->fft);
-  srslte_dft_plan_free(p->zc_fft);
-  free(p->zc_fft);
-  srslte_dft_plan_free(p->zc_ifft);
-  free(p->zc_ifft);
+  srslte_dft_plan_free(&p->fft);
+  srslte_dft_plan_free(&p->zc_fft);
+  srslte_dft_plan_free(&p->zc_ifft);
 
   if (p->signal_fft) {
     free(p->signal_fft); 
diff --git a/lib/src/phy/phch/pucch.c b/lib/src/phy/phch/pucch.c
index 56349dee4..498d34f3a 100644
--- a/lib/src/phy/phch/pucch.c
+++ b/lib/src/phy/phch/pucch.c
@@ -416,37 +416,24 @@ void srslte_pucch_set_threshold(srslte_pucch_t *q, float format1_threshold) {
 }
 
 /** Initializes the PDCCH transmitter and receiver */
-int srslte_pucch_init(srslte_pucch_t *q, srslte_cell_t cell) {
+int srslte_pucch_init(srslte_pucch_t *q) {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
-  if (q != NULL && srslte_cell_isvalid(&cell)) {
+  if (q != NULL) {
     ret = SRSLTE_ERROR;
     bzero(q, sizeof(srslte_pucch_t));
     
-    q->cell = cell; 
-    
-    srslte_pucch_cfg_default(&q->pucch_cfg);
-    
     if (srslte_modem_table_lte(&q->mod, SRSLTE_MOD_QPSK)) {
       return SRSLTE_ERROR;
     }
-    
-    // Precompute group hopping values u. 
-    if (srslte_group_hopping_f_gh(q->f_gh, q->cell.id)) {
-      return SRSLTE_ERROR;
-    }
-    
-    if (srslte_pucch_n_cs_cell(q->cell, q->n_cs_cell)) {
-      return SRSLTE_ERROR;
-    }
-    
+
     q->users = calloc(sizeof(srslte_pucch_user_t*), 1+SRSLTE_SIRNTI);
     if (!q->users) {
       perror("malloc");
-      return SRSLTE_ERROR;
+      goto clean_exit;
     }
     
     srslte_uci_cqi_pucch_init(&q->cqi);
-    
+
     q->z = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_PUCCH_MAX_SYMBOLS);
     q->z_tmp = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_PUCCH_MAX_SYMBOLS);
     q->ce = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_PUCCH_MAX_SYMBOLS);
@@ -455,12 +442,16 @@ int srslte_pucch_init(srslte_pucch_t *q, srslte_cell_t cell) {
 
     ret = SRSLTE_SUCCESS;
   }
+clean_exit:
+  if (ret == SRSLTE_ERROR) {
+    srslte_pucch_free(q);
+  }
   return ret;
 }
 
 void srslte_pucch_free(srslte_pucch_t *q) {
   if (q->users) {
-    for (int rnti=0;rnti<SRSLTE_SIRNTI;rnti++) {      
+    for (int rnti=0;rnti<=SRSLTE_SIRNTI;rnti++) {
       srslte_pucch_clear_rnti(q, rnti);
     }
     free(q->users);
@@ -480,6 +471,31 @@ void srslte_pucch_free(srslte_pucch_t *q) {
   bzero(q, sizeof(srslte_pucch_t));
 }
 
+int srslte_pucch_set_cell(srslte_pucch_t *q, srslte_cell_t cell) {
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+  if (q != NULL && srslte_cell_isvalid(&cell)) {
+
+    srslte_pucch_cfg_default(&q->pucch_cfg);
+
+    if (cell.id != q->cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+
+      // Precompute group hopping values u.
+      if (srslte_group_hopping_f_gh(q->f_gh, q->cell.id)) {
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pucch_n_cs_cell(q->cell, q->n_cs_cell)) {
+        return SRSLTE_ERROR;
+      }
+    }
+
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
+
 void srslte_pucch_clear_rnti(srslte_pucch_t *q, uint16_t rnti) {
   if (q->users[rnti]) {
     for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
@@ -498,6 +514,7 @@ int srslte_pucch_set_crnti(srslte_pucch_t *q, uint16_t rnti) {
         // Precompute scrambling sequence for pucch format 2    
         if (srslte_sequence_pucch(&q->users[rnti]->seq_f2[sf_idx], rnti, 2*sf_idx, q->cell.id)) {
           fprintf(stderr, "Error computing PUCCH Format 2 scrambling sequence\n");
+          srslte_pucch_clear_rnti(q, rnti);
           return SRSLTE_ERROR; 
         }        
       }
diff --git a/lib/src/phy/phch/pusch.c b/lib/src/phy/phch/pusch.c
index 9e0df307b..f52bbbb42 100644
--- a/lib/src/phy/phch/pusch.c
+++ b/lib/src/phy/phch/pusch.c
@@ -32,6 +32,8 @@
 #include <stdbool.h>
 #include <assert.h>
 #include <math.h>
+#include <srslte/srslte.h>
+#include <srslte/phy/phch/pusch.h>
 
 #include "srslte/phy/ch_estimation/refsignal_ul.h"
 #include "srslte/phy/phch/pusch.h"
@@ -185,22 +187,18 @@ int pusch_get(srslte_pusch_t *q, srslte_ra_ul_grant_t *grant, cf_t *input, cf_t
 
 
 /** Initializes the PDCCH transmitter and receiver */
-int srslte_pusch_init(srslte_pusch_t *q, srslte_cell_t cell) {
+int pusch_init(srslte_pusch_t *q, uint32_t max_prb, bool is_ue) {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   int i;
 
- if (q                         != NULL                  &&
-     srslte_cell_isvalid(&cell)) 
+ if (q != NULL)
   {   
     
     bzero(q, sizeof(srslte_pusch_t));
     ret = SRSLTE_ERROR;
-    
-    q->cell = cell;
-    q->max_re = q->cell.nof_prb * MAX_PUSCH_RE(q->cell.cp);
+    q->max_re = max_prb * MAX_PUSCH_RE(SRSLTE_CP_NORM);
 
-    INFO("Init PUSCH: %d ports %d PRBs, max_symbols: %d\n", q->cell.nof_ports,
-        q->cell.nof_prb, q->max_re);
+    INFO("Init PUSCH: %d PRBs\n", max_prb);
 
     for (i = 0; i < 4; i++) {
       if (srslte_modem_table_lte(&q->mod[i], modulations[i])) {
@@ -208,26 +206,26 @@ int srslte_pusch_init(srslte_pusch_t *q, srslte_cell_t cell) {
       }
       srslte_modem_table_bytes(&q->mod[i]);
     }
-    
-    q->users = calloc(sizeof(srslte_pusch_user_t*), 1+SRSLTE_SIRNTI);
+
+    q->is_ue = is_ue;
+
+    q->users = calloc(sizeof(srslte_pusch_user_t*), q->is_ue?1:(1+SRSLTE_SIRNTI));
     if (!q->users) {
       perror("malloc");
       goto clean;
     }
 
-    /* Precompute sequence for type2 frequency hopping */
-    if (srslte_sequence_LTE_pr(&q->seq_type2_fo, 210, q->cell.id)) {
-      fprintf(stderr, "Error initiating type2 frequency hopping sequence\n");
-      goto clean; 
+    if (srslte_sequence_init(&q->tmp_seq, q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
+      goto clean;
     }
 
     srslte_sch_init(&q->ul_sch);
-    
-    if (srslte_dft_precoding_init(&q->dft_precoding, cell.nof_prb)) {
+
+    if (srslte_dft_precoding_init(&q->dft_precoding, max_prb, is_ue)) {
       fprintf(stderr, "Error initiating DFT transform precoding\n");
       goto clean; 
     }
-    
+
     // Allocate int16 for reception (LLRs). Buffer casted to uint8_t for transmission
     q->q = srslte_vec_malloc(sizeof(int16_t) * q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM));
     if (!q->q) {
@@ -244,9 +242,11 @@ int srslte_pusch_init(srslte_pusch_t *q, srslte_cell_t cell) {
       goto clean; 
     }
 
-    q->ce = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
-    if (!q->ce) {
-      goto clean;
+    if (!q->is_ue) {
+      q->ce = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
+      if (!q->ce) {
+        goto clean;
+      }
     }
     q->z = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
     if (!q->z) {
@@ -262,6 +262,14 @@ int srslte_pusch_init(srslte_pusch_t *q, srslte_cell_t cell) {
   return ret;
 }
 
+int srslte_pusch_init_ue(srslte_pusch_t *q, uint32_t max_prb) {
+  return pusch_init(q, max_prb, true);
+}
+
+int srslte_pusch_init_enb(srslte_pusch_t *q, uint32_t max_prb) {
+  return pusch_init(q, max_prb, false);
+}
+
 void srslte_pusch_free(srslte_pusch_t *q) {
   int i;
 
@@ -284,13 +292,20 @@ void srslte_pusch_free(srslte_pusch_t *q) {
   srslte_dft_precoding_free(&q->dft_precoding);
 
   if (q->users) {
-    for (int rnti=0;rnti<SRSLTE_SIRNTI;rnti++) {      
-      srslte_pusch_clear_rnti(q, rnti);
+    if (q->is_ue) {
+      srslte_pusch_free_rnti(q, 0);
+    } else {
+      for (int rnti=0;rnti<=SRSLTE_SIRNTI;rnti++) {
+        srslte_pusch_free_rnti(q, rnti);
+      }
     }
     free(q->users);
   }
+
   srslte_sequence_free(&q->seq_type2_fo);
   
+  srslte_sequence_free(&q->tmp_seq);
+  
   for (i = 0; i < 4; i++) {
     srslte_modem_table_free(&q->mod[i]);
   }
@@ -300,6 +315,33 @@ void srslte_pusch_free(srslte_pusch_t *q) {
 
 }
 
+int srslte_pusch_set_cell(srslte_pusch_t *q, srslte_cell_t cell) {
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q != NULL && srslte_cell_isvalid(&cell))
+  {
+
+    q->max_re = cell.nof_prb * MAX_PUSCH_RE(q->cell.cp);
+
+    INFO("PUSCH: Cell config PCI=5d, %d ports %d PRBs, max_symbols: %d\n",
+         q->cell.id, q->cell.nof_ports, q->cell.nof_prb, q->max_re);
+
+    if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+      /* Precompute sequence for type2 frequency hopping */
+      if (srslte_sequence_LTE_pr(&q->seq_type2_fo, 210, q->cell.id)) {
+        fprintf(stderr, "Error initiating type2 frequency hopping sequence\n");
+        return SRSLTE_ERROR;
+      }
+
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
+
+
 /* Configures the structure srslte_pusch_cfg_t from the UL DCI allocation dci_msg. 
  * If dci_msg is NULL, the grant is assumed to be already stored in cfg->grant
  */
@@ -391,29 +433,63 @@ int srslte_pusch_cfg(srslte_pusch_t             *q,
  * For the connection procedure, use srslte_pusch_encode() functions */
 int srslte_pusch_set_rnti(srslte_pusch_t *q, uint16_t rnti) {
   uint32_t i;
-  
-  if (!q->users[rnti]) {
-    q->users[rnti] = calloc(1, sizeof(srslte_pusch_user_t));
-    if (q->users[rnti]) {
-      for (i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-        if (srslte_sequence_pusch(&q->users[rnti]->seq[i], rnti, 2 * i, q->cell.id,
-            q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
-          return SRSLTE_ERROR; 
-        }
+
+  uint32_t rnti_idx = q->is_ue?0:rnti;
+
+  if (!q->users[rnti_idx] || q->is_ue) {
+    if (!q->users[rnti_idx]) {
+      q->users[rnti_idx] = calloc(1, sizeof(srslte_pusch_user_t));
+      if (!q->users[rnti_idx]) {
+        perror("calloc");
+        return -1;
       }
-      q->users[rnti]->sequences_generated = true;
     }
+    for (i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
+      if (srslte_sequence_pusch(&q->users[rnti_idx]->seq[i], rnti, 2 * i, q->cell.id,
+                                q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM)))
+      {
+        fprintf(stderr, "Error initializing PUSCH scrambling sequence\n");
+        srslte_pusch_free_rnti(q, rnti);
+        return SRSLTE_ERROR;
+      }
+    }
+    q->ue_rnti = rnti;
+    q->users[rnti_idx]->cell_id = q->cell.id;
+    q->users[rnti_idx]->sequence_generated = true;
+  } else {
+    fprintf(stderr, "Error generating PUSCH sequence: rnti=0x%x already generated\n", rnti);
   }
   return SRSLTE_SUCCESS;
 }
 
-void srslte_pusch_clear_rnti(srslte_pusch_t *q, uint16_t rnti) {
-  if (q->users[rnti]) {
+void srslte_pusch_free_rnti(srslte_pusch_t *q, uint16_t rnti) {
+
+  uint32_t rnti_idx = q->is_ue?0:rnti;
+
+  if (q->users[rnti_idx]) {
     for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-      srslte_sequence_free(&q->users[rnti]->seq[i]);
+      srslte_sequence_free(&q->users[rnti_idx]->seq[i]);
     }    
-    free(q->users[rnti]);
-    q->users[rnti] = NULL; 
+    free(q->users[rnti_idx]);
+    q->users[rnti_idx] = NULL;
+    q->ue_rnti = 0;
+  }
+}
+
+static srslte_sequence_t *get_user_sequence(srslte_pusch_t *q, uint16_t rnti, uint32_t sf_idx, uint32_t len)
+{
+  uint32_t rnti_idx = q->is_ue?0:rnti;
+
+  // The scrambling sequence is pregenerated for all RNTIs in the eNodeB but only for C-RNTI in the UE
+  if (q->users[rnti_idx] && q->users[rnti_idx]->sequence_generated &&
+      q->users[rnti_idx]->cell_id == q->cell.id                    &&
+      q->ue_rnti == rnti                                           &&
+      ((rnti >= SRSLTE_CRNTI_START && rnti < SRSLTE_CRNTI_END) || !q->is_ue))
+  {
+    return &q->users[rnti_idx]->seq[sf_idx];
+  } else {
+    srslte_sequence_pusch(&q->tmp_seq, rnti, 2 * sf_idx, q->cell.id, len);
+    return &q->tmp_seq;
   }
 }
 
@@ -445,18 +521,13 @@ int srslte_pusch_encode(srslte_pusch_t *q, srslte_pusch_cfg_t *cfg, srslte_softb
       return SRSLTE_ERROR;
     }
 
-    if (q->users[rnti] && q->users[rnti]->sequences_generated) {
-      srslte_scrambling_bytes(&q->users[rnti]->seq[cfg->sf_idx], (uint8_t*) q->q, cfg->nbits.nof_bits);
-    } else {
-      srslte_sequence_t seq;
-      if (srslte_sequence_pusch(&seq, rnti, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-        return SRSLTE_ERROR;
-      }
-      srslte_scrambling_bytes(&seq, (uint8_t*) q->q, cfg->nbits.nof_bits);
-      srslte_sequence_free(&seq);
-    }
-    
-    // Correct UCI placeholder/repetition bits    
+    // Generate scrambling sequence if not pre-generated
+    srslte_sequence_t *seq = get_user_sequence(q, rnti, cfg->sf_idx, cfg->nbits.nof_bits);
+
+    // Run scrambling
+    srslte_scrambling_bytes(seq, (uint8_t*) q->q, cfg->nbits.nof_bits);
+
+    // Correct UCI placeholder/repetition bits
     uint8_t *d = q->q; 
     for (int i = 0; i < q->ul_sch.nof_ri_ack_bits; i++) {     
       if (q->ul_sch.ack_ri_bits[i].type == UCI_BIT_PLACEHOLDER) {
@@ -528,24 +599,15 @@ int srslte_pusch_decode(srslte_pusch_t *q,
     srslte_predecoding_single(q->d, q->ce, q->z, cfg->nbits.nof_re, noise_estimate);
     
     // DFT predecoding
-    srslte_dft_predecoding(&q->dft_precoding, q->z, q->d, cfg->grant.L_prb, cfg->nbits.nof_symb);
+    srslte_dft_precoding(&q->dft_precoding, q->z, q->d, cfg->grant.L_prb, cfg->nbits.nof_symb);
     
     // Soft demodulation
     srslte_demod_soft_demodulate_s(cfg->grant.mcs.mod, q->d, q->q, cfg->nbits.nof_re);
 
-    srslte_sequence_t *seq = NULL;
+    // Generate scrambling sequence if not pre-generated
+    srslte_sequence_t *seq = get_user_sequence(q, rnti, cfg->sf_idx, cfg->nbits.nof_bits);
 
-    // Create sequence if does not exist
-    if (q->users[rnti] && q->users[rnti]->sequences_generated) {
-      seq = &q->users[rnti]->seq[cfg->sf_idx];
-    } else {
-      seq = &q->tmp_seq;
-      if (srslte_sequence_pusch(seq, rnti, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-        return SRSLTE_ERROR;
-      }
-    }
-    
-    // Decode RI/HARQ bits before descrambling 
+    // Decode RI/HARQ bits before descrambling
     if (srslte_ulsch_uci_decode_ri_ack(&q->ul_sch, cfg, softbuffer, q->q, seq->c, uci_data)) {
       fprintf(stderr, "Error decoding RI/HARQ bits\n");
       return SRSLTE_ERROR; 
@@ -554,11 +616,7 @@ int srslte_pusch_decode(srslte_pusch_t *q,
     // Descrambling
     srslte_scrambling_s_offset(seq, q->q, 0, cfg->nbits.nof_bits);
         
-    if (!(q->users[rnti] && q->users[rnti]->sequences_generated)) {
-      srslte_sequence_free(seq);
-    }
-    
-    return srslte_ulsch_uci_decode(&q->ul_sch, cfg, softbuffer, q->q, q->g, data, uci_data);      
+    return srslte_ulsch_uci_decode(&q->ul_sch, cfg, softbuffer, q->q, q->g, data, uci_data);
   } else {
     return SRSLTE_ERROR_INVALID_INPUTS;
   }
diff --git a/lib/src/phy/phch/ra.c b/lib/src/phy/phch/ra.c
index 00713e6c1..b85d2ea2f 100644
--- a/lib/src/phy/phch/ra.c
+++ b/lib/src/phy/phch/ra.c
@@ -521,11 +521,10 @@ static int dl_dci_to_grant_mcs(srslte_ra_dl_dci_t *dci, srslte_ra_dl_grant_t *gr
       grant->mcs[1].tbs = 0;
     }
   }
-  grant->nof_tb = 0;
   for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    grant->tb_en[tb] = dci->tb_en[tb];
     if (dci->tb_en[tb]) {
       grant->Qm[tb] = srslte_mod_bits_x_symbol(grant->mcs[tb].mod);
-      grant->nof_tb++;
     }
   }
   grant->pinfo = dci->pinfo;
@@ -541,12 +540,14 @@ void srslte_ra_dl_grant_to_nbits(srslte_ra_dl_grant_t *grant, uint32_t cfi, srsl
                                  srslte_ra_nbits_t nbits [SRSLTE_MAX_CODEWORDS])
 {
   // Compute number of RE 
-  for (int i = 0; i < grant->nof_tb; i++) {
-    /* Compute number of RE for first transport block */
-    nbits[i].nof_re = srslte_ra_dl_grant_nof_re(grant, cell, sf_idx, cell.nof_prb < 10 ? (cfi + 1) : cfi);
-    nbits[i].lstart = cell.nof_prb < 10 ? (cfi + 1) : cfi;
-    nbits[i].nof_symb = 2 * SRSLTE_CP_NSYMB(cell.cp) - nbits[0].lstart;
-    nbits[i].nof_bits = nbits[i].nof_re * grant->Qm[i];
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (grant->tb_en[i]) {
+      /* Compute number of RE for first transport block */
+      nbits[i].nof_re = srslte_ra_dl_grant_nof_re(grant, cell, sf_idx, cell.nof_prb < 10 ? (cfi + 1) : cfi);
+      nbits[i].lstart = cell.nof_prb < 10 ? (cfi + 1) : cfi;
+      nbits[i].nof_symb = 2 * SRSLTE_CP_NSYMB(cell.cp) - nbits[0].lstart;
+      nbits[i].nof_bits = nbits[i].nof_re * grant->Qm[i];
+    }
   }
 }
 
@@ -820,11 +821,13 @@ void srslte_ra_pdsch_fprint(FILE *f, srslte_ra_dl_dci_t *dci, uint32_t nof_prb)
 void srslte_ra_dl_grant_fprint(FILE *f, srslte_ra_dl_grant_t *grant) {
   srslte_ra_prb_fprint(f, grant);
   fprintf(f, " - Number of PRBs:\t\t\t%d\n", grant->nof_prb);
-  fprintf(f, " - Number of TBs:\t\t\t%d\n", grant->nof_tb);
-  for (int i = 0; i < grant->nof_tb; i++) {
-    fprintf(f, "  - Transport block:\t\t\t%d\n", i);
-    fprintf(f, "   -> Modulation type:\t\t\t%s\n", srslte_mod_string(grant->mcs[i].mod));
-    fprintf(f, "   -> Transport block size:\t\t%d\n", grant->mcs[i].tbs);
+  fprintf(f, " - Number of TBs:\t\t\t%d\n", SRSLTE_RA_DL_GRANT_NOF_TB(grant));
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (grant->tb_en[i]) {
+      fprintf(f, "  - Transport block:\t\t\t%d\n", i);
+      fprintf(f, "   -> Modulation type:\t\t\t%s\n", srslte_mod_string(grant->mcs[i].mod));
+      fprintf(f, "   -> Transport block size:\t\t%d\n", grant->mcs[i].tbs);
+    }
   }
 }
 
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index e0002429e..a44ced2d8 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -524,7 +524,7 @@ int srslte_dlsch_decode2(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbu
                          int16_t *e_bits, uint8_t *data, int codeword_idx) {
   uint32_t Nl = 1;
 
-  if (cfg->nof_layers != cfg->grant.nof_tb) {
+  if (cfg->nof_layers != SRSLTE_RA_DL_GRANT_NOF_TB(&cfg->grant)) {
     Nl = 2;
   }
 
@@ -553,7 +553,7 @@ int srslte_dlsch_encode2(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbu
                               uint8_t *data, uint8_t *e_bits, int codeword_idx) {
   uint32_t Nl = 1;
 
-  if (cfg->nof_layers != cfg->grant.nof_tb) {
+  if (cfg->nof_layers != SRSLTE_RA_DL_GRANT_NOF_TB(&cfg->grant)) {
     Nl = 2;
   }
 
@@ -657,14 +657,17 @@ int srslte_ulsch_uci_decode_ri_ack(srslte_sch_t *q, srslte_pusch_cfg_t *cfg, srs
   
   // Deinterleave and decode HARQ bits
   if (uci_data->uci_ack_len > 0) {
+    uint8_t acks[2] = {0, 0};
     float beta = beta_harq_offset[cfg->uci_cfg.I_offset_ack]; 
     if (cfg->cb_segm.tbs == 0) {
         beta /= beta_cqi_offset[cfg->uci_cfg.I_offset_cqi];
     }
-    ret = srslte_uci_decode_ack(cfg, q_bits, c_seq, beta, nb_q/Qm, uci_data->uci_cqi_len, q->ack_ri_bits, &uci_data->uci_ack);
+    ret = srslte_uci_decode_ack(cfg, q_bits, c_seq, beta, nb_q/Qm, uci_data->uci_cqi_len, q->ack_ri_bits, acks, uci_data->uci_ack_len);
     if (ret < 0) {
       return ret; 
     }
+    uci_data->uci_ack = acks[0];
+    uci_data->uci_ack_2 = acks[1];
     Q_prime_ack = (uint32_t) ret; 
 
     // Set zeros to HARQ bits
@@ -805,11 +808,13 @@ int srslte_ulsch_uci_encode(srslte_sch_t *q,
   
   // Encode (and interleave) ACK
   if (uci_data.uci_ack_len > 0) {
+    uint8_t acks [2] = {uci_data.uci_ack, uci_data.uci_ack_2};
     float beta = beta_harq_offset[cfg->uci_cfg.I_offset_ack]; 
     if (cfg->cb_segm.tbs == 0) {
         beta /= beta_cqi_offset[cfg->uci_cfg.I_offset_cqi];
     }
-    ret = srslte_uci_encode_ack(cfg, uci_data.uci_ack, uci_data.uci_cqi_len, beta, nb_q/Qm, &q->ack_ri_bits[Q_prime_ri*Qm]);
+    ret = srslte_uci_encode_ack(cfg, acks, uci_data.uci_ack_len, uci_data.uci_cqi_len,
+                                beta, nb_q / Qm, &q->ack_ri_bits[Q_prime_ri * Qm]);
     if (ret < 0) {
       return ret; 
     }
diff --git a/lib/src/phy/phch/sequences.c b/lib/src/phy/phch/sequences.c
index 2816709b4..23ac410c4 100644
--- a/lib/src/phy/phch/sequences.c
+++ b/lib/src/phy/phch/sequences.c
@@ -33,7 +33,6 @@
  * 36.211 6.6.1
  */
 int srslte_sequence_pbch(srslte_sequence_t *seq, srslte_cp_t cp, uint32_t cell_id) {
-  bzero(seq, sizeof(srslte_sequence_t));
   return srslte_sequence_LTE_pr(seq, SRSLTE_CP_ISNORM(cp)?1920:1728, cell_id);
 }
 
@@ -41,7 +40,6 @@ int srslte_sequence_pbch(srslte_sequence_t *seq, srslte_cp_t cp, uint32_t cell_i
  * 36.211 6.7.1
  */
 int srslte_sequence_pcfich(srslte_sequence_t *seq, uint32_t nslot, uint32_t cell_id) {
-  bzero(seq, sizeof(srslte_sequence_t));
   return srslte_sequence_LTE_pr(seq, 32, (nslot/2+1) * (2*cell_id + 1) * 512 + cell_id);
 }
 
@@ -50,7 +48,6 @@ int srslte_sequence_pcfich(srslte_sequence_t *seq, uint32_t nslot, uint32_t cell
  * 36.211 6.9.1
  */
 int srslte_sequence_phich(srslte_sequence_t *seq, uint32_t nslot, uint32_t cell_id) {
-  bzero(seq, sizeof(srslte_sequence_t));
   return srslte_sequence_LTE_pr(seq, 12, (nslot/2+1) * (2*cell_id + 1) * 512 + cell_id);
 }
 
@@ -58,7 +55,6 @@ int srslte_sequence_phich(srslte_sequence_t *seq, uint32_t nslot, uint32_t cell_
  * 36.211 6.8.2
  */
 int srslte_sequence_pdcch(srslte_sequence_t *seq, uint32_t nslot, uint32_t cell_id, uint32_t len) {
-  bzero(seq, sizeof(srslte_sequence_t));
   return srslte_sequence_LTE_pr(seq, len, (nslot/2) * 512 + cell_id);
 }
 
@@ -66,7 +62,6 @@ int srslte_sequence_pdcch(srslte_sequence_t *seq, uint32_t nslot, uint32_t cell_
  * 36.211 6.3.1
  */
 int srslte_sequence_pdsch(srslte_sequence_t *seq, uint16_t rnti, int q, uint32_t nslot, uint32_t cell_id, uint32_t len) {
-  bzero(seq, sizeof(srslte_sequence_t));
   return srslte_sequence_LTE_pr(seq, len, (rnti<<14) + (q<<13) + ((nslot/2)<<9) + cell_id);
 }
 
@@ -74,7 +69,6 @@ int srslte_sequence_pdsch(srslte_sequence_t *seq, uint16_t rnti, int q, uint32_t
  * 36.211 5.3.1
  */
 int srslte_sequence_pusch(srslte_sequence_t *seq, uint16_t rnti, uint32_t nslot, uint32_t cell_id, uint32_t len) {
-  bzero(seq, sizeof(srslte_sequence_t));
   return srslte_sequence_LTE_pr(seq, len, (rnti<<14) + ((nslot/2)<<9) + cell_id);
 }
 
@@ -82,6 +76,5 @@ int srslte_sequence_pusch(srslte_sequence_t *seq, uint16_t rnti, uint32_t nslot,
  * 36.211 5.4.2
  */
 int srslte_sequence_pucch(srslte_sequence_t *seq, uint16_t rnti, uint32_t nslot, uint32_t cell_id) {
-  bzero(seq, sizeof(srslte_sequence_t));
   return srslte_sequence_LTE_pr(seq, 20, ((((nslot/2)+1)*(2*cell_id+1))<<16)+rnti);
 }
diff --git a/lib/src/phy/phch/test/pbch_file_test.c b/lib/src/phy/phch/test/pbch_file_test.c
index f3e4b9b08..734640d55 100644
--- a/lib/src/phy/phch/test/pbch_file_test.c
+++ b/lib/src/phy/phch/test/pbch_file_test.c
@@ -131,7 +131,11 @@ int base_init() {
     return -1;
   }
 
-  if (srslte_chest_dl_init(&chest, cell)) {
+  if (srslte_chest_dl_init(&chest, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing equalizer\n");
+    return -1;
+  }
+  if (srslte_chest_dl_set_cell(&chest, cell)) {
     fprintf(stderr, "Error initializing equalizer\n");
     return -1;
   }
@@ -141,7 +145,11 @@ int base_init() {
     return -1;
   }
 
-  if (srslte_pbch_init(&pbch, cell)) {
+  if (srslte_pbch_init(&pbch)) {
+    fprintf(stderr, "Error initiating PBCH\n");
+    return -1;
+  }
+  if (srslte_pbch_set_cell(&pbch, cell)) {
     fprintf(stderr, "Error initiating PBCH\n");
     return -1;
   }
diff --git a/lib/src/phy/phch/test/pbch_test.c b/lib/src/phy/phch/test/pbch_test.c
index c86e6f48a..a8c673714 100644
--- a/lib/src/phy/phch/test/pbch_test.c
+++ b/lib/src/phy/phch/test/pbch_test.c
@@ -104,7 +104,11 @@ int main(int argc, char **argv) {
     }
 
   }
-  if (srslte_pbch_init(&pbch, cell)) {
+  if (srslte_pbch_init(&pbch)) {
+    fprintf(stderr, "Error creating PBCH object\n");
+    exit(-1);
+  }
+  if (srslte_pbch_set_cell(&pbch, cell)) {
     fprintf(stderr, "Error creating PBCH object\n");
     exit(-1);
   }
diff --git a/lib/src/phy/phch/test/pcfich_file_test.c b/lib/src/phy/phch/test/pcfich_file_test.c
index 27b50baeb..dfb8d72e3 100644
--- a/lib/src/phy/phch/test/pcfich_file_test.c
+++ b/lib/src/phy/phch/test/pcfich_file_test.c
@@ -142,7 +142,11 @@ int base_init() {
     }
   }
   
-  if (srslte_chest_dl_init(&chest, cell)) {
+  if (srslte_chest_dl_init(&chest, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing equalizer\n");
+    return -1;
+  }
+  if (srslte_chest_dl_set_cell(&chest, cell)) {
     fprintf(stderr, "Error initializing equalizer\n");
     return -1;
   }
@@ -157,7 +161,11 @@ int base_init() {
     return -1;
   }
 
-  if (srslte_pcfich_init(&pcfich, &regs, cell)) {
+  if (srslte_pcfich_init(&pcfich, 1)) {
+    fprintf(stderr, "Error creating PBCH object\n");
+    return -1;
+  }
+  if (srslte_pcfich_set_cell(&pcfich, &regs, cell)) {
     fprintf(stderr, "Error creating PBCH object\n");
     return -1;
   }
diff --git a/lib/src/phy/phch/test/pcfich_test.c b/lib/src/phy/phch/test/pcfich_test.c
index 5d90b645a..32e9925b2 100644
--- a/lib/src/phy/phch/test/pcfich_test.c
+++ b/lib/src/phy/phch/test/pcfich_test.c
@@ -124,7 +124,11 @@ int main(int argc, char **argv) {
       exit(-1);
     }
 
-    if (srslte_pcfich_init(&pcfich, &regs, cell)) {
+    if (srslte_pcfich_init(&pcfich, 1)) {
+      fprintf(stderr, "Error creating PBCH object\n");
+      exit(-1);
+    }
+    if (srslte_pcfich_set_cell(&pcfich, &regs, cell)) {
       fprintf(stderr, "Error creating PBCH object\n");
       exit(-1);
     }
diff --git a/lib/src/phy/phch/test/pdcch_file_test.c b/lib/src/phy/phch/test/pdcch_file_test.c
index b505e6b68..d4ceed4b6 100644
--- a/lib/src/phy/phch/test/pdcch_file_test.c
+++ b/lib/src/phy/phch/test/pdcch_file_test.c
@@ -148,7 +148,11 @@ int base_init() {
     }
   }
 
-  if (srslte_chest_dl_init(&chest, cell)) {
+  if (srslte_chest_dl_init(&chest, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing equalizer\n");
+    return -1;
+  }
+  if (srslte_chest_dl_set_cell(&chest, cell)) {
     fprintf(stderr, "Error initializing equalizer\n");
     return -1;
   }
@@ -167,11 +171,15 @@ int base_init() {
     fprintf(stderr, "Error setting CFI %d\n", cfi);
     return -1;
   }
-  if (srslte_pdcch_init(&pdcch, &regs, cell)) {
+  if (srslte_pdcch_init_ue(&pdcch, cell.nof_prb, 1)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);
   }
-  
+  if (srslte_pdcch_set_cell(&pdcch, &regs, cell)) {
+    fprintf(stderr, "Error creating PDCCH object\n");
+    exit(-1);
+  }
+
   DEBUG("Memory init OK\n",0);
   return 0;
 }
diff --git a/lib/src/phy/phch/test/pdcch_test.c b/lib/src/phy/phch/test/pdcch_test.c
index 9495c497b..d5c33a709 100644
--- a/lib/src/phy/phch/test/pdcch_test.c
+++ b/lib/src/phy/phch/test/pdcch_test.c
@@ -204,15 +204,23 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-  if (srslte_pdcch_init_tx(&pdcch_tx, &regs, cell)) {
+  if (srslte_pdcch_init_enb(&pdcch_tx, cell.nof_prb)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);
   }
+  if (srslte_pdcch_set_cell(&pdcch_tx, &regs, cell)) {
+    fprintf(stderr, "Error setting cell in PDCCH object\n");
+    exit(-1);
+  }
 
-  if (srslte_pdcch_init_rx(&pdcch_rx, &regs, cell, nof_rx_ant)) {
+  if (srslte_pdcch_init_ue(&pdcch_rx, cell.nof_prb, nof_rx_ant)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);
   }
+  if (srslte_pdcch_set_cell(&pdcch_rx, &regs, cell)) {
+    fprintf(stderr, "Error setting cell in PDCCH object\n");
+    exit(-1);
+  }
 
   /* Resource allocate init */
   nof_dcis = 0;
diff --git a/lib/src/phy/phch/test/pdsch_pdcch_file_test.c b/lib/src/phy/phch/test/pdsch_pdcch_file_test.c
index c32f6ac76..90c0e1c17 100644
--- a/lib/src/phy/phch/test/pdsch_pdcch_file_test.c
+++ b/lib/src/phy/phch/test/pdsch_pdcch_file_test.c
@@ -137,11 +137,15 @@ int base_init() {
     exit(-1);
   }
 
-  if (srslte_ue_dl_init(&ue_dl, cell, 1)) {
+  if (srslte_ue_dl_init(&ue_dl, cell.nof_prb, 1)) {
     fprintf(stderr, "Error initializing UE DL\n");
     return -1;
   }
-  
+  if (srslte_ue_dl_set_cell(&ue_dl, cell)) {
+    fprintf(stderr, "Error initializing UE DL\n");
+    return -1;
+  }
+
   srslte_ue_dl_set_rnti(&ue_dl, rnti); 
 
   DEBUG("Memory init OK\n",0);
diff --git a/lib/src/phy/phch/test/pdsch_test.c b/lib/src/phy/phch/test/pdsch_test.c
index b98d75c22..c5b55b48d 100644
--- a/lib/src/phy/phch/test/pdsch_test.c
+++ b/lib/src/phy/phch/test/pdsch_test.c
@@ -153,7 +153,7 @@ int main(int argc, char **argv) {
   int ret = -1;
   struct timeval t[3];
   srslte_softbuffer_tx_t *softbuffers_tx[SRSLTE_MAX_CODEWORDS];
-  int M=10;
+  int M=1;
   bool acks[SRSLTE_MAX_CODEWORDS] = {false};
 
   parse_args(argc,argv);
@@ -213,8 +213,6 @@ int main(int argc, char **argv) {
     return ret;
   }
 
-
-
 #ifdef DO_OFDM
   srslte_ofdm_tx_init(&ofdm_tx, cell.cp, cell.nof_prb);
   srslte_ofdm_rx_init(&ofdm_rx, cell.cp, cell.nof_prb);
@@ -257,8 +255,8 @@ int main(int argc, char **argv) {
   }
 
 
-  for (int i = 0; i < grant.nof_tb; i++) {
-    if (grant.mcs[i].tbs) {
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (grant.tb_en[i]) {
       data_tx[i] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs[i].tbs);
       if (!data_tx[i]) {
         perror("srslte_vec_malloc");
@@ -276,7 +274,11 @@ int main(int argc, char **argv) {
     }
   }
 
-  if (srslte_pdsch_init_rx(&pdsch_rx, cell, nof_rx_antennas)) {
+  if (srslte_pdsch_init_ue(&pdsch_rx, cell.nof_prb, nof_rx_antennas)) {
+    fprintf(stderr, "Error creating PDSCH object\n");
+    goto quit;
+  }
+  if (srslte_pdsch_set_cell(&pdsch_rx, cell)) {
     fprintf(stderr, "Error creating PDSCH object\n");
     goto quit;
   }
@@ -303,12 +305,11 @@ int main(int argc, char **argv) {
   INFO("              cp=%s\n", srslte_cp_string(cell.cp));
   INFO("    phich_length=%d\n", (int) cell.phich_length);
   INFO(" phich_resources=%d\n", (int) cell.phich_resources);
-  INFO("          nof_tb=%d\n", pdsch_cfg.grant.nof_tb);
   INFO("         nof_prb=%d\n", pdsch_cfg.grant.nof_prb);
   INFO("          sf_idx=%d\n", pdsch_cfg.sf_idx);
   INFO("       mimo_type=%s\n", srslte_mimotype2str(pdsch_cfg.mimo_type));
   INFO("      nof_layers=%d\n", pdsch_cfg.nof_layers);
-  INFO("          nof_tb=%d\n", pdsch_cfg.grant.nof_tb);
+  INFO("          nof_tb=%d\n", SRSLTE_RA_DL_GRANT_NOF_TB(&pdsch_cfg.grant));
   for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
     INFO(" Tranport block index %d:\n", i);
     INFO("              Qm=%d\n", pdsch_cfg.grant.Qm[i]);
@@ -334,18 +335,26 @@ int main(int argc, char **argv) {
     srslte_filesource_read(&fsrc, rx_slot_symbols[0], SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp));
 #endif
     
-    srslte_chest_dl_t chest; 
-    if (srslte_chest_dl_init(&chest, cell)) {
+    srslte_chest_dl_t chest;
+    if (srslte_chest_dl_init(&chest, cell.nof_prb)) {
       fprintf(stderr, "Error initializing equalizer\n");
       exit(-1);
     }
+    if (srslte_chest_dl_set_cell(&chest, cell)) {
+      printf("Error initializing equalizer\n");
+      exit(-1);
+    }
     srslte_chest_dl_estimate(&chest, rx_slot_symbols[0], ce[0], subframe);
     srslte_chest_dl_free(&chest);
     
     srslte_filesource_free(&fsrc);
   } else {
 
-    if (srslte_pdsch_init_tx(&pdsch_tx, cell)) {
+    if (srslte_pdsch_init_enb(&pdsch_tx, cell.nof_prb)) {
+      fprintf(stderr, "Error creating PDSCH object\n");
+      goto quit;
+    }
+    if (srslte_pdsch_set_cell(&pdsch_tx, cell)) {
       fprintf(stderr, "Error creating PDSCH object\n");
       goto quit;
     }
@@ -372,9 +381,11 @@ int main(int argc, char **argv) {
       }
     }
 
-    for (int tb = 0; tb < grant.nof_tb; tb++) {
-      for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
-        data_tx[tb][byte] = (uint8_t)(rand() % 256);
+    for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+      if (grant.tb_en[tb]) {
+        for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
+          data_tx[tb][byte] = (uint8_t) (rand() % 256);
+        }
       }
     }
 
@@ -444,8 +455,8 @@ int main(int argc, char **argv) {
       srslte_ofdm_rx_sf(&ofdm_rx, tx_sf_symbols[i], rx_slot_symbols[i]);
     }
 #endif
-    for (i = 0; i < grant.nof_tb; i++) {
-      if (grant.mcs[i].tbs) {
+    for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+      if (grant.tb_en[i]) {
         srslte_softbuffer_rx_reset_tbs(softbuffers_rx[i], (uint32_t) grant.mcs[i].tbs);
       }
     }
@@ -464,19 +475,23 @@ int main(int argc, char **argv) {
   }
 
   /* Check Tx and Rx bytes */
-  for (int tb = 0; tb < grant.nof_tb; tb++) {
-    for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
-      if (data_tx[tb][byte] != data_rx[tb][byte]) {
-        ERROR("Found BYTE error in TB %d (%02X != %02X), quiting...", tb, data_tx[tb][byte], data_rx[tb][byte]);
-        ret = SRSLTE_ERROR;
-        goto quit;
+  for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    if (grant.tb_en[tb]) {
+      for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
+        if (data_tx[tb][byte] != data_rx[tb][byte]) {
+          ERROR("Found BYTE error in TB %d (%02X != %02X), quiting...", tb, data_tx[tb][byte], data_rx[tb][byte]);
+          ret = SRSLTE_ERROR;
+          goto quit;
+        }
       }
     }
   }
 
   /* Check all transport blocks have been decoded OK */
-  for (int tb = 0; tb < grant.nof_tb; tb++) {
-    ret |= (acks[tb]) ? SRSLTE_SUCCESS : SRSLTE_ERROR;
+  for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    if (grant.tb_en[tb]) {
+      ret |= (acks[tb]) ? SRSLTE_SUCCESS : SRSLTE_ERROR;
+    }
   }
 
   ret = SRSLTE_SUCCESS;
diff --git a/lib/src/phy/phch/test/phich_file_test.c b/lib/src/phy/phch/test/phich_file_test.c
index f037a55da..0250c6f0b 100644
--- a/lib/src/phy/phch/test/phich_file_test.c
+++ b/lib/src/phy/phch/test/phich_file_test.c
@@ -29,6 +29,7 @@
 #include <string.h>
 #include <strings.h>
 #include <unistd.h>
+#include <srslte/phy/common/phy_common.h>
 
 #include "srslte/srslte.h"
 
@@ -165,7 +166,11 @@ int base_init() {
     }
   }
 
-  if (srslte_chest_dl_init(&chest, cell)) {
+  if (srslte_chest_dl_init(&chest, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing equalizer\n");
+    return -1;
+  }
+  if (srslte_chest_dl_set_cell(&chest, cell)) {
     fprintf(stderr, "Error initializing equalizer\n");
     return -1;
   }
@@ -180,7 +185,11 @@ int base_init() {
     return -1;
   }
 
-  if (srslte_phich_init(&phich, &regs, cell)) {
+  if (srslte_phich_init(&phich, 1)) {
+    fprintf(stderr, "Error creating PBCH object\n");
+    return -1;
+  }
+  if (srslte_phich_set_cell(&phich, &regs, cell)) {
     fprintf(stderr, "Error creating PBCH object\n");
     return -1;
   }
diff --git a/lib/src/phy/phch/test/phich_test.c b/lib/src/phy/phch/test/phich_test.c
index d1c6ef964..5334b210f 100644
--- a/lib/src/phy/phch/test/phich_test.c
+++ b/lib/src/phy/phch/test/phich_test.c
@@ -142,6 +142,10 @@ int main(int argc, char **argv) {
     cid = cell.id;
     max_cid = cell.id;
   }
+  if (srslte_phich_init(&phich, 1)) {
+    fprintf(stderr, "Error creating PBCH object\n");
+    exit(-1);
+  }
   while(cid <= max_cid) {
     cell.id = cid;
     
@@ -152,7 +156,7 @@ int main(int argc, char **argv) {
       exit(-1);
     }
 
-    if (srslte_phich_init(&phich, &regs, cell)) {
+    if (srslte_phich_set_cell(&phich, &regs, cell)) {
       fprintf(stderr, "Error creating PBCH object\n");
       exit(-1);
     }
@@ -198,10 +202,10 @@ int main(int argc, char **argv) {
         }
       }
     }
-    srslte_phich_free(&phich);
     srslte_regs_free(&regs);
     cid++;
   }
+  srslte_phich_free(&phich);
 
   for (i=0;i<SRSLTE_MAX_PORTS;i++) {
     free(ce[i]);
diff --git a/lib/src/phy/phch/test/prach_test.c b/lib/src/phy/phch/test/prach_test.c
index ddd05eba0..9d09aaf1d 100644
--- a/lib/src/phy/phch/test/prach_test.c
+++ b/lib/src/phy/phch/test/prach_test.c
@@ -85,12 +85,14 @@ int main(int argc, char **argv) {
   cf_t preamble[MAX_LEN];
   memset(preamble, 0, sizeof(cf_t)*MAX_LEN);
 
-  srslte_prach_init(p,
-             N_ifft_ul,
-             config_idx,
-             root_seq_idx,
-             high_speed_flag,
-             zero_corr_zone);
+  srslte_prach_init(p, N_ifft_ul);
+
+  srslte_prach_set_cell( p,
+                         N_ifft_ul,
+                         config_idx,
+                         root_seq_idx,
+                         high_speed_flag,
+                         zero_corr_zone);
 
   uint32_t seq_index = 0;
   uint32_t frequency_offset = 0;
diff --git a/lib/src/phy/phch/test/prach_test_multi.c b/lib/src/phy/phch/test/prach_test_multi.c
index 9e3dc1c97..58f6d6af8 100644
--- a/lib/src/phy/phch/test/prach_test_multi.c
+++ b/lib/src/phy/phch/test/prach_test_multi.c
@@ -90,12 +90,14 @@ int main(int argc, char **argv) {
   cf_t preamble_sum[MAX_LEN];
   memset(preamble_sum, 0, sizeof(cf_t)*MAX_LEN);
 
-  srslte_prach_init(p,
-             N_ifft_ul,
-             preamble_format,
-             root_seq_idx,
-             high_speed_flag,
-             zero_corr_zone);
+  srslte_prach_init(p, N_ifft_ul);
+
+  srslte_prach_set_cell( p,
+                         N_ifft_ul,
+                         preamble_format,
+                         root_seq_idx,
+                         high_speed_flag,
+                         zero_corr_zone);
 
   uint32_t seq_index = 0;
   uint32_t frequency_offset = 0;
diff --git a/lib/src/phy/phch/test/prach_test_usrp.c b/lib/src/phy/phch/test/prach_test_usrp.c
index f5f1a9e5c..2defdec4f 100644
--- a/lib/src/phy/phch/test/prach_test_usrp.c
+++ b/lib/src/phy/phch/test/prach_test_usrp.c
@@ -133,6 +133,9 @@ int main(int argc, char **argv) {
   memset(preamble, 0, sizeof(cf_t)*MAX_LEN);
 
   srslte_prach_init(p,
+                    srslte_symbol_sz(nof_prb));
+
+  srslte_prach_set_cell(p,
                     srslte_symbol_sz(nof_prb),
                     preamble_format,
                     root_seq_idx,
diff --git a/lib/src/phy/phch/test/pucch_test.c b/lib/src/phy/phch/test/pucch_test.c
index 316a5efc4..43aaca953 100644
--- a/lib/src/phy/phch/test/pucch_test.c
+++ b/lib/src/phy/phch/test/pucch_test.c
@@ -87,15 +87,23 @@ int main(int argc, char **argv) {
   
   parse_args(argc,argv);
 
-  if (srslte_pucch_init(&pucch, cell)) {
+  if (srslte_pucch_init(&pucch)) {
     fprintf(stderr, "Error creating PDSCH object\n");
     exit(-1);
   }
-  if (srslte_refsignal_ul_init(&dmrs, cell)) {
+  if (srslte_pucch_set_cell(&pucch, cell)) {
     fprintf(stderr, "Error creating PDSCH object\n");
     exit(-1);
   }
-  
+  if (srslte_refsignal_ul_init(&dmrs, cell.nof_prb)) {
+    fprintf(stderr, "Error creating PDSCH object\n");
+    exit(-1);
+  }
+  if (srslte_refsignal_ul_set_cell(&dmrs, cell)) {
+    fprintf(stderr, "Error creating PDSCH object\n");
+    exit(-1);
+  }
+
   bzero(&pucch_cfg, sizeof(srslte_pucch_cfg_t));
   
   for (int i=0;i<SRSLTE_PUCCH_MAX_BITS;i++) {
diff --git a/lib/src/phy/phch/test/pusch_test.c b/lib/src/phy/phch/test/pusch_test.c
index 0cb2847b7..ece098dcf 100644
--- a/lib/src/phy/phch/test/pusch_test.c
+++ b/lib/src/phy/phch/test/pusch_test.c
@@ -110,7 +110,8 @@ void parse_args(int argc, char **argv) {
 }
 
 int main(int argc, char **argv) {
-  srslte_pusch_t pusch;
+  srslte_pusch_t pusch_tx;
+  srslte_pusch_t pusch_rx;
   uint8_t *data = NULL;
   cf_t *sf_symbols = NULL;
   cf_t *ce = NULL; 
@@ -145,11 +146,23 @@ int main(int argc, char **argv) {
   ul_hopping.hopping_offset = 0;
   ul_hopping.hop_mode = SRSLTE_PUSCH_HOP_MODE_INTER_SF;
   
-  if (srslte_pusch_init(&pusch, cell)) {
-    fprintf(stderr, "Error creating PDSCH object\n");
+  if (srslte_pusch_init_ue(&pusch_tx, cell.nof_prb)) {
+    fprintf(stderr, "Error creating PUSCH object\n");
     goto quit;
   }
-  
+  if (srslte_pusch_set_cell(&pusch_tx, cell)) {
+    fprintf(stderr, "Error creating PUSCH object\n");
+    goto quit;
+  }
+  if (srslte_pusch_init_enb(&pusch_rx, cell.nof_prb)) {
+    fprintf(stderr, "Error creating PUSCH object\n");
+    goto quit;
+  }
+  if (srslte_pusch_set_cell(&pusch_rx, cell)) {
+    fprintf(stderr, "Error creating PUSCH object\n");
+    goto quit;
+  }
+
   /* Configure PUSCH */
     
   printf("Encoding rv_idx=%d\n",rv_idx);
@@ -159,27 +172,33 @@ int main(int argc, char **argv) {
   uci_cfg.I_offset_ri = 2; 
   uci_cfg.I_offset_ack = 4; 
 
-  if (srslte_pusch_cfg(&pusch, &cfg, &grant, &uci_cfg, &ul_hopping, NULL, subframe, 0, 0)) {
+  if (srslte_pusch_cfg(&pusch_tx, &cfg, &grant, &uci_cfg, &ul_hopping, NULL, subframe, 0, 0)) {
     fprintf(stderr, "Error configuring PDSCH\n");
     exit(-1);
   }
-  
+  if (srslte_pusch_cfg(&pusch_rx, &cfg, &grant, &uci_cfg, &ul_hopping, NULL, subframe, 0, 0)) {
+    fprintf(stderr, "Error configuring PDSCH\n");
+    exit(-1);
+  }
+
   uint16_t rnti = 1234; 
-  srslte_pusch_set_rnti(&pusch, rnti);
-  
+  srslte_pusch_set_rnti(&pusch_tx, rnti);
+  srslte_pusch_set_rnti(&pusch_rx, rnti);
+
   srslte_uci_data_t uci_data_tx; 
   srslte_uci_data_t uci_data_rx; 
   bzero(&uci_data_tx, sizeof(srslte_uci_data_t));
   uci_data_tx.uci_cqi_len = 4; 
   uci_data_tx.uci_ri_len = 0; 
-  uci_data_tx.uci_ack_len = 0; 
+  uci_data_tx.uci_ack_len = 2;
   memcpy(&uci_data_rx, &uci_data_tx, sizeof(srslte_uci_data_t));
     
   for (uint32_t i=0;i<20;i++) {
     uci_data_tx.uci_cqi [i] = 1;
   }
-  uci_data_tx.uci_ri = 1; 
-  uci_data_tx.uci_ack = 1; 
+  uci_data_tx.uci_ri = 1;
+  uci_data_tx.uci_ack = 1;
+  uci_data_tx.uci_ack_2 = 1;
 
   uint32_t nof_re = SRSLTE_NRE*cell.nof_prb*2*SRSLTE_CP_NSYMB(cell.cp);
   sf_symbols = srslte_vec_malloc(sizeof(cf_t) * nof_re);
@@ -211,13 +230,13 @@ int main(int argc, char **argv) {
   
   uint32_t ntrials = 100; 
 
-  if (srslte_pusch_encode(&pusch, &cfg, &softbuffer_tx, data, uci_data_tx, rnti, sf_symbols)) {
+  if (srslte_pusch_encode(&pusch_tx, &cfg, &softbuffer_tx, data, uci_data_tx, rnti, sf_symbols)) {
     fprintf(stderr, "Error encoding TB\n");
     exit(-1);
   }
   if (rv_idx > 0) {
     cfg.rv = rv_idx; 
-    if (srslte_pusch_encode(&pusch, &cfg, &softbuffer_tx, data, uci_data_tx, rnti, sf_symbols)) {
+    if (srslte_pusch_encode(&pusch_tx, &cfg, &softbuffer_tx, data, uci_data_tx, rnti, sf_symbols)) {
       fprintf(stderr, "Error encoding TB\n");
       exit(-1);
     }
@@ -233,7 +252,7 @@ int main(int argc, char **argv) {
   }
   
   gettimeofday(&t[1], NULL);
-  int r = srslte_pusch_decode(&pusch, &cfg, &softbuffer_rx, sf_symbols, ce, 0, rnti, data, &uci_data_rx);
+  int r = srslte_pusch_decode(&pusch_rx, &cfg, &softbuffer_rx, sf_symbols, ce, 0, rnti, data, &uci_data_rx);
   gettimeofday(&t[2], NULL);
   get_time_interval(t);
   if (r) {
@@ -250,11 +269,19 @@ int main(int argc, char **argv) {
   if (uci_data_tx.uci_ack_len) {
     if (uci_data_tx.uci_ack != uci_data_rx.uci_ack) {
       printf("UCI ACK bit error: %d != %d\n", uci_data_tx.uci_ack, uci_data_rx.uci_ack);
+      ret = SRSLTE_ERROR;
+    }
+  }
+  if (uci_data_tx.uci_ack_len > 1) {
+    if (uci_data_tx.uci_ack_2 != uci_data_rx.uci_ack_2) {
+      printf("UCI ACK 2 bit error: %d != %d\n", uci_data_tx.uci_ack_2, uci_data_rx.uci_ack_2);
+      ret = SRSLTE_ERROR;
     }
   }
   if (uci_data_tx.uci_ri_len) {
     if (uci_data_tx.uci_ri != uci_data_rx.uci_ri) {
       printf("UCI RI bit error: %d != %d\n", uci_data_tx.uci_ri, uci_data_rx.uci_ri);
+      ret = SRSLTE_ERROR;
     }
   }
   if (uci_data_tx.uci_cqi_len) {
@@ -265,7 +292,8 @@ int main(int argc, char **argv) {
   }
 
 quit:
-  srslte_pusch_free(&pusch);
+  srslte_pusch_free(&pusch_tx);
+  srslte_pusch_free(&pusch_rx);
   srslte_softbuffer_tx_free(&softbuffer_tx);
   
   if (sf_symbols) {
diff --git a/lib/src/phy/phch/uci.c b/lib/src/phy/phch/uci.c
index 3b22eb792..ed253d2cc 100644
--- a/lib/src/phy/phch/uci.c
+++ b/lib/src/phy/phch/uci.c
@@ -551,24 +551,56 @@ static uint32_t Q_prime_ri_ack(srslte_pusch_cfg_t *cfg,
   return Q_prime; 
 }
 
-static void encode_ri_ack(uint8_t data, srslte_uci_bit_type_t q_encoded_bits[6], uint8_t Qm) 
+static uint32_t encode_ri_ack(uint8_t data[2], uint32_t data_len, srslte_uci_bit_type_t q_encoded_bits[18], uint8_t Qm)
 {
-  q_encoded_bits[0] = data?UCI_BIT_1:UCI_BIT_0;
-  q_encoded_bits[1] = UCI_BIT_REPETITION; 
-  for (uint32_t i=2;i<Qm;i++) {
-    q_encoded_bits[i] = UCI_BIT_PLACEHOLDER;
+  uint32_t i = 0;
+
+  if (data_len == 1) {
+    q_encoded_bits[i++] = data[0] ? UCI_BIT_1 : UCI_BIT_0;
+    q_encoded_bits[i++] = UCI_BIT_REPETITION;
+    while(i < Qm) {
+      q_encoded_bits[i++] = UCI_BIT_PLACEHOLDER;
+    }
+  } else {
+    q_encoded_bits[i++] = data[0] ? UCI_BIT_1 : UCI_BIT_0;
+    q_encoded_bits[i++] = data[1] ? UCI_BIT_1 : UCI_BIT_0;
+    while(i<Qm) {
+      q_encoded_bits[i++] = UCI_BIT_PLACEHOLDER;
+    }
+    q_encoded_bits[i++] = (data[0]^data[1]) ? UCI_BIT_1 : UCI_BIT_0;
+    q_encoded_bits[i++] = data[0] ? UCI_BIT_1 : UCI_BIT_0;
+    while(i<Qm*2) {
+      q_encoded_bits[i++] = UCI_BIT_PLACEHOLDER;
+    }
+    q_encoded_bits[i++] = data[1] ? UCI_BIT_1 : UCI_BIT_0;
+    q_encoded_bits[i++] = (data[0]^data[1]) ? UCI_BIT_1 : UCI_BIT_0;
+    while(i<Qm*3) {
+      q_encoded_bits[i++] = UCI_BIT_PLACEHOLDER;
+    }
   }
+
+  return i;
 }
                        
-static int32_t decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *pos) 
+static void decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *pos, uint32_t Qm, int32_t data[3])
 {
-  uint32_t p0 = pos[0].position;
-  uint32_t p1 = pos[1].position;
-  
-  uint32_t q0 = c_seq[p0]?q_bits[p0]:-q_bits[p0];  
-  uint32_t q1 = c_seq[p0]?q_bits[p1]:-q_bits[p1];
+  uint32_t p0 = pos[Qm * 0 + 0].position;
+  uint32_t p1 = pos[Qm * 0 + 1].position;
+  uint32_t p2 = pos[Qm * 1 + 0].position;
+  uint32_t p3 = pos[Qm * 1 + 1].position;
+  uint32_t p4 = pos[Qm * 2 + 0].position;
+  uint32_t p5 = pos[Qm * 2 + 1].position;
 
-  return -(q0+q1);
+  int32_t q0 = c_seq[p0] ? q_bits[p0] : -q_bits[p0];
+  int32_t q1 = c_seq[p1] ? q_bits[p1] : -q_bits[p1];
+  int32_t q2 = c_seq[p2] ? q_bits[p2] : -q_bits[p2];
+  int32_t q3 = c_seq[p3] ? q_bits[p3] : -q_bits[p3];
+  int32_t q4 = c_seq[p4] ? q_bits[p4] : -q_bits[p4];
+  int32_t q5 = c_seq[p5] ? q_bits[p5] : -q_bits[p5];
+
+  data[0] -= q0 + q3;
+  data[1] -= q1 + q4;
+  data[2] -= q2 + q5;
 }
 
 
@@ -577,25 +609,29 @@ static int32_t decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *
  */
 int srslte_uci_decode_ack(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_seq, 
                           float beta, uint32_t H_prime_total, 
-                          uint32_t O_cqi, srslte_uci_bit_t *ack_bits, uint8_t *data)
+                          uint32_t O_cqi, srslte_uci_bit_t *ack_bits, uint8_t acks[2], uint32_t nof_acks)
 {
-  int32_t rx_ack = 0; 
-  
+  int32_t acks_sum[3] = {0, 0, 0};
+
   if (beta < 0) {
     fprintf(stderr, "Error beta is reserved\n");
     return -1; 
   }
 
-  uint32_t Qprime = Q_prime_ri_ack(cfg, 1, O_cqi, beta);
+  uint32_t Qprime = Q_prime_ri_ack(cfg, nof_acks, O_cqi, beta);
 
   // Use the same interleaver function to get the HARQ bit position
-  for (uint32_t i=0;i<Qprime;i++) {
+  for (uint32_t i = 0; i < Qprime; i++) {
     uci_ulsch_interleave_ack_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ack_bits[cfg->grant.Qm*i]);
-    rx_ack += (int32_t) decode_ri_ack(q_bits, c_seq, ack_bits);
+    if ((i % 3 == 0) && i > 0) {
+      decode_ri_ack(q_bits, &c_seq[0], &ack_bits[cfg->grant.Qm*(i-3)], cfg->grant.Qm, acks_sum);
+    }
   }
-  
-  if (data) {
-    *data = rx_ack>0;
+
+  if (acks) {
+    acks[0] = (uint8_t)(acks_sum[0] > 0);
+    acks[1] = (uint8_t)(acks_sum[1] > 0);
+    // TODO: Do something with acks_sum[2]
   }
   return (int) Qprime;  
 }
@@ -603,23 +639,23 @@ int srslte_uci_decode_ack(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_s
 /* Encode UCI HARQ/ACK bits as described in 5.2.2.6 of 36.212 
  *  Currently only supporting 1-bit HARQ
  */
-int srslte_uci_encode_ack(srslte_pusch_cfg_t *cfg, uint8_t data, 
+int srslte_uci_encode_ack(srslte_pusch_cfg_t *cfg, uint8_t acks[2], uint32_t nof_acks,
                           uint32_t O_cqi, float beta, uint32_t H_prime_total, 
                           srslte_uci_bit_t *ack_bits)
-{  
+{
   if (beta < 0) {
     fprintf(stderr, "Error beta is reserved\n");
     return -1; 
   }
 
-  uint32_t Qprime = Q_prime_ri_ack(cfg, 1, O_cqi, beta);
-  srslte_uci_bit_type_t q_encoded_bits[6];
+  uint32_t Qprime = Q_prime_ri_ack(cfg, nof_acks, O_cqi, beta);
+  srslte_uci_bit_type_t q_encoded_bits[18];
 
-  encode_ri_ack(data, q_encoded_bits, cfg->grant.Qm);
+  uint32_t nof_encoded_bits = encode_ri_ack(acks, nof_acks, q_encoded_bits, cfg->grant.Qm);
   
   for (uint32_t i=0;i<Qprime;i++) {
     uci_ulsch_interleave_ack_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ack_bits[cfg->grant.Qm*i]);
-    uci_ulsch_interleave_put(q_encoded_bits, cfg->grant.Qm, &ack_bits[cfg->grant.Qm*i]);
+    uci_ulsch_interleave_put(&q_encoded_bits[(i*cfg->grant.Qm)%nof_encoded_bits], cfg->grant.Qm, &ack_bits[cfg->grant.Qm*i]);
   }
   
   return (int) Qprime;
@@ -632,7 +668,7 @@ int srslte_uci_decode_ri(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_se
                           float beta, uint32_t H_prime_total, 
                           uint32_t O_cqi, srslte_uci_bit_t *ri_bits, uint8_t *data)
 {
-  int32_t rx_ri = 0; 
+  int32_t ri_sum[3] = {0, 0, 0};
   
   if (beta < 0) {
     fprintf(stderr, "Error beta is reserved\n");
@@ -644,11 +680,13 @@ int srslte_uci_decode_ri(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_se
   // Use the same interleaver function to get the HARQ bit position
   for (uint32_t i=0;i<Qprime;i++) {
     uci_ulsch_interleave_ri_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ri_bits[cfg->grant.Qm*i]);
-    rx_ri += (int32_t) decode_ri_ack(q_bits, c_seq, ri_bits);
+    if ((i % 3 == 0) && i > 0) {
+      decode_ri_ack(q_bits, &c_seq[0], &ri_bits[cfg->grant.Qm*(i-3)], cfg->grant.Qm, ri_sum);
+    }
   }
 
   if (data) {
-    *data = rx_ri>0;
+    *data = (uint8_t) ((ri_sum[0] + ri_sum[1] + ri_sum[2]) > 0);
   }
 
   return (int) Qprime;  
@@ -659,22 +697,24 @@ int srslte_uci_decode_ri(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_se
  *  Currently only supporting 1-bit RI
  */
 int srslte_uci_encode_ri(srslte_pusch_cfg_t *cfg,
-                         uint8_t data, 
+                         uint8_t ri,
                          uint32_t O_cqi, float beta, uint32_t H_prime_total, 
                          srslte_uci_bit_t *ri_bits)
 {
+  // FIXME: It supports RI of 1 bit only
+  uint8_t data[2] = {ri, 0};
   if (beta < 0) {
     fprintf(stderr, "Error beta is reserved\n");
     return -1; 
   }
   uint32_t Qprime = Q_prime_ri_ack(cfg, 1, O_cqi, beta);
-  srslte_uci_bit_type_t q_encoded_bits[6];
+  srslte_uci_bit_type_t q_encoded_bits[18];
 
-  encode_ri_ack(data, q_encoded_bits, cfg->grant.Qm);
+  uint32_t nof_encoded_bits = encode_ri_ack(data, 1, q_encoded_bits, cfg->grant.Qm);
   
   for (uint32_t i=0;i<Qprime;i++) {
     uci_ulsch_interleave_ri_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ri_bits[cfg->grant.Qm*i]);
-    uci_ulsch_interleave_put(q_encoded_bits, cfg->grant.Qm, &ri_bits[cfg->grant.Qm*i]);
+    uci_ulsch_interleave_put(&q_encoded_bits[(i*cfg->grant.Qm)%nof_encoded_bits], cfg->grant.Qm, &ri_bits[cfg->grant.Qm*i]);
   }
   
   return (int) Qprime;
diff --git a/lib/src/phy/resampling/interp.c b/lib/src/phy/resampling/interp.c
index 553caf4b7..180b3f9fc 100644
--- a/lib/src/phy/resampling/interp.c
+++ b/lib/src/phy/resampling/interp.c
@@ -27,7 +27,8 @@
 #include <complex.h>
 #include <math.h>
 #include <stdlib.h>
-#include <strings.h> 
+#include <strings.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/resampling/interp.h"
 #include "srslte/phy/utils/vector.h"
@@ -108,11 +109,23 @@ int srslte_interp_linear_vector_init(srslte_interp_linsrslte_vec_t *q, uint32_t
       perror("malloc");
       return SRSLTE_ERROR; 
     }    
-    q->vector_len = vector_len; 
+    q->vector_len     = vector_len;
+    q->max_vector_len = vector_len;
   }
   return ret; 
 }
 
+int srslte_interp_linear_vector_resize(srslte_interp_linsrslte_vec_t *q, uint32_t vector_len)
+{
+  if (vector_len <= q->max_vector_len) {
+    q->vector_len = vector_len;
+    return SRSLTE_SUCCESS;
+  } else {
+    fprintf(stderr, "Error resizing interp_linear: vector_len must be lower or equal than initialized\n");
+    return SRSLTE_ERROR;
+  }
+}
+
 void srslte_interp_linear_vector_free(srslte_interp_linsrslte_vec_t *q) {
   if (q->diff_vec) {
     free(q->diff_vec);
@@ -189,7 +202,9 @@ int srslte_interp_linear_init(srslte_interp_lin_t *q, uint32_t vector_len, uint3
     }
 
     q->vector_len = vector_len; 
-    q->M = M; 
+    q->M = M;
+    q->max_vector_len = vector_len;
+    q->max_M = M;
   }
   return ret; 
 }
@@ -209,6 +224,24 @@ void srslte_interp_linear_free(srslte_interp_lin_t *q) {
 
 }
 
+
+int srslte_interp_linear_resize(srslte_interp_lin_t *q, uint32_t vector_len, uint32_t M)
+{
+  if (vector_len <= q->max_vector_len && M <= q->max_M) {
+
+    for (int i=0;i<M;i++) {
+      q->ramp[i] = (float) i;
+    }
+
+    q->vector_len = vector_len;
+    q->M = M;
+    return SRSLTE_SUCCESS;
+  } else {
+    fprintf(stderr, "Error resizing interp_linear: vector_len and M must be lower or equal than initialized\n");
+    return SRSLTE_ERROR;
+  }
+}
+
 void srslte_interp_linear_offset(srslte_interp_lin_t *q, cf_t *input, cf_t *output, 
                           uint32_t off_st, uint32_t off_end) 
 {
diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index 573400f4f..16fb2e07e 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -117,6 +117,7 @@ static void* async_thread(void *h) {
       return NULL; 
     }
   }
+  uhd_async_metadata_free(&md);
   return NULL; 
 }
 
@@ -226,7 +227,7 @@ int rf_uhd_start_rx_stream(void *h)
         .stream_now = false
   };
   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.5; 
+  stream_cmd.time_spec_frac_secs += 0.1;
   if (stream_cmd.time_spec_frac_secs > 1) {
     stream_cmd.time_spec_frac_secs -= 1;
     stream_cmd.time_spec_full_secs += 1; 
@@ -457,14 +458,17 @@ 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);
-  uhd_tx_streamer_free(&handler->tx_stream);
-  uhd_rx_streamer_free(&handler->rx_stream);
   if (handler->has_rssi) {
     uhd_sensor_value_free(&handler->rssi_value);
   }
   handler->async_thread_running = false; 
-  pthread_join(handler->async_thread, NULL); 
+  pthread_join(handler->async_thread, NULL);
+
+  uhd_tx_streamer_free(&handler->tx_stream);
+  uhd_rx_streamer_free(&handler->rx_stream);
   uhd_usrp_free(&handler->usrp);
+
+  free(handler);
   
   /** Something else to close the USRP?? */
   return 0;
@@ -488,8 +492,7 @@ double rf_uhd_set_rx_srate(void *h, double freq)
   for (int i=0;i<handler->nof_rx_channels;i++) {
     uhd_usrp_set_rx_rate(handler->usrp, freq, i);
   }
-  uhd_usrp_get_rx_rate(handler->usrp, 0, &freq);
-  return freq; 
+  return freq;
 }
 
 double rf_uhd_set_tx_srate(void *h, double freq)
@@ -498,7 +501,6 @@ double rf_uhd_set_tx_srate(void *h, double freq)
   for (int i=0;i<handler->nof_tx_channels;i++) {
     uhd_usrp_set_tx_rate(handler->usrp, freq, i);
   }
-  uhd_usrp_get_tx_rate(handler->usrp, 0, &freq);
   handler->tx_rate = freq;
   return freq; 
 }
@@ -509,7 +511,6 @@ double rf_uhd_set_rx_gain(void *h, double gain)
   for (int i=0;i<handler->nof_rx_channels;i++) {
     uhd_usrp_set_rx_gain(handler->usrp, gain, i, "");
   }
-  uhd_usrp_get_rx_gain(handler->usrp, 0, "", &gain);
   return gain;
 }
 
@@ -519,7 +520,6 @@ double rf_uhd_set_tx_gain(void *h, double gain)
   for (int i=0;i<handler->nof_tx_channels;i++) {
     uhd_usrp_set_tx_gain(handler->usrp, gain, i, "");
   }
-  uhd_usrp_get_tx_gain(handler->usrp, 0, "", &gain);
   return gain;
 }
 
@@ -551,7 +551,6 @@ double rf_uhd_set_rx_freq(void *h, double freq)
   for (int i=0;i<handler->nof_rx_channels;i++) {
     uhd_usrp_set_rx_freq(handler->usrp, &tune_request, i, &tune_result);
   }
-  uhd_usrp_get_rx_freq(handler->usrp, 0, &freq);
   return freq;
 }
 
@@ -567,7 +566,6 @@ double rf_uhd_set_tx_freq(void *h, double freq)
   for (int i=0;i<handler->nof_tx_channels;i++) {
     uhd_usrp_set_tx_freq(handler->usrp, &tune_request, i, &tune_result);
   }
-  uhd_usrp_get_tx_freq(handler->usrp, 0, &freq);
   return freq;
 }
 
@@ -629,7 +627,9 @@ int rf_uhd_recv_with_time_multi(void *h,
         log_overflow(handler);
       } else if (error_code == UHD_RX_METADATA_ERROR_CODE_LATE_COMMAND) {
         log_late(handler);
-      } else if (error_code != UHD_RX_METADATA_ERROR_CODE_NONE) {
+      } else if (error_code == UHD_RX_METADATA_ERROR_CODE_TIMEOUT) {
+        fprintf(stderr, "Error timed out while receiving asynchronoous messages from UHD.\n");
+      } else if (error_code != UHD_RX_METADATA_ERROR_CODE_NONE ) {
         fprintf(stderr, "Error code 0x%x was returned during streaming. Aborting.\n", error_code);
       }
       
diff --git a/lib/src/phy/rf/rf_utils.c b/lib/src/phy/rf/rf_utils.c
index f4a2c6790..ce288b5c5 100644
--- a/lib/src/phy/rf/rf_utils.c
+++ b/lib/src/phy/rf/rf_utils.c
@@ -103,11 +103,16 @@ int rf_mib_decoder(srslte_rf_t *rf, uint32_t nof_rx_antennas,cell_search_cfg_t *
   srslte_ue_mib_sync_t ue_mib; 
   uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
 
-  if (srslte_ue_mib_sync_init_multi(&ue_mib, cell->id, cell->cp, srslte_rf_recv_wrapper_cs, nof_rx_antennas, (void*) rf)) {
+  if (srslte_ue_mib_sync_init_multi(&ue_mib, srslte_rf_recv_wrapper_cs, nof_rx_antennas, (void*) rf)) {
     fprintf(stderr, "Error initiating srslte_ue_mib_sync\n");
     goto clean_exit; 
   }
-  
+
+  if (srslte_ue_mib_sync_set_cell(&ue_mib, cell->id, cell->cp)) {
+    fprintf(stderr, "Error initiating srslte_ue_mib_sync\n");
+    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);    
   }
@@ -168,7 +173,6 @@ int rf_cell_search(srslte_rf_t *rf, uint32_t nof_rx_antennas,
     fprintf(stderr, "Error initiating UE cell detect\n");
     return SRSLTE_ERROR; 
   }
-  
   if (config->nof_valid_pss_frames) {
     srslte_ue_cellsearch_set_nof_valid_frames(&cs, config->nof_valid_pss_frames);
   }
diff --git a/lib/src/phy/scrambling/scrambling.c b/lib/src/phy/scrambling/scrambling.c
index bb5637814..42f16d1e8 100644
--- a/lib/src/phy/scrambling/scrambling.c
+++ b/lib/src/phy/scrambling/scrambling.c
@@ -33,29 +33,29 @@
 #include "srslte/phy/scrambling/scrambling.h"
 
 void srslte_scrambling_f(srslte_sequence_t *s, float *data) {
-  srslte_scrambling_f_offset(s, data, 0, s->len);
+  srslte_scrambling_f_offset(s, data, 0, s->cur_len);
 }
 
 void srslte_scrambling_f_offset(srslte_sequence_t *s, float *data, int offset, int len) {
-  assert (len + offset <= s->len);
+  assert (len + offset <= s->cur_len);
   srslte_vec_prod_fff(data, &s->c_float[offset], data, len);
 }
 
 void srslte_scrambling_s(srslte_sequence_t *s, short *data) {
-  srslte_scrambling_s_offset(s, data, 0, s->len);
+  srslte_scrambling_s_offset(s, data, 0, s->cur_len);
 }
 
 void srslte_scrambling_s_offset(srslte_sequence_t *s, short *data, int offset, int len) {
-  assert (len + offset <= s->len);
+  assert (len + offset <= s->cur_len);
   srslte_vec_prod_sss(data, &s->c_short[offset], data, len);
 }
 
 void srslte_scrambling_c(srslte_sequence_t *s, cf_t *data) {
-  srslte_scrambling_c_offset(s, data, 0, s->len);
+  srslte_scrambling_c_offset(s, data, 0, s->cur_len);
 }
 
 void srslte_scrambling_c_offset(srslte_sequence_t *s, cf_t *data, int offset, int len) {
-  assert (len + offset <= s->len);
+  assert (len + offset <= s->cur_len);
   srslte_vec_prod_cfc(data, &s->c_float[offset], data, len);
 }
 
@@ -81,7 +81,7 @@ void scrambling_b_word(uint8_t *c, uint8_t *data, int len) {
 
 
 void srslte_scrambling_b(srslte_sequence_t *s, uint8_t *data) {
-  scrambling_b_word(s->c, data, s->len);  
+  scrambling_b_word(s->c, data, s->cur_len);  
 }
 
 void srslte_scrambling_b_offset(srslte_sequence_t *s, uint8_t *data, int offset, int len) {
diff --git a/lib/src/phy/scrambling/test/scrambling_test.c b/lib/src/phy/scrambling/test/scrambling_test.c
index 9ff47dc85..40722f09b 100644
--- a/lib/src/phy/scrambling/test/scrambling_test.c
+++ b/lib/src/phy/scrambling/test/scrambling_test.c
@@ -84,8 +84,10 @@ void parse_args(int argc, char **argv) {
 
 int init_sequence(srslte_sequence_t *seq, char *name) {
   if (!strcmp(name, "PBCH")) {
+    bzero(seq, sizeof(srslte_sequence_t));
     return srslte_sequence_pbch(seq, cp, cell_id);
   } else if (!strcmp(name, "PDSCH")) {
+    bzero(seq, sizeof(srslte_sequence_t));
     return srslte_sequence_pdsch(seq, 1234, 0, 0, cell_id, nof_bits);
   } else {
     fprintf(stderr, "Unsupported sequence name %s\n", name);
@@ -109,18 +111,18 @@ int main(int argc, char **argv) {
   }
 
   if (!do_floats) {
-    input_b = malloc(sizeof(uint8_t) * seq.len);
+    input_b = malloc(sizeof(uint8_t) * seq.cur_len);
     if (!input_b) {
       perror("malloc");
       exit(-1);
     }
-    scrambled_b = malloc(sizeof(uint8_t) * seq.len);
+    scrambled_b = malloc(sizeof(uint8_t) * seq.cur_len);
     if (!scrambled_b) {
       perror("malloc");
       exit(-1);
     }
 
-    for (i=0;i<seq.len;i++) {
+    for (i=0;i<seq.cur_len;i++) {
       input_b[i] = rand()%2;
       scrambled_b[i] = input_b[i];
     }
@@ -131,9 +133,9 @@ int main(int argc, char **argv) {
     srslte_scrambling_b(&seq, scrambled_b);
 
     get_time_interval(t);
-    printf("Texec=%ld us for %d bits\n", t[0].tv_usec, seq.len);
+    printf("Texec=%ld us for %d bits\n", t[0].tv_usec, seq.cur_len);
     
-    for (i=0;i<seq.len;i++) {
+    for (i=0;i<seq.cur_len;i++) {
       if (scrambled_b[i] != input_b[i]) {
         printf("Error in %d\n", i);
         exit(-1);
@@ -142,18 +144,18 @@ int main(int argc, char **argv) {
     free(input_b);
     free(scrambled_b);
   } else {
-    input_f = malloc(sizeof(float) * seq.len);
+    input_f = malloc(sizeof(float) * seq.cur_len);
     if (!input_f) {
       perror("malloc");
       exit(-1);
     }
-    scrambled_f = malloc(sizeof(float) * seq.len);
+    scrambled_f = malloc(sizeof(float) * seq.cur_len);
     if (!scrambled_f) {
       perror("malloc");
       exit(-1);
     }
 
-    for (i=0;i<seq.len;i++) {
+    for (i=0;i<seq.cur_len;i++) {
       input_f[i] = 100*(rand()/RAND_MAX);
       scrambled_f[i] = input_f[i];
     }
@@ -164,9 +166,9 @@ int main(int argc, char **argv) {
     srslte_scrambling_f(&seq, scrambled_f);
 
     get_time_interval(t);
-    printf("Texec=%ld us for %d bits\n", t[0].tv_usec, seq.len);
+    printf("Texec=%ld us for %d bits\n", t[0].tv_usec, seq.cur_len);
 
-    for (i=0;i<seq.len;i++) {
+    for (i=0;i<seq.cur_len;i++) {
       if (scrambled_f[i] != input_f[i]) {
         printf("Error in %d\n", i);
         exit(-1);
diff --git a/lib/src/phy/sync/cfo.c b/lib/src/phy/sync/cfo.c
index 456015164..34498ce58 100644
--- a/lib/src/phy/sync/cfo.c
+++ b/lib/src/phy/sync/cfo.c
@@ -27,6 +27,7 @@
 #include <strings.h>
 #include <stdlib.h>
 #include <math.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/utils/cexptab.h"
 #include "srslte/phy/sync/cfo.h"
@@ -47,6 +48,7 @@ int srslte_cfo_init(srslte_cfo_t *h, uint32_t nsamples) {
   h->tol = SRSLTE_CFO_TOLERANCE;
   h->last_freq = 0;
   h->nsamples = nsamples;
+  h->max_samples = nsamples;
   srslte_cexptab_gen(&h->tab, h->cur_cexp, h->last_freq, h->nsamples);
 
   ret = SRSLTE_SUCCESS;
@@ -69,15 +71,15 @@ void srslte_cfo_set_tol(srslte_cfo_t *h, float tol) {
   h->tol = tol;
 }
 
-int srslte_cfo_realloc(srslte_cfo_t *h, uint32_t samples) {
-  h->cur_cexp = realloc(h->cur_cexp, sizeof(cf_t) * samples); 
-  if (!h->cur_cexp) {
-    perror("realloc");
+int srslte_cfo_resize(srslte_cfo_t *h, uint32_t samples) {
+  if (samples <= h->max_samples) {
+    srslte_cexptab_gen(&h->tab, h->cur_cexp, h->last_freq, samples);
+    h->nsamples = samples;
+  } else {
+    fprintf(stderr, "Error in cfo_resize(): nof_samples must be lower than initialized\n");
     return SRSLTE_ERROR;
   }
-  srslte_cexptab_gen(&h->tab, h->cur_cexp, h->last_freq, samples);
-  h->nsamples = samples;
-  
+
   return SRSLTE_SUCCESS;
 }
 
diff --git a/lib/src/phy/sync/cp.c b/lib/src/phy/sync/cp.c
index c745aca85..a282f6dfd 100644
--- a/lib/src/phy/sync/cp.c
+++ b/lib/src/phy/sync/cp.c
@@ -25,6 +25,7 @@
  */
 
 #include <stdlib.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/sync/cp.h"
 #include "srslte/phy/utils/vector.h"
@@ -33,7 +34,8 @@
 int srslte_cp_synch_init(srslte_cp_synch_t *q, uint32_t symbol_sz) 
 {
   q->symbol_sz = symbol_sz;
-  
+  q->max_symbol_sz = symbol_sz;
+
   q->corr = srslte_vec_malloc(sizeof(cf_t) * q->symbol_sz);
   if (!q->corr) {
     perror("malloc");
@@ -49,6 +51,18 @@ void srslte_cp_synch_free(srslte_cp_synch_t *q)
   }
 }
 
+int srslte_cp_synch_resize(srslte_cp_synch_t *q, uint32_t symbol_sz)
+{
+  if (symbol_sz > q->max_symbol_sz) {
+    fprintf(stderr, "Error in cp_synch_resize(): symbol_sz must be lower than initialized\n");
+    return SRSLTE_ERROR;
+  }
+  q->symbol_sz = symbol_sz;
+
+  return SRSLTE_SUCCESS;
+}
+
+
 uint32_t srslte_cp_synch(srslte_cp_synch_t *q, cf_t *input, uint32_t max_offset, uint32_t nof_symbols, uint32_t cp_len)
 {  
   if (max_offset > q->symbol_sz) {    
diff --git a/lib/src/phy/sync/pss.c b/lib/src/phy/sync/pss.c
index 2ad166bc0..f7b35071d 100644
--- a/lib/src/phy/sync/pss.c
+++ b/lib/src/phy/sync/pss.c
@@ -30,6 +30,7 @@
 #include <stdlib.h>
 #include <complex.h>
 #include <math.h>
+#include <srslte/phy/sync/pss.h>
 
 #include "srslte/phy/sync/pss.h"
 #include "srslte/phy/dft/dft.h"
@@ -93,9 +94,11 @@ int srslte_pss_synch_init_fft_offset(srslte_pss_synch_t *q, uint32_t frame_size,
  * It correlates a signal of frame_size samples with the PSS sequence in the frequency 
  * domain. The PSS sequence is transformed using fft_size samples. 
  */
-int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q, uint32_t frame_size, uint32_t fft_size, int offset, int decimate) {
+int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q,
+                                           uint32_t max_frame_size, uint32_t max_fft_size,
+                                           int offset, int decimate)
+{
 
-    
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   if (q != NULL) {
   
@@ -107,18 +110,20 @@ int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q, uint32_t frame
     
     q->N_id_2 = 10;  
     q->ema_alpha = 0.2;
-    
+
+    q->max_fft_size  = max_fft_size;
+    q->max_frame_size = max_frame_size;
+
     q->decimate = decimate;
-    fft_size = fft_size/q->decimate;
-    frame_size = frame_size/q->decimate;
+    uint32_t fft_size = max_fft_size/q->decimate;
+    uint32_t frame_size = max_frame_size/q->decimate;
     
     q->fft_size = fft_size;
     q->frame_size = frame_size;
 
     buffer_size = fft_size + frame_size + 1;
-    
-    if(q->decimate > 1)
-    {
+
+    if(q->decimate > 1) {
         int filter_order = 3;
         srslte_filt_decim_cc_init(&q->filter,q->decimate,filter_order);
         q->filter.filter_output = srslte_vec_malloc((buffer_size) * sizeof(cf_t));
@@ -175,21 +180,19 @@ int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q, uint32_t frame
         goto clean_and_exit;
       }      
       bzero(&q->pss_signal_time[N_id_2][q->fft_size], q->frame_size * sizeof(cf_t));
-
     }    
     #ifdef CONVOLUTION_FFT
 
 
-    for(N_id_2 = 0; N_id_2<3; N_id_2++)
+    for(N_id_2=0; N_id_2<3; N_id_2++)
     q->pss_signal_freq_full[N_id_2]   = srslte_vec_malloc(buffer_size * sizeof(cf_t));
 
     if (srslte_conv_fft_cc_init(&q->conv_fft, frame_size, fft_size)) {
       fprintf(stderr, "Error initiating convolution FFT\n");
       goto clean_and_exit;
     }
-    for(int i =0; i< 3; i++)
-    {
-        srslte_dft_run_c(&q->conv_fft.filter_plan, q->pss_signal_time[i], q->pss_signal_freq_full[i]);
+    for(int i=0; i<3; i++) {
+      srslte_dft_run_c(&q->conv_fft.filter_plan, q->pss_signal_time[i], q->pss_signal_freq_full[i]);
     }
     
     #endif
@@ -208,6 +211,79 @@ clean_and_exit:
 }
 
 
+/* Initializes the PSS synchronization object.
+ *
+ * It correlates a signal of frame_size samples with the PSS sequence in the frequency
+ * domain. The PSS sequence is transformed using fft_size samples.
+ */
+int srslte_pss_synch_resize(srslte_pss_synch_t *q, uint32_t frame_size, uint32_t fft_size, int offset) {
+
+
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+  if (q != NULL) {
+
+    ret = SRSLTE_ERROR;
+
+    if (fft_size > q->max_fft_size || frame_size > q->max_frame_size) {
+      fprintf(stderr, "Error in pss_synch_config(): fft_size and frame_size must be lower than initialized\n");
+      return SRSLTE_ERROR;
+    }
+
+    uint32_t N_id_2;
+    uint32_t buffer_size;
+
+    q->N_id_2 = 10;
+    q->ema_alpha = 0.2;
+
+    fft_size = fft_size/q->decimate;
+    frame_size = frame_size/q->decimate;
+
+    q->fft_size = fft_size;
+    q->frame_size = frame_size;
+
+    buffer_size = fft_size + frame_size + 1;
+
+    if (srslte_dft_replan(&q->dftp_input, fft_size)) {
+      fprintf(stderr, "Error creating DFT plan \n");
+      return SRSLTE_ERROR;
+    }
+
+    bzero(&q->tmp_input[q->frame_size], q->fft_size * sizeof(cf_t));
+    bzero(q->conv_output, sizeof(cf_t) * buffer_size);
+    bzero(q->conv_output_avg, sizeof(float) * buffer_size);
+
+#ifdef SRSLTE_PSS_ACCUMULATE_ABS
+    bzero(q->conv_output_abs, sizeof(float) * buffer_size);
+#endif
+
+    // Generate PSS sequences for this FFT size
+    for (N_id_2=0;N_id_2<3;N_id_2++) {
+      if (srslte_pss_synch_init_N_id_2(q->pss_signal_freq[N_id_2], q->pss_signal_time[N_id_2], N_id_2, fft_size, offset)) {
+        fprintf(stderr, "Error initiating PSS detector for N_id_2=%d fft_size=%d\n", N_id_2, fft_size);
+        return SRSLTE_ERROR;
+      }
+      bzero(&q->pss_signal_time[N_id_2][q->fft_size], q->frame_size * sizeof(cf_t));
+    }
+#ifdef CONVOLUTION_FFT
+
+    if (srslte_conv_fft_cc_replan(&q->conv_fft, frame_size, fft_size)) {
+      fprintf(stderr, "Error initiating convolution FFT\n");
+      return SRSLTE_ERROR;
+    }
+    for(int i =0; i< 3; i++) {
+      srslte_dft_run_c(&q->conv_fft.filter_plan, q->pss_signal_time[i], q->pss_signal_freq_full[i]);
+    }
+
+#endif
+
+    srslte_pss_synch_reset(q);
+
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+
+}
+
 void srslte_pss_synch_free(srslte_pss_synch_t *q) {
   uint32_t i;
 
diff --git a/lib/src/phy/sync/sss.c b/lib/src/phy/sync/sss.c
index 3586e04de..4ed7b1d6c 100644
--- a/lib/src/phy/sync/sss.c
+++ b/lib/src/phy/sync/sss.c
@@ -58,7 +58,8 @@ int srslte_sss_synch_init(srslte_sss_synch_t *q, uint32_t fft_size) {
     srslte_dft_plan_set_dc(&q->dftp_input, true);
 
     q->fft_size = fft_size; 
-    
+    q->max_fft_size = fft_size;
+
     generate_N_id_1_table(q->N_id_1_table);
     
     for (N_id_2=0;N_id_2<3;N_id_2++) {
@@ -71,19 +72,18 @@ int srslte_sss_synch_init(srslte_sss_synch_t *q, uint32_t fft_size) {
   return SRSLTE_ERROR_INVALID_INPUTS;
 }
 
-int srslte_sss_synch_realloc(srslte_sss_synch_t *q, uint32_t fft_size) {
+int srslte_sss_synch_resize(srslte_sss_synch_t *q, uint32_t fft_size) {
   if (q                 != NULL  &&
       fft_size          <= 2048)
   {
-    srslte_dft_plan_free(&q->dftp_input);
-    if (srslte_dft_plan(&q->dftp_input, fft_size, SRSLTE_DFT_FORWARD, SRSLTE_DFT_COMPLEX)) {
+    if (fft_size > q->max_fft_size) {
+      fprintf(stderr, "Error in sss_synch_resize(): fft_size must be lower than initialized\n");
+      return SRSLTE_ERROR;
+    }
+    if (srslte_dft_replan(&q->dftp_input, fft_size)) {
       srslte_sss_synch_free(q);
       return SRSLTE_ERROR;
     }
-    srslte_dft_plan_set_mirror(&q->dftp_input, true);
-    srslte_dft_plan_set_norm(&q->dftp_input, true);
-    srslte_dft_plan_set_dc(&q->dftp_input, true);
-    
     q->fft_size = fft_size;
     return SRSLTE_SUCCESS;
   }
diff --git a/lib/src/phy/sync/sync.c b/lib/src/phy/sync/sync.c
index fb77e1b25..ee2ba76d1 100644
--- a/lib/src/phy/sync/sync.c
+++ b/lib/src/phy/sync/sync.c
@@ -28,6 +28,8 @@
 #include <strings.h>
 #include <complex.h>
 #include <math.h>
+#include <srslte/srslte.h>
+#include <srslte/phy/sync/sync.h>
 
 #include "srslte/phy/utils/debug.h"
 #include "srslte/phy/common/phy_common.h"
@@ -39,6 +41,8 @@
 #define CFO_EMA_ALPHA           0.1
 #define CP_EMA_ALPHA            0.1
 
+#define DEFAULT_CFO_TOL         50.0 // Hz
+
 static bool fft_size_isvalid(uint32_t fft_size) {
   if (fft_size >= SRSLTE_SYNC_FFT_SZ_MIN && fft_size <= SRSLTE_SYNC_FFT_SZ_MAX && (fft_size%64) == 0) {
     return true;
@@ -77,7 +81,10 @@ int srslte_sync_init_decim(srslte_sync_t *q, uint32_t frame_size, uint32_t max_o
     q->frame_size = frame_size;
     q->max_offset = max_offset;
     q->sss_alg = SSS_FULL;
-        
+    q->max_frame_size = frame_size;
+    q->mean_cfo_isunset = true;
+    q->mean_cfo2_isunset = true;
+
     q->enable_cfo_corr = true; 
     if (srslte_cfo_init(&q->cfocorr, q->frame_size)) {
       fprintf(stderr, "Error initiating CFO\n");
@@ -89,11 +96,8 @@ int srslte_sync_init_decim(srslte_sync_t *q, uint32_t frame_size, uint32_t max_o
       goto clean_exit;
     }
     
-    // Set a CFO tolerance of approx 50 Hz
-    srslte_cfo_set_tol(&q->cfocorr, 50.0/(15000.0*q->fft_size));
-
-    // Set a CFO tolerance of approx 50 Hz
-    srslte_cfo_set_tol(&q->cfocorr2, 50.0/(15000.0*q->fft_size));
+    // Set default CFO tolerance
+    srslte_sync_set_cfo_tol(q, DEFAULT_CFO_TOL);
 
     for (int i=0;i<2;i++) {
       q->cfo_i_corr[i] = srslte_vec_malloc(sizeof(cf_t)*q->frame_size);
@@ -111,11 +115,11 @@ int srslte_sync_init_decim(srslte_sync_t *q, uint32_t frame_size, uint32_t max_o
     
     srslte_sync_set_cp(q, SRSLTE_CP_NORM);
     q->decimate = decimate;
-    if(!decimate)
+    if(!decimate) {
       decimate = 1;
-                 
-    
-    if (srslte_pss_synch_init_fft_offset_decim(&q->pss, max_offset, fft_size,0,decimate)) {
+    }
+
+    if (srslte_pss_synch_init_fft_offset_decim(&q->pss, max_offset, fft_size, 0, decimate)) {
       fprintf(stderr, "Error initializing PSS object\n");
       goto clean_exit;
     }
@@ -162,6 +166,80 @@ void srslte_sync_free(srslte_sync_t *q) {
   }
 }
 
+int srslte_sync_resize(srslte_sync_t *q, uint32_t frame_size, uint32_t max_offset, uint32_t fft_size) {
+
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q                 != NULL         &&
+      frame_size        <= 307200       &&
+      fft_size_isvalid(fft_size))
+  {
+    ret = SRSLTE_ERROR;
+
+    if (frame_size > q->max_frame_size) {
+      fprintf(stderr, "Error in sync_resize(): frame_size must be lower than initialized\n");
+      return SRSLTE_ERROR;
+    }
+    q->detect_cp = true;
+    q->sss_en = true;
+    q->mean_cfo = 0;
+    q->mean_cfo2 = 0;
+    q->N_id_2 = 1000;
+    q->N_id_1 = 1000;
+    q->cfo_i = 0;
+    q->find_cfo_i = false;
+    q->find_cfo_i_initiated = false;
+    q->cfo_ema_alpha = CFO_EMA_ALPHA;
+    q->fft_size = fft_size;
+    q->frame_size = frame_size;
+    q->max_offset = max_offset;
+    q->sss_alg = SSS_FULL;
+
+    q->enable_cfo_corr = true;
+
+    if (srslte_pss_synch_resize(&q->pss, max_offset, fft_size, 0)) {
+      fprintf(stderr, "Error resizing PSS object\n");
+      return SRSLTE_ERROR;
+    }
+    if (srslte_sss_synch_resize(&q->sss, fft_size)) {
+      fprintf(stderr, "Error resizing SSS object\n");
+      return SRSLTE_ERROR;
+    }
+
+    if (srslte_cp_synch_resize(&q->cp_synch, fft_size)) {
+      fprintf(stderr, "Error resizing CFO\n");
+      return SRSLTE_ERROR;
+    }
+
+    if (srslte_cfo_resize(&q->cfocorr, q->frame_size)) {
+      fprintf(stderr, "Error resizing CFO\n");
+      return SRSLTE_ERROR;
+    }
+
+    if (srslte_cfo_resize(&q->cfocorr2, q->frame_size)) {
+      fprintf(stderr, "Error resizing CFO\n");
+      return SRSLTE_ERROR;
+    }
+
+    // Update CFO tolerance
+    srslte_sync_set_cfo_tol(q, q->current_cfo_tol);
+
+    DEBUG("SYNC init with frame_size=%d, max_offset=%d and fft_size=%d\n", frame_size, max_offset, fft_size);
+
+    ret = SRSLTE_SUCCESS;
+  }  else {
+    fprintf(stderr, "Invalid parameters frame_size: %d, fft_size: %d\n", frame_size, fft_size);
+  }
+
+  return ret;
+}
+
+void srslte_sync_set_cfo_tol(srslte_sync_t *q, float tol) {
+  q->current_cfo_tol = tol;
+  srslte_cfo_set_tol(&q->cfocorr, tol/(15000.0*q->fft_size));
+  srslte_cfo_set_tol(&q->cfocorr2, tol/(15000.0*q->fft_size));
+}
+
 void srslte_sync_set_threshold(srslte_sync_t *q, float threshold) {
   q->threshold = threshold;
 }
@@ -217,7 +295,10 @@ float srslte_sync_get_cfo(srslte_sync_t *q) {
 }
 
 void srslte_sync_set_cfo(srslte_sync_t *q, float cfo) {
-  q->mean_cfo = cfo;
+  q->mean_cfo  = cfo;
+  q->mean_cfo2 = cfo;
+  q->mean_cfo2_isunset = false;
+  q->mean_cfo_isunset  = false;
 }
 
 void srslte_sync_set_cfo_i(srslte_sync_t *q, int cfo_i) {
@@ -445,13 +526,18 @@ srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, cf_t *input, uint32_t
     cf_t *input_cfo = input; 
 
     if (q->enable_cfo_corr) {
-      float cfo = cfo_estimate(q, input); 
-      
-      /* compute exponential moving average CFO */
-      q->mean_cfo = SRSLTE_VEC_EMA(cfo, q->mean_cfo, q->cfo_ema_alpha);
-      
+      float cfo = cfo_estimate(q, input);
+
+      if (q->mean_cfo_isunset) {
+        q->mean_cfo = cfo;
+        q->mean_cfo_isunset = false;
+      } else {
+        /* compute exponential moving average CFO */
+        q->mean_cfo = SRSLTE_VEC_EMA(cfo, q->mean_cfo, q->cfo_ema_alpha);
+      }
+
       /* Correct CFO with the averaged CFO estimation */
-      srslte_cfo_correct(&q->cfocorr2, input, q->temp, -q->mean_cfo / q->fft_size);      
+      srslte_cfo_correct(&q->cfocorr2, input, q->temp, -q->mean_cfo / q->fft_size);
       
       input_cfo = q->temp; 
     }
@@ -504,8 +590,13 @@ srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, cf_t *input, uint32_t
   
       if (peak_pos + find_offset >= 2*(q->fft_size + SRSLTE_CP_LEN_EXT(q->fft_size))) {
         float cfo2   = srslte_pss_synch_cfo_compute(&q->pss, &input[find_offset + peak_pos - q->fft_size]);
-        q->mean_cfo2 = SRSLTE_VEC_EMA(cfo2, q->mean_cfo2, q->cfo_ema_alpha);
-        
+        if (q->mean_cfo2_isunset) {
+          q->mean_cfo2 = cfo2;
+          q->mean_cfo2_isunset = true;
+        } else {
+          q->mean_cfo2 = SRSLTE_VEC_EMA(cfo2, q->mean_cfo2, q->cfo_ema_alpha);
+        }
+
         ret = SRSLTE_SYNC_FOUND;
       } else {
         ret = SRSLTE_SYNC_FOUND_NOSPACE; 
@@ -526,7 +617,7 @@ srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, cf_t *input, uint32_t
 }
 
 void srslte_sync_reset(srslte_sync_t *q) {
-  q->M_ext_avg = 0; 
+  q->M_ext_avg  = 0;
   q->M_norm_avg = 0; 
   srslte_pss_synch_reset(&q->pss);
 }
diff --git a/lib/src/phy/ue/ue_cell_search.c b/lib/src/phy/ue/ue_cell_search.c
index f7d244387..51b0858bb 100644
--- a/lib/src/phy/ue/ue_cell_search.c
+++ b/lib/src/phy/ue/ue_cell_search.c
@@ -29,6 +29,7 @@
 #include <strings.h>
 #include <assert.h>
 #include <unistd.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/ue/ue_cell_search.h"
 
@@ -51,11 +52,16 @@ int srslte_ue_cellsearch_init(srslte_ue_cellsearch_t * q, uint32_t max_frames,
     cell.id = SRSLTE_CELL_ID_UNKNOWN; 
     cell.nof_prb = SRSLTE_CS_NOF_PRB; 
 
-    if (srslte_ue_sync_init(&q->ue_sync, cell, recv_callback, stream_handler)) {
+    if (srslte_ue_sync_init(&q->ue_sync, cell.nof_prb, true, recv_callback, stream_handler)) {
       fprintf(stderr, "Error initiating ue_sync\n");
       goto clean_exit; 
     }
-    
+
+    if (srslte_ue_sync_set_cell(&q->ue_sync, cell)) {
+      fprintf(stderr, "Error initiating ue_sync\n");
+      goto clean_exit;
+    }
+
     q->sf_buffer[0] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(100));
     q->nof_rx_antennas = 1; 
     
@@ -105,11 +111,15 @@ int srslte_ue_cellsearch_init_multi(srslte_ue_cellsearch_t * q, uint32_t max_fra
     cell.id = SRSLTE_CELL_ID_UNKNOWN; 
     cell.nof_prb = SRSLTE_CS_NOF_PRB; 
 
-    if (srslte_ue_sync_init_multi(&q->ue_sync, cell, recv_callback, nof_rx_antennas, stream_handler)) {
+    if (srslte_ue_sync_init_multi(&q->ue_sync, cell.nof_prb, true, recv_callback, nof_rx_antennas, stream_handler)) {
       fprintf(stderr, "Error initiating ue_sync\n");
       goto clean_exit; 
     }
-    
+    if (srslte_ue_sync_set_cell(&q->ue_sync, cell)) {
+      fprintf(stderr, "Error initiating ue_sync\n");
+      goto clean_exit;
+    }
+
     for (int i=0;i<nof_rx_antennas;i++) {
       q->sf_buffer[i] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(100));
     }
@@ -242,7 +252,8 @@ int srslte_ue_cellsearch_scan(srslte_ue_cellsearch_t * q,
 {
   int ret = 0; 
   float max_peak_value = -1.0;
-  uint32_t nof_detected_cells = 0; 
+  uint32_t nof_detected_cells = 0;
+
   for (uint32_t N_id_2=0;N_id_2<3 && ret >= 0;N_id_2++) {
     ret = srslte_ue_cellsearch_scan_N_id_2(q, N_id_2, &found_cells[N_id_2]);
     if (ret < 0) {
@@ -273,8 +284,12 @@ int srslte_ue_cellsearch_scan_N_id_2(srslte_ue_cellsearch_t * q,
 
   if (q != NULL) 
   {
-    ret = SRSLTE_SUCCESS; 
-    
+    ret = SRSLTE_SUCCESS;
+
+    bzero(q->candidates, sizeof(srslte_ue_cellsearch_result_t)*q->max_frames);
+    bzero(q->mode_ntimes, sizeof(uint32_t)*q->max_frames);
+    bzero(q->mode_counted, sizeof(uint8_t)*q->max_frames);
+
     srslte_ue_sync_set_N_id_2(&q->ue_sync, N_id_2);
     srslte_ue_sync_reset(&q->ue_sync);
     do {
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 888b0d9b3..46dfea566 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -27,6 +27,7 @@
 #include "srslte/phy/ue/ue_dl.h"
 
 #include <string.h>
+#include <srslte/srslte.h>
 
 
 #define CURRENT_FFTSIZE   srslte_symbol_sz(q->cell.nof_prb)
@@ -35,6 +36,7 @@
 #define CURRENT_SLOTLEN_RE SRSLTE_SLOT_LEN_RE(q->cell.nof_prb, q->cell.cp)
 #define CURRENT_SFLEN_RE SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)
 
+#define MAX_SFLEN_RE SRSLTE_SF_LEN_RE(max_prb, q->cell.cp)
 
 const static srslte_dci_format_t ue_dci_formats[8][2] = {
     /* Mode 1 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1},
@@ -51,57 +53,50 @@ static srslte_dci_format_t common_formats[] = {SRSLTE_DCI_FORMAT1A,SRSLTE_DCI_FO
 const uint32_t nof_common_formats = 2; 
 
 int srslte_ue_dl_init(srslte_ue_dl_t *q,
-                      srslte_cell_t cell,
+                      uint32_t max_prb,
                       uint32_t nof_rx_antennas)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS; 
   
   if (q               != NULL             &&
-      nof_rx_antennas <= SRSLTE_MAX_PORTS &&
-      srslte_cell_isvalid(&cell))   
+      nof_rx_antennas <= SRSLTE_MAX_PORTS)
   {
     ret = SRSLTE_ERROR;
     
     bzero(q, sizeof(srslte_ue_dl_t));
     
-    q->cell = cell; 
     q->pkt_errors = 0;
     q->pkts_total = 0;
     q->pending_ul_dci_rnti = 0; 
     q->sample_offset = 0; 
-    q->nof_rx_antennas = nof_rx_antennas; 
-    
-    if (srslte_ofdm_rx_init(&q->fft, q->cell.cp, q->cell.nof_prb)) {
+    q->nof_rx_antennas = nof_rx_antennas;
+
+    if (srslte_ofdm_rx_init(&q->fft, SRSLTE_CP_NORM, max_prb)) {
       fprintf(stderr, "Error initiating FFT\n");
       goto clean_exit;
     }
-    if (srslte_chest_dl_init(&q->chest, cell)) {
+    if (srslte_chest_dl_init(&q->chest, max_prb)) {
       fprintf(stderr, "Error initiating channel estimator\n");
       goto clean_exit;
     }
-    if (srslte_regs_init(&q->regs, q->cell)) {
-      fprintf(stderr, "Error initiating REGs\n");
-      goto clean_exit;
-    }
-    if (srslte_pcfich_init_multi(&q->pcfich, &q->regs, q->cell, nof_rx_antennas)) {
+    if (srslte_pcfich_init(&q->pcfich, nof_rx_antennas)) {
       fprintf(stderr, "Error creating PCFICH object\n");
       goto clean_exit;
     }
-    if (srslte_phich_init(&q->phich, &q->regs, q->cell)) {
+    if (srslte_phich_init(&q->phich, nof_rx_antennas)) {
       fprintf(stderr, "Error creating PHICH object\n");
       goto clean_exit;
     }
 
-    if (srslte_pdcch_init_multi(&q->pdcch, &q->regs, q->cell, nof_rx_antennas)) {
+    if (srslte_pdcch_init_ue(&q->pdcch, max_prb, nof_rx_antennas)) {
       fprintf(stderr, "Error creating PDCCH object\n");
       goto clean_exit;
     }
 
-    if (srslte_pdsch_init_rx(&q->pdsch, q->cell, nof_rx_antennas)) {
+    if (srslte_pdsch_init_ue(&q->pdsch, max_prb, nof_rx_antennas)) {
       fprintf(stderr, "Error creating PDSCH object\n");
       goto clean_exit;
     }
-
     for (int i = 0; i < SRSLTE_MAX_TB; i++) {
       q->softbuffers[i] = srslte_vec_malloc(sizeof(srslte_softbuffer_rx_t));
       if (!q->softbuffers[i]) {
@@ -109,25 +104,25 @@ int srslte_ue_dl_init(srslte_ue_dl_t *q,
         goto clean_exit;
       }
 
-      if (srslte_softbuffer_rx_init(q->softbuffers[i], q->cell.nof_prb)) {
+      if (srslte_softbuffer_rx_init(q->softbuffers[i], max_prb)) {
         fprintf(stderr, "Error initiating soft buffer\n");
         goto clean_exit;
       }
     }
-    if (srslte_cfo_init(&q->sfo_correct, q->cell.nof_prb*SRSLTE_NRE)) {
+    if (srslte_cfo_init(&q->sfo_correct, max_prb*SRSLTE_NRE)) {
       fprintf(stderr, "Error initiating SFO correct\n");
       goto clean_exit;
     }
     srslte_cfo_set_tol(&q->sfo_correct, 1e-5f/q->fft.symbol_sz);
     
     for (int j=0;j<nof_rx_antennas;j++) {
-      q->sf_symbols_m[j] = srslte_vec_malloc(CURRENT_SFLEN_RE * sizeof(cf_t));
+      q->sf_symbols_m[j] = srslte_vec_malloc(MAX_SFLEN_RE * sizeof(cf_t));
       if (!q->sf_symbols_m[j]) {
         perror("malloc");
         goto clean_exit; 
       }
-      for (uint32_t i=0;i<q->cell.nof_ports;i++) {
-        q->ce_m[i][j] = srslte_vec_malloc(CURRENT_SFLEN_RE * sizeof(cf_t));
+      for (uint32_t i=0;i<SRSLTE_MAX_PORTS;i++) {
+        q->ce_m[i][j] = srslte_vec_malloc(MAX_SFLEN_RE * sizeof(cf_t));
         if (!q->ce_m[i][j]) {
           perror("malloc");
           goto clean_exit; 
@@ -136,14 +131,13 @@ int srslte_ue_dl_init(srslte_ue_dl_t *q,
     }
     
     q->sf_symbols = q->sf_symbols_m[0];
-    for (int i=0;i<q->cell.nof_ports;i++) {
+    for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
       q->ce[i] = q->ce_m[i][0];
     }
     
     ret = SRSLTE_SUCCESS;
   } else {
-    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
-            cell.id, cell.nof_ports, cell.nof_prb);      
+    fprintf(stderr, "Invalid parametres\n");
   }
 
 clean_exit: 
@@ -173,7 +167,7 @@ void srslte_ue_dl_free(srslte_ue_dl_t *q) {
       if (q->sf_symbols_m[j]) {
         free(q->sf_symbols_m[j]);
       }
-      for (uint32_t i=0;i<q->cell.nof_ports;i++) {
+      for (uint32_t i=0;i<SRSLTE_MAX_PORTS;i++) {
         if (q->ce_m[i][j]) {
           free(q->ce_m[i][j]);
         }
@@ -183,11 +177,74 @@ void srslte_ue_dl_free(srslte_ue_dl_t *q) {
   }
 }
 
+int srslte_ue_dl_set_cell(srslte_ue_dl_t *q, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q               != NULL             &&
+      srslte_cell_isvalid(&cell))
+  {
+    q->pkt_errors = 0;
+    q->pkts_total = 0;
+    q->pending_ul_dci_rnti = 0;
+    q->sample_offset = 0;
+
+    if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
+      if (q->cell.nof_prb != 0) {
+        srslte_regs_free(&q->regs);
+      }
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+      if (srslte_regs_init(&q->regs, q->cell)) {
+        fprintf(stderr, "Error resizing REGs\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_cfo_resize(&q->sfo_correct, q->cell.nof_prb*SRSLTE_NRE)) {
+        fprintf(stderr, "Error resizing SFO correct\n");
+        return SRSLTE_ERROR;
+      }
+      srslte_cfo_set_tol(&q->sfo_correct, 1e-5/q->fft.symbol_sz);
+      if (srslte_ofdm_rx_set_prb(&q->fft, q->cell.cp, q->cell.nof_prb)) {
+        fprintf(stderr, "Error resizing FFT\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_chest_dl_set_cell(&q->chest, q->cell)) {
+        fprintf(stderr, "Error resizing channel estimator\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_pcfich_set_cell(&q->pcfich, &q->regs, q->cell)) {
+        fprintf(stderr, "Error resizing PCFICH object\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_phich_set_cell(&q->phich, &q->regs, q->cell)) {
+        fprintf(stderr, "Error resizing PHICH object\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pdcch_set_cell(&q->pdcch, &q->regs, q->cell)) {
+        fprintf(stderr, "Error resizing PDCCH object\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pdsch_set_cell(&q->pdsch, q->cell)) {
+        fprintf(stderr, "Error creating PDSCH object\n");
+        return SRSLTE_ERROR;
+      }
+      q->current_rnti = 0;
+    }
+    ret = SRSLTE_SUCCESS;
+  } else {
+    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
+            cell.id, cell.nof_ports, cell.nof_prb);
+  }
+  return ret;
+}
+
 /* Precalculate the PDSCH scramble sequences for a given RNTI. This function takes a while 
  * to execute, so shall be called once the final C-RNTI has been allocated for the session.
  * For the connection procedure, use srslte_pusch_encode_rnti() or srslte_pusch_decode_rnti() functions 
  */
 void srslte_ue_dl_set_rnti(srslte_ue_dl_t *q, uint16_t rnti) {
+
   srslte_pdsch_set_rnti(&q->pdsch, rnti);
   
   // Compute UE-specific and Common search space for this RNTI
@@ -235,7 +292,7 @@ int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_P
       /* Correct SFO multiplying by complex exponential in the time domain */
       if (q->sample_offset) {
         for (int i=0;i<2*SRSLTE_CP_NSYMB(q->cell.cp);i++) {
-          srslte_cfo_correct(&q->sfo_correct, 
+          srslte_cfo_correct(&q->sfo_correct,
                           &q->sf_symbols_m[j][i*q->cell.nof_prb*SRSLTE_NRE], 
                           &q->sf_symbols_m[j][i*q->cell.nof_prb*SRSLTE_NRE], 
                           q->sample_offset / q->fft.symbol_sz);
@@ -280,21 +337,22 @@ int srslte_ue_dl_decode_estimate(srslte_ue_dl_t *q, uint32_t sf_idx, uint32_t *c
 int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx,
                                  int rvidx[SRSLTE_MAX_CODEWORDS], srslte_mimo_type_t mimo_type) {
   uint32_t pmi = 0;
+  uint32_t nof_tb = SRSLTE_RA_DL_GRANT_NOF_TB(grant);
 
   /* Translates Precoding Information (pinfo) to Precoding matrix Index (pmi) as 3GPP 36.212 Table 5.3.3.1.5-4 */
   if (mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
-    if (grant->nof_tb == 1) {
+    if (nof_tb == 1) {
       if (grant->pinfo > 0 && grant->pinfo < 5) {
         pmi = grant->pinfo - 1;
       } else {
-        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", q->pdsch_cfg.grant.nof_tb, grant->pinfo);
+        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", nof_tb, grant->pinfo);
         return SRSLTE_ERROR;
       }
     } else {
       if (grant->pinfo < 2) {
         pmi = grant->pinfo;
       } else {
-        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", q->pdsch_cfg.grant.nof_tb, grant->pinfo);
+        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", nof_tb, grant->pinfo);
         return SRSLTE_ERROR;
       }
     }
@@ -336,29 +394,33 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
     
     /* ===== These lines of code are supposed to be MAC functionality === */
 
-    
+
     int rvidx[SRSLTE_MAX_CODEWORDS] = {1};
     if (dci_unpacked.rv_idx < 0) {
-      uint32_t sfn = tti/10; 
-      uint32_t k   = (sfn/2)%4;
-      for (int i = 0; i < grant.nof_tb; i++) {
-        rvidx[i] = ((uint32_t) ceilf((float) 1.5 * k)) % 4;
-        srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
+      uint32_t sfn = tti / 10;
+      uint32_t k = (sfn / 2) % 4;
+      for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+        if (grant.tb_en[i]) {
+          rvidx[i] = ((uint32_t) ceilf((float) 1.5 * k)) % 4;
+          srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
+        }
       }
     } else {
-      for (int i = 0; i < grant.nof_tb; i++) {
-        switch(i) {
-          case 0:
-            rvidx[i] = (uint32_t) dci_unpacked.rv_idx;
-            break;
-          case 1:
-            rvidx[i] = (uint32_t) dci_unpacked.rv_idx_1;
-            break;
-          default:
-            ERROR("Wrong number of transport blocks");
-            return SRSLTE_ERROR;
+      for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+        if (grant.tb_en[i]) {
+          switch (i) {
+            case 0:
+              rvidx[i] = (uint32_t) dci_unpacked.rv_idx;
+              break;
+            case 1:
+              rvidx[i] = (uint32_t) dci_unpacked.rv_idx_1;
+              break;
+            default:
+              ERROR("Wrong number of transport blocks");
+              return SRSLTE_ERROR;
+          }
+          srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
         }
-        srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
       }
     }
 
@@ -372,14 +434,14 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
         }
         break;
       case SRSLTE_DCI_FORMAT2:
-        if (grant.nof_tb == 1 && dci_unpacked.pinfo == 0) {
+        if (SRSLTE_RA_DL_GRANT_NOF_TB(&grant) == 1 && dci_unpacked.pinfo == 0) {
           mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
         } else {
           mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
         }
         break;
       case SRSLTE_DCI_FORMAT2A:
-        if (grant.nof_tb == 1 && dci_unpacked.pinfo == 0) {
+        if (SRSLTE_RA_DL_GRANT_NOF_TB(&grant) == 1 && dci_unpacked.pinfo == 0) {
           mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
         } else {
           mimo_type = SRSLTE_MIMO_TYPE_CDD;
@@ -413,11 +475,13 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
                                 noise_estimate,
                                 rnti, data, acks);
 
-      for (int tb = 0; tb < q->pdsch_cfg.grant.nof_tb; tb++) {
-        if (!acks[tb]) {
-          q->pkt_errors++;
+      for (int tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+        if (grant.tb_en[tb]) {
+          if (!acks[tb]) {
+            q->pkt_errors++;
+          }
+          q->pkts_total++;
         }
-        q->pkts_total++;
       }
 
       if (ret == SRSLTE_ERROR) {
@@ -447,10 +511,10 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
 
 /* Compute the Rank Indicator (RI) and Precoder Matrix Indicator (PMI) by computing the Signal to Interference plus
  * Noise Ratio (SINR), valid for TM4 */
-int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, float *current_sinr) {
+int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint8_t *ri, uint8_t *pmi, float *current_sinr) {
   float noise_estimate = srslte_chest_dl_get_noise_estimate(&q->chest);
   float best_sinr = -INFINITY;
-  uint32_t best_pmi = 0, best_ri = 0;
+  uint8_t best_pmi = 0, best_ri = 0;
 
   if (q->cell.nof_ports < 2 || q->nof_rx_antennas < 2) {
     /* Do nothing */
@@ -464,11 +528,11 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, f
 
     /* Select the best Rank indicator (RI) and Precoding Matrix Indicator (PMI) */
     for (uint32_t nof_layers = 1; nof_layers <= 2; nof_layers++) {
-      float _sinr = q->sinr[nof_layers - 1][q->pmi[nof_layers - 1]] * nof_layers;
+      float _sinr = q->sinr[nof_layers - 1][q->pmi[nof_layers - 1]] * nof_layers * nof_layers;
       if (_sinr > best_sinr + 0.1) {
         best_sinr = _sinr;
-        best_pmi = q->pmi[nof_layers - 1];
-        best_ri = nof_layers;
+        best_pmi = (uint8_t) q->pmi[nof_layers - 1];
+        best_ri = (uint8_t) (nof_layers - 1);
       }
     }
 
@@ -509,7 +573,7 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, f
 
 
 /* Compute the Rank Indicator (RI) by computing the condition number, valid for TM3 */
-int srslte_ue_dl_ri_select(srslte_ue_dl_t *q, uint32_t *ri, float *cn) {
+int srslte_ue_dl_ri_select(srslte_ue_dl_t *q, uint8_t *ri, float *cn) {
   float _cn;
   int ret = srslte_pdsch_cn_compute(&q->pdsch, q->ce_m, SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp), &_cn);
 
@@ -520,7 +584,7 @@ int srslte_ue_dl_ri_select(srslte_ue_dl_t *q, uint32_t *ri, float *cn) {
 
   /* Set rank indicator */
   if (!ret && ri) {
-    *ri = (_cn > 3.0f)? 1:0;
+    *ri = (uint8_t)((_cn < 17.0f)? 1:0);
   }
 
   return ret;
@@ -732,11 +796,11 @@ bool srslte_ue_dl_decode_phich(srslte_ue_dl_t *q, uint32_t sf_idx, uint32_t n_pr
 }
 
 void srslte_ue_dl_save_signal(srslte_ue_dl_t *q, srslte_softbuffer_rx_t *softbuffer, uint32_t tti, uint32_t rv_idx, uint16_t rnti, uint32_t cfi) {
-  srslte_vec_save_file("sf_symbols", q->sf_symbols_m, SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+  srslte_vec_save_file("sf_symbols", q->sf_symbols, SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
   printf("%d samples\n", SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp));
-  srslte_vec_save_file("ce0", q->ce_m[0], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+  srslte_vec_save_file("ce0", q->ce[0], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
   if (q->cell.nof_ports > 1) {
-    srslte_vec_save_file("ce1", q->ce_m[1], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+    srslte_vec_save_file("ce1", q->ce[1], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
   }
   srslte_vec_save_file("pcfich_ce0", q->pcfich.ce[0], q->pcfich.nof_symbols*sizeof(cf_t));
   srslte_vec_save_file("pcfich_ce1", q->pcfich.ce[1], q->pcfich.nof_symbols*sizeof(cf_t));
@@ -749,13 +813,13 @@ void srslte_ue_dl_save_signal(srslte_ue_dl_t *q, srslte_softbuffer_rx_t *softbuf
   srslte_vec_save_file("pdcch_symbols", q->pdcch.symbols[0], q->pdcch.nof_cce*36*sizeof(cf_t));
   srslte_vec_save_file("pdcch_eq_symbols", q->pdcch.d, q->pdcch.nof_cce*36*sizeof(cf_t));
   srslte_vec_save_file("pdcch_llr", q->pdcch.llr, q->pdcch.nof_cce*72*sizeof(float));
-  
+
   
   srslte_vec_save_file("pdsch_symbols", q->pdsch.d, q->pdsch_cfg.nbits[0].nof_re*sizeof(cf_t));
   srslte_vec_save_file("llr", q->pdsch.e, q->pdsch_cfg.nbits[0].nof_bits*sizeof(cf_t));
   int cb_len = q->pdsch_cfg.cb_segm[0].K1;
   for (int i=0;i<q->pdsch_cfg.cb_segm[0].C;i++) {
-    char tmpstr[64]; 
+    char tmpstr[64];
     snprintf(tmpstr,64,"rmout_%d.dat",i);
     srslte_vec_save_file(tmpstr, softbuffer->buffer_f[i], (3*cb_len+12)*sizeof(int16_t));  
   }
diff --git a/lib/src/phy/ue/ue_mib.c b/lib/src/phy/ue/ue_mib.c
index 34c2bc2e3..49ab30657 100644
--- a/lib/src/phy/ue/ue_mib.c
+++ b/lib/src/phy/ue/ue_mib.c
@@ -36,45 +36,40 @@
 #include "srslte/phy/utils/vector.h"
 
 int srslte_ue_mib_init(srslte_ue_mib_t * q, 
-                       srslte_cell_t cell) 
+                       uint32_t max_prb)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
-  if (q != NULL && 
-      cell.nof_ports <= SRSLTE_MAX_PORTS) 
+  if (q != NULL)
   {
 
     ret = SRSLTE_ERROR;    
     bzero(q, sizeof(srslte_ue_mib_t));
     
-    if (srslte_pbch_init(&q->pbch, cell)) {
+    if (srslte_pbch_init(&q->pbch)) {
       fprintf(stderr, "Error initiating PBCH\n");
       goto clean_exit;
     }
 
-    if (cell.nof_ports == 0) {
-      cell.nof_ports = SRSLTE_MAX_PORTS; 
-    }
-    
-    q->sf_symbols = srslte_vec_malloc(SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp) * sizeof(cf_t));
+    q->sf_symbols = srslte_vec_malloc(SRSLTE_SF_LEN_RE(max_prb, SRSLTE_CP_NORM) * sizeof(cf_t));
     if (!q->sf_symbols) {
       perror("malloc");
       goto clean_exit;
     }
     
-    for (int i=0;i<cell.nof_ports;i++) {
-      q->ce[i] = srslte_vec_malloc(SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp) * sizeof(cf_t));
+    for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
+      q->ce[i] = srslte_vec_malloc(SRSLTE_SF_LEN_RE(max_prb, SRSLTE_CP_NORM) * sizeof(cf_t));
       if (!q->ce[i]) {
         perror("malloc");
         goto clean_exit;
       }
     }
 
-    if (srslte_ofdm_rx_init(&q->fft, cell.cp, cell.nof_prb)) {
+    if (srslte_ofdm_rx_init(&q->fft, SRSLTE_CP_NORM, max_prb)) {
       fprintf(stderr, "Error initializing FFT\n");
       goto clean_exit;
     }
-    if (srslte_chest_dl_init(&q->chest, cell)) {
+    if (srslte_chest_dl_init(&q->chest, max_prb)) {
       fprintf(stderr, "Error initializing reference signal\n");
       goto clean_exit;
     }
@@ -109,6 +104,38 @@ void srslte_ue_mib_free(srslte_ue_mib_t * q)
     
 }
 
+int srslte_ue_mib_set_cell(srslte_ue_mib_t * q,
+                           srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q != NULL &&
+      cell.nof_ports <= SRSLTE_MAX_PORTS)
+  {
+    if (srslte_pbch_set_cell(&q->pbch, cell)) {
+      fprintf(stderr, "Error initiating PBCH\n");
+      return SRSLTE_ERROR;
+    }
+    if (srslte_ofdm_rx_set_prb(&q->fft, cell.cp, cell.nof_prb)) {
+      fprintf(stderr, "Error initializing FFT\n");
+      return SRSLTE_ERROR;
+    }
+
+    if (cell.nof_ports == 0) {
+      cell.nof_ports = SRSLTE_MAX_PORTS;
+    }
+
+    if (srslte_chest_dl_set_cell(&q->chest, cell)) {
+      fprintf(stderr, "Error initializing reference signal\n");
+      return SRSLTE_ERROR;
+    }
+    srslte_ue_mib_reset(q);
+
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
 
 void srslte_ue_mib_reset(srslte_ue_mib_t * q)
 {
@@ -142,7 +169,7 @@ int srslte_ue_mib_decode(srslte_ue_mib_t * q, cf_t *input,
 
   /* Decode PBCH */
   ret = srslte_pbch_decode(&q->pbch, &q->sf_symbols[SRSLTE_SLOT_LEN_RE(q->chest.cell.nof_prb, q->chest.cell.cp)], 
-                    ce_slot1, srslte_chest_dl_get_noise_estimate(&q->chest),
+                    ce_slot1, 0,
                     bch_payload, nof_tx_ports, sfn_offset);
   
 
@@ -161,27 +188,21 @@ int srslte_ue_mib_decode(srslte_ue_mib_t * q, cf_t *input,
   return ret;
 }
 
-int srslte_ue_mib_sync_init(srslte_ue_mib_sync_t *q, 
-                            uint32_t cell_id, 
-                            srslte_cp_t cp, 
-                            int (recv_callback)(void*, void*, uint32_t, srslte_timestamp_t*),                             
-                            void *stream_handler) 
+int srslte_ue_mib_sync_init_multi(srslte_ue_mib_sync_t *q, 
+                                  int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t, srslte_timestamp_t*),
+                                  uint32_t nof_rx_antennas,
+                                  void *stream_handler) 
 {
-  srslte_cell_t cell; 
-  // If the ports are set to 0, ue_mib goes through 1, 2 and 4 ports to blindly detect nof_ports
-  cell.nof_ports = 0;  
-  cell.id = cell_id; 
-  cell.cp = cp; 
-  cell.nof_prb = SRSLTE_UE_MIB_NOF_PRB; 
+  for (int i=0;i<nof_rx_antennas;i++) {
+    q->sf_buffer[i] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(SRSLTE_UE_MIB_NOF_PRB));
+  }
+  q->nof_rx_antennas = nof_rx_antennas;
   
-  q->sf_buffer[0] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(cell.nof_prb));
-  q->nof_rx_antennas = 1;
-  
-  if (srslte_ue_mib_init(&q->ue_mib, cell)) {
+  if (srslte_ue_mib_init(&q->ue_mib, SRSLTE_UE_MIB_NOF_PRB)) {
     fprintf(stderr, "Error initiating ue_mib\n");
     return SRSLTE_ERROR;
   }
-  if (srslte_ue_sync_init(&q->ue_sync, cell, recv_callback, stream_handler)) {
+  if (srslte_ue_sync_init_multi(&q->ue_sync, SRSLTE_UE_MIB_NOF_PRB, false, recv_callback, nof_rx_antennas, stream_handler)) {
     fprintf(stderr, "Error initiating ue_sync\n");
     srslte_ue_mib_free(&q->ue_mib);
     return SRSLTE_ERROR;
@@ -190,38 +211,30 @@ int srslte_ue_mib_sync_init(srslte_ue_mib_sync_t *q,
   return SRSLTE_SUCCESS;
 }
 
-int srslte_ue_mib_sync_init_multi(srslte_ue_mib_sync_t *q, 
-                                  uint32_t cell_id, 
-                                  srslte_cp_t cp, 
-                                  int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t, srslte_timestamp_t*),                             
-                                  uint32_t nof_rx_antennas,
-                                  void *stream_handler) 
+int srslte_ue_mib_sync_set_cell(srslte_ue_mib_sync_t *q,
+                                uint32_t cell_id,
+                                srslte_cp_t cp)
 {
-  srslte_cell_t cell; 
+  srslte_cell_t cell;
   // If the ports are set to 0, ue_mib goes through 1, 2 and 4 ports to blindly detect nof_ports
-  cell.nof_ports = 0;  
-  cell.id = cell_id; 
-  cell.cp = cp; 
-  cell.nof_prb = SRSLTE_UE_MIB_NOF_PRB; 
-  
-  for (int i=0;i<nof_rx_antennas;i++) {
-    q->sf_buffer[i] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(cell.nof_prb));
-  }
-  q->nof_rx_antennas = nof_rx_antennas;
-  
-  if (srslte_ue_mib_init(&q->ue_mib, cell)) {
+  cell.nof_ports = 0;
+  cell.id = cell_id;
+  cell.cp = cp;
+  cell.nof_prb = SRSLTE_UE_MIB_NOF_PRB;
+
+  if (srslte_ue_mib_set_cell(&q->ue_mib, cell)) {
     fprintf(stderr, "Error initiating ue_mib\n");
     return SRSLTE_ERROR;
   }
-  if (srslte_ue_sync_init_multi(&q->ue_sync, cell, recv_callback, nof_rx_antennas, stream_handler)) {
+  if (srslte_ue_sync_set_cell(&q->ue_sync, cell)) {
     fprintf(stderr, "Error initiating ue_sync\n");
     srslte_ue_mib_free(&q->ue_mib);
     return SRSLTE_ERROR;
   }
-  srslte_ue_sync_decode_sss_on_track(&q->ue_sync, true);
   return SRSLTE_SUCCESS;
 }
 
+
 void srslte_ue_mib_sync_free(srslte_ue_mib_sync_t *q) {
   for (int i=0;i<q->nof_rx_antennas;i++) {
     if (q->sf_buffer[i]) {
@@ -246,7 +259,9 @@ int srslte_ue_mib_sync_decode(srslte_ue_mib_sync_t * q,
   
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   uint32_t nof_frames = 0; 
-  int mib_ret = SRSLTE_UE_MIB_NOTFOUND; 
+  int mib_ret = SRSLTE_UE_MIB_NOTFOUND;
+
+  srslte_ue_mib_sync_reset(q);
 
   if (q != NULL) 
   {
diff --git a/lib/src/phy/ue/ue_sync.c b/lib/src/phy/ue/ue_sync.c
index e011789ca..2126950a6 100644
--- a/lib/src/phy/ue/ue_sync.c
+++ b/lib/src/phy/ue/ue_sync.c
@@ -29,6 +29,7 @@
 #include <strings.h>
 #include <assert.h>
 #include <unistd.h>
+#include <srslte/srslte.h>
 
 
 #include "srslte/phy/ue/ue_sync.h"
@@ -128,39 +129,41 @@ int recv_callback_multi_to_single(void *h, cf_t *x[SRSLTE_MAX_PORTS], uint32_t n
 }
 
 int srslte_ue_sync_init(srslte_ue_sync_t *q, 
-                 srslte_cell_t cell,
-                 int (recv_callback)(void*, void*, uint32_t,srslte_timestamp_t*),                 
-                 void *stream_handler) 
+                        uint32_t max_prb,
+                        bool search_cell,
+                        int (recv_callback)(void*, void*, uint32_t,srslte_timestamp_t*),
+                        void *stream_handler)
 {
-  int ret = srslte_ue_sync_init_multi(q, cell, recv_callback_multi_to_single, 1, (void*) q);
+  int ret = srslte_ue_sync_init_multi(q, max_prb, search_cell, recv_callback_multi_to_single, 1, (void*) q);
   q->recv_callback_single = recv_callback;
   q->stream_single = stream_handler;
   return ret; 
 }
 
 int srslte_ue_sync_init_multi(srslte_ue_sync_t *q, 
-                              srslte_cell_t cell,
+                              uint32_t max_prb,
+                              bool search_cell,
                               int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t,srslte_timestamp_t*),
                               uint32_t nof_rx_antennas,
                               void *stream_handler) 
 
 {
     
-    return srslte_ue_sync_init_multi_decim(q,  cell,recv_callback ,nof_rx_antennas,stream_handler,1); 
+    return srslte_ue_sync_init_multi_decim(q, max_prb,search_cell, recv_callback ,nof_rx_antennas,stream_handler,1);
 }
 
 int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q, 
-                              srslte_cell_t cell,
-                              int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t,srslte_timestamp_t*),
-                              uint32_t nof_rx_antennas,
-                              void *stream_handler,
-                              int decimate) 
+                                    uint32_t max_prb,
+                                    bool search_cell,
+                                    int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t,srslte_timestamp_t*),
+                                    uint32_t nof_rx_antennas,
+                                    void *stream_handler,
+                                    int decimate)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   
   if (q                                 != NULL && 
       stream_handler                    != NULL && 
-      srslte_nofprb_isvalid(cell.nof_prb)       &&
       nof_rx_antennas <= SRSLTE_MAX_PORTS       &&
       recv_callback                     != NULL)
   {
@@ -171,104 +174,58 @@ int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q,
     q->stream = stream_handler;
     q->recv_callback = recv_callback;
     q->nof_rx_antennas = nof_rx_antennas;
-    q->cell = cell;
-    q->fft_size = srslte_symbol_sz(q->cell.nof_prb);
+    q->fft_size = srslte_symbol_sz(max_prb);
     q->sf_len = SRSLTE_SF_LEN(q->fft_size);
     q->file_mode = false; 
     q->correct_cfo = true; 
     q->agc_period = 0; 
     q->sample_offset_correct_period = DEFAULT_SAMPLE_OFFSET_CORRECT_PERIOD; 
     q->sfo_ema                      = DEFAULT_SFO_EMA_COEFF; 
-    
-    if (cell.id == 1000) {
+
+    q->max_prb = max_prb;
+
+    if (search_cell) {
       
       /* If the cell is unkown, we search PSS/SSS in 5 ms */
       q->nof_recv_sf = 5; 
 
-      q->decode_sss_on_track = true; 
-
     } else {
       
       /* If the cell is known, we work on a 1ms basis */
       q->nof_recv_sf = 1; 
 
-      q->decode_sss_on_track = true; 
     }
 
     q->frame_len = q->nof_recv_sf*q->sf_len;
     
-    if(q->fft_size < 700  && q->decimate)
-    {
+    if(q->fft_size < 700 && q->decimate) {
         q->decimate = 1;    
     }
-    
-  
+
     if(srslte_sync_init_decim(&q->sfind, q->frame_len, q->frame_len, q->fft_size,q->decimate)) {
       fprintf(stderr, "Error initiating sync find\n");
       goto clean_exit;
     }
-    if (cell.id == 1000) {
+    if (search_cell) {
       if(srslte_sync_init(&q->strack, q->frame_len, TRACK_FRAME_SIZE, q->fft_size)) {
         fprintf(stderr, "Error initiating sync track\n");
         goto clean_exit;
-      }      
+      }
     } else {
       if(srslte_sync_init(&q->strack, q->frame_len, SRSLTE_CP_LEN_NORM(1,q->fft_size), q->fft_size)) {
         fprintf(stderr, "Error initiating sync track\n");
         goto clean_exit;
       }
     }
-    
-    if (cell.id == 1000) {
-      /* If the cell id is unknown, enable CP detection on find */ 
-      // FIXME: CP detection not working very well. Not supporting Extended CP right now
-      srslte_sync_cp_en(&q->sfind, false);      
-      srslte_sync_cp_en(&q->strack, false); 
 
-      srslte_sync_set_cfo_ema_alpha(&q->sfind, 0.8);
-      srslte_sync_set_cfo_ema_alpha(&q->strack, 0.1);
-
-      srslte_sync_cfo_i_detec_en(&q->sfind, false); 
-
-      q->nof_avg_find_frames = FIND_NOF_AVG_FRAMES; 
-      srslte_sync_set_threshold(&q->sfind, 2.0);
-      srslte_sync_set_threshold(&q->strack, 1.2);
-      
-    } else {
-      srslte_sync_set_N_id_2(&q->sfind, cell.id%3);
-      srslte_sync_set_N_id_2(&q->strack, cell.id%3);
-      q->sfind.cp = cell.cp;
-      q->strack.cp = cell.cp;
-      srslte_sync_cp_en(&q->sfind, false);      
-      srslte_sync_cp_en(&q->strack, false);        
-
-      srslte_sync_cfo_i_detec_en(&q->sfind, false); 
-      
-      srslte_sync_set_cfo_ema_alpha(&q->sfind, 0.1);
-      srslte_sync_set_cfo_ema_alpha(&q->strack, 0.1);
-
-      /* In find phase and if the cell is known, do not average pss correlation
-       * because we only capture 1 subframe and do not know where the peak is. 
-       */
-      q->nof_avg_find_frames = 1; 
-      srslte_sync_set_em_alpha(&q->sfind, 1);
-      srslte_sync_set_threshold(&q->sfind, 3.0);
-      
-      srslte_sync_set_em_alpha(&q->strack, 0.2);
-      srslte_sync_set_threshold(&q->strack, 1.2);
-
-    }
-      
-    srslte_ue_sync_reset(q);
-    
     ret = SRSLTE_SUCCESS;
   }
-  
+
 clean_exit:
   if (ret == SRSLTE_ERROR) {
     srslte_ue_sync_free(q);
   }
-  return ret; 
+  return ret;
 }
 
 uint32_t srslte_ue_sync_sf_len(srslte_ue_sync_t *q) {
@@ -281,13 +238,119 @@ void srslte_ue_sync_free(srslte_ue_sync_t *q) {
   }
   if (!q->file_mode) {
     srslte_sync_free(&q->sfind);
-    srslte_sync_free(&q->strack);    
+    srslte_sync_free(&q->strack);
   } else {
     srslte_filesource_free(&q->file_source);
   }
   bzero(q, sizeof(srslte_ue_sync_t));
 }
 
+
+int srslte_ue_sync_set_cell(srslte_ue_sync_t *q, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q                                 != NULL &&
+      srslte_nofprb_isvalid(cell.nof_prb))
+  {
+    ret = SRSLTE_ERROR;
+
+    if (cell.nof_prb > q->max_prb) {
+      fprintf(stderr, "Error in ue_sync_set_cell(): cell.nof_prb must be lower than initialized\n");
+      return SRSLTE_ERROR;
+    }
+
+    memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+    q->fft_size = srslte_symbol_sz(q->cell.nof_prb);
+    q->sf_len = SRSLTE_SF_LEN(q->fft_size);
+    q->agc_period = 0;
+
+    if (cell.id == 1000) {
+
+      /* If the cell is unkown, we search PSS/SSS in 5 ms */
+      q->nof_recv_sf = 5;
+
+      q->decode_sss_on_track = true;
+
+    } else {
+
+      /* If the cell is known, we work on a 1ms basis */
+      q->nof_recv_sf = 1;
+
+      q->decode_sss_on_track = true;
+    }
+
+    q->frame_len = q->nof_recv_sf*q->sf_len;
+
+    if(q->fft_size < 700 && q->decimate) {
+      q->decimate = 1;
+    }
+
+    if(srslte_sync_resize(&q->sfind, q->frame_len, q->frame_len, q->fft_size)) {
+      fprintf(stderr, "Error initiating sync find\n");
+      return SRSLTE_ERROR;
+    }
+    if (cell.id == 1000) {
+      if(srslte_sync_resize(&q->strack, q->frame_len, TRACK_FRAME_SIZE, q->fft_size)) {
+        fprintf(stderr, "Error initiating sync track\n");
+        return SRSLTE_ERROR;
+      }
+    } else {
+      if(srslte_sync_resize(&q->strack, q->frame_len, SRSLTE_CP_LEN_NORM(1,q->fft_size), q->fft_size)) {
+        fprintf(stderr, "Error initiating sync track\n");
+        return SRSLTE_ERROR;
+      }
+    }
+
+    if (cell.id == 1000) {
+      /* If the cell id is unknown, enable CP detection on find */
+      // FIXME: CP detection not working very well. Not supporting Extended CP right now
+      srslte_sync_cp_en(&q->sfind, false);
+      srslte_sync_cp_en(&q->strack, false);
+
+      srslte_sync_set_cfo_ema_alpha(&q->sfind, 0.8);
+      srslte_sync_set_cfo_ema_alpha(&q->strack, 0.1);
+
+      srslte_sync_cfo_i_detec_en(&q->sfind, false);
+
+      q->nof_avg_find_frames = FIND_NOF_AVG_FRAMES;
+      srslte_sync_set_threshold(&q->sfind, 2.0);
+      srslte_sync_set_threshold(&q->strack, 1.2);
+
+    } else {
+      srslte_sync_set_N_id_2(&q->sfind, cell.id%3);
+      srslte_sync_set_N_id_2(&q->strack, cell.id%3);
+      q->sfind.cp = cell.cp;
+      q->strack.cp = cell.cp;
+      srslte_sync_cp_en(&q->sfind, false);
+      srslte_sync_cp_en(&q->strack, false);
+
+      srslte_sync_cfo_i_detec_en(&q->sfind, false);
+
+      srslte_sync_set_cfo_ema_alpha(&q->sfind, 0.1);
+      srslte_sync_set_cfo_ema_alpha(&q->strack, 0.1);
+
+      /* In find phase and if the cell is known, do not average pss correlation
+       * because we only capture 1 subframe and do not know where the peak is.
+       */
+      q->nof_avg_find_frames = 1;
+      srslte_sync_set_em_alpha(&q->sfind, 1);
+      srslte_sync_set_threshold(&q->sfind, 3.0);
+
+      srslte_sync_set_em_alpha(&q->strack, 0.2);
+      srslte_sync_set_threshold(&q->strack, 1.2);
+
+    }
+
+    srslte_ue_sync_reset(q);
+
+    ret = SRSLTE_SUCCESS;
+  }
+
+  return ret;
+}
+
+
 void srslte_ue_sync_get_last_timestamp(srslte_ue_sync_t *q, srslte_timestamp_t *timestamp) {
   memcpy(timestamp, &q->last_timestamp, sizeof(srslte_timestamp_t));
 }
@@ -317,6 +380,8 @@ void srslte_ue_sync_set_cfo(srslte_ue_sync_t *q, float cfo) {
   srslte_sync_set_cfo(&q->strack, cfo/15000);
 }
 
+void srslte_ue_sync_set_cfo_tol(srslte_ue_sync_t *q, float cfo_tol) {}
+
 float srslte_ue_sync_get_sfo(srslte_ue_sync_t *q) {
   return q->mean_sfo/5e-3;
 }
@@ -649,7 +714,7 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
           } 
           if (q->correct_cfo) {
             for (int i=0;i<q->nof_rx_antennas;i++) {
-              srslte_cfo_correct(&q->sfind.cfocorr, 
+              srslte_cfo_correct(&q->strack.cfocorr,
                           input_buffer[i], 
                           input_buffer[i], 
                           -srslte_sync_get_cfo(&q->strack) / q->fft_size);                         
diff --git a/lib/src/phy/ue/ue_ul.c b/lib/src/phy/ue/ue_ul.c
index 08edcafe0..37dfecd93 100644
--- a/lib/src/phy/ue/ue_ul.c
+++ b/lib/src/phy/ue/ue_ul.c
@@ -36,22 +36,22 @@
 #define CURRENT_SLOTLEN_RE SRSLTE_SLOT_LEN_RE(q->cell.nof_prb, q->cell.cp)
 #define CURRENT_SFLEN_RE SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)
 
+#define MAX_SFLEN     SRSLTE_SF_LEN(srslte_symbol_sz(max_prb))
 
-int srslte_ue_ul_init(srslte_ue_ul_t *q, 
-                      srslte_cell_t cell) 
+#define DEFAULT_CFO_TOL   50.0 // Hz
+
+int srslte_ue_ul_init(srslte_ue_ul_t *q,
+                      uint32_t max_prb)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS; 
   
-  if (q                 != NULL &&
-      srslte_cell_isvalid(&cell))   
+  if (q != NULL)
   {
     ret = SRSLTE_ERROR;
     
     bzero(q, sizeof(srslte_ue_ul_t));
     
-    q->cell = cell; 
-    
-    if (srslte_ofdm_tx_init(&q->fft, q->cell.cp, q->cell.nof_prb)) {
+    if (srslte_ofdm_tx_init(&q->fft, SRSLTE_CP_NORM, max_prb)) {
       fprintf(stderr, "Error initiating FFT\n");
       goto clean_exit;
     }
@@ -60,41 +60,41 @@ int srslte_ue_ul_init(srslte_ue_ul_t *q,
 
     q->normalize_en = false; 
 
-    if (srslte_cfo_init(&q->cfo, CURRENT_SFLEN)) {
+    if (srslte_cfo_init(&q->cfo, MAX_SFLEN)) {
       fprintf(stderr, "Error creating CFO object\n");
       goto clean_exit;
     }
-    
-    srslte_cfo_set_tol(&q->cfo, 0);
-    
-    if (srslte_pusch_init(&q->pusch, q->cell)) {
+
+    srslte_ue_ul_set_cfo_tol(q, DEFAULT_CFO_TOL);
+
+    if (srslte_pusch_init_ue(&q->pusch, max_prb)) {
       fprintf(stderr, "Error creating PUSCH object\n");
       goto clean_exit;
     }
-    if (srslte_pucch_init(&q->pucch, q->cell)) {
+    if (srslte_pucch_init(&q->pucch)) {
       fprintf(stderr, "Error creating PUSCH object\n");
       goto clean_exit;
     }
-    if (srslte_softbuffer_tx_init(&q->softbuffer, q->cell.nof_prb)) {
+    if (srslte_softbuffer_tx_init(&q->softbuffer, max_prb)) {
       fprintf(stderr, "Error initiating soft buffer\n");
       goto clean_exit;
     }
-    if (srslte_refsignal_ul_init(&q->signals, cell)) {
+    if (srslte_refsignal_ul_init(&q->signals, max_prb)) {
       fprintf(stderr, "Error initiating srslte_refsignal_ul\n");
       goto clean_exit;
     }
-    q->sf_symbols = srslte_vec_malloc(CURRENT_SFLEN_RE * sizeof(cf_t));
+    q->sf_symbols = srslte_vec_malloc(SRSLTE_SF_LEN_PRB(max_prb) * sizeof(cf_t));
     if (!q->sf_symbols) {
       perror("malloc");
       goto clean_exit; 
     }
-    q->refsignal = srslte_vec_malloc(2 * SRSLTE_NRE * q->cell.nof_prb * sizeof(cf_t));
+    q->refsignal = srslte_vec_malloc(2 * SRSLTE_NRE * max_prb * sizeof(cf_t));
     if (!q->refsignal) {
       perror("malloc");
       goto clean_exit; 
     }
     
-    q->srs_signal = srslte_vec_malloc(SRSLTE_NRE * q->cell.nof_prb * sizeof(cf_t));
+    q->srs_signal = srslte_vec_malloc(SRSLTE_NRE * max_prb * sizeof(cf_t));
     if (!q->srs_signal) {
       perror("malloc");
       goto clean_exit; 
@@ -102,8 +102,7 @@ int srslte_ue_ul_init(srslte_ue_ul_t *q,
     q->signals_pregenerated = false; 
     ret = SRSLTE_SUCCESS;
   } else {
-    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
-            cell.id, cell.nof_ports, cell.nof_prb);      
+    fprintf(stderr, "Invalid parameters\n");
   }
 
 clean_exit: 
@@ -115,7 +114,7 @@ clean_exit:
 
 void srslte_ue_ul_free(srslte_ue_ul_t *q) {
   if (q) {
-    srslte_ofdm_rx_free(&q->fft);
+    srslte_ofdm_tx_free(&q->fft);
     srslte_pusch_free(&q->pusch);
     srslte_pucch_free(&q->pucch);
     srslte_softbuffer_tx_free(&q->softbuffer);
@@ -140,6 +139,54 @@ void srslte_ue_ul_free(srslte_ue_ul_t *q) {
   }
 }
 
+int srslte_ue_ul_set_cell(srslte_ue_ul_t *q,
+                          srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q != NULL && srslte_cell_isvalid(&cell))
+  {
+    if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+
+      if (srslte_ofdm_tx_set_prb(&q->fft, q->cell.cp, q->cell.nof_prb)) {
+        fprintf(stderr, "Error resizing FFT\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_cfo_resize(&q->cfo, SRSLTE_SF_LEN_PRB(q->cell.nof_prb))) {
+        fprintf(stderr, "Error resizing CFO object\n");
+        return SRSLTE_ERROR;
+      }
+
+      srslte_ue_ul_set_cfo_tol(q, q->current_cfo_tol);
+
+      if (srslte_pusch_set_cell(&q->pusch, q->cell)) {
+        fprintf(stderr, "Error resizing PUSCH object\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_pucch_set_cell(&q->pucch, q->cell)) {
+        fprintf(stderr, "Error resizing PUSCH object\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_refsignal_ul_set_cell(&q->signals, q->cell)) {
+        fprintf(stderr, "Error resizing srslte_refsignal_ul\n");
+        return SRSLTE_ERROR;
+      }
+      q->signals_pregenerated = false;
+    }
+    ret = SRSLTE_SUCCESS;
+  } else {
+    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
+            cell.id, cell.nof_ports, cell.nof_prb);
+  }
+  return ret;
+}
+
+void srslte_ue_ul_set_cfo_tol(srslte_ue_ul_t *q, float tol) {
+  q->current_cfo_tol = tol;
+  srslte_cfo_set_tol(&q->cfo, tol/(15000.0*srslte_symbol_sz(q->cell.nof_prb)));
+}
+
 void srslte_ue_ul_set_cfo(srslte_ue_ul_t *q, float cur_cfo) {
   q->current_cfo = cur_cfo; 
 }
@@ -154,7 +201,7 @@ void srslte_ue_ul_set_normalization(srslte_ue_ul_t *q, bool enabled)
   q->normalize_en = enabled;
 }
 
-/* Precalculate the PDSCH scramble sequences for a given RNTI. This function takes a while 
+/* Precalculate the PUSCH scramble sequences for a given RNTI. This function takes a while
  * to execute, so shall be called once the final C-RNTI has been allocated for the session.
  * For the connection procedure, use srslte_pusch_encode_rnti() or srslte_pusch_decode_rnti() functions 
  */
@@ -230,6 +277,14 @@ void pucch_encode_bits(srslte_uci_data_t *uci_data, srslte_pucch_format_t format
     pucch_bits[1] = uci_data->uci_ack_2; // this will be ignored in format 1a 
   }
   if (format >= SRSLTE_PUCCH_FORMAT_2) {
+    /* Append Differential CQI */
+    memcpy(&uci_data->uci_cqi[uci_data->uci_cqi_len], uci_data->uci_dif_cqi, uci_data->uci_dif_cqi_len);
+    uci_data->uci_cqi_len += uci_data->uci_dif_cqi_len;
+
+    /* Append PMI */
+    memcpy(&uci_data->uci_cqi[uci_data->uci_cqi_len], uci_data->uci_pmi, uci_data->uci_pmi_len);
+    uci_data->uci_cqi_len += uci_data->uci_pmi_len;
+
     srslte_uci_encode_cqi_pucch(uci_data->uci_cqi, uci_data->uci_cqi_len, pucch_bits);
     if (format > SRSLTE_PUCCH_FORMAT_2) {
       pucch2_bits[0] = uci_data->uci_ack; 
@@ -295,7 +350,7 @@ int srslte_ue_ul_pucch_encode(srslte_ue_ul_t *q, srslte_uci_data_t uci_data,
     srslte_ofdm_tx_sf(&q->fft, q->sf_symbols, output_signal);
     
     if (q->cfo_en) {
-      srslte_cfo_correct(&q->cfo, output_signal, output_signal, q->current_cfo / srslte_symbol_sz(q->cell.nof_prb));            
+      srslte_cfo_correct(&q->cfo, output_signal, output_signal, q->current_cfo / srslte_symbol_sz(q->cell.nof_prb));
     }
     
     if (q->normalize_en) {
@@ -365,7 +420,7 @@ int srslte_ue_ul_srs_encode(srslte_ue_ul_t *q, uint32_t tti, cf_t *output_signal
     srslte_ofdm_tx_sf(&q->fft, q->sf_symbols, output_signal);
     
     if (q->cfo_en) {
-      srslte_cfo_correct(&q->cfo, output_signal, output_signal, q->current_cfo / srslte_symbol_sz(q->cell.nof_prb));            
+      srslte_cfo_correct(&q->cfo, output_signal, output_signal, q->current_cfo / srslte_symbol_sz(q->cell.nof_prb));
     }
     
     if (q->normalize_en) {
@@ -407,7 +462,7 @@ int srslte_ue_ul_pusch_encode_rnti_softbuffer(srslte_ue_ul_t *q,
                                              q->pusch_cfg.grant.n_prb_tilde, 
                                              q->sf_symbols);
     } else {
-   
+
       if (srslte_refsignal_dmrs_pusch_gen(&q->signals, q->pusch_cfg.grant.L_prb, 
                                           q->pusch_cfg.sf_idx,
                                           q->pusch_cfg.grant.ncs_dmrs,
@@ -434,7 +489,7 @@ int srslte_ue_ul_pusch_encode_rnti_softbuffer(srslte_ue_ul_t *q,
     srslte_ofdm_tx_sf(&q->fft, q->sf_symbols, output_signal);
     
     if (q->cfo_en) {
-      srslte_cfo_correct(&q->cfo, output_signal, output_signal, q->current_cfo / srslte_symbol_sz(q->cell.nof_prb));            
+      srslte_cfo_correct(&q->cfo, output_signal, output_signal, q->current_cfo / srslte_symbol_sz(q->cell.nof_prb));
     }
     
     if (q->normalize_en) {
diff --git a/lib/src/phy/utils/cexptab.c b/lib/src/phy/utils/cexptab.c
index 47ff8ef96..0f9232e85 100644
--- a/lib/src/phy/utils/cexptab.c
+++ b/lib/src/phy/utils/cexptab.c
@@ -36,7 +36,7 @@ int srslte_cexptab_init(srslte_cexptab_t *h, uint32_t size) {
   uint32_t i;
 
   h->size = size;
-  h->tab = malloc(sizeof(cf_t) * size);
+  h->tab = malloc(sizeof(cf_t) * (1+size));
   if (h->tab) {
     for (i = 0; i < size; i++) {
       h->tab[i] = cexpf(_Complex_I * 2 * M_PI * (float) i / size);
diff --git a/lib/src/phy/utils/convolution.c b/lib/src/phy/utils/convolution.c
index d3b852b76..f96bdd4a2 100644
--- a/lib/src/phy/utils/convolution.c
+++ b/lib/src/phy/utils/convolution.c
@@ -27,6 +27,7 @@
 
 #include <stdlib.h>
 #include <string.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/dft/dft.h"
 #include "srslte/phy/utils/vector.h"
@@ -34,14 +35,17 @@
 
 
 int srslte_conv_fft_cc_init(srslte_conv_fft_cc_t *q, uint32_t input_len, uint32_t filter_len) {
+  bzero(q, sizeof(srslte_conv_fft_cc_t));
+
   q->input_len = input_len;
   q->filter_len = filter_len;
   q->output_len = input_len+filter_len;
+  q->max_filter_len = filter_len;
+  q->max_input_len  = input_len;
   q->input_fft = srslte_vec_malloc(sizeof(cf_t)*q->output_len);
   q->filter_fft = srslte_vec_malloc(sizeof(cf_t)*q->output_len);
   q->output_fft = srslte_vec_malloc(sizeof(cf_t)*q->output_len);
-  
-  
+
   if (!q->input_fft || !q->filter_fft || !q->output_fft) {
     return SRSLTE_ERROR;
   }
@@ -64,6 +68,34 @@ int srslte_conv_fft_cc_init(srslte_conv_fft_cc_t *q, uint32_t input_len, uint32_
   return SRSLTE_SUCCESS;
 }
 
+int srslte_conv_fft_cc_replan(srslte_conv_fft_cc_t *q, uint32_t input_len, uint32_t filter_len) {
+  if (input_len > q->max_input_len || filter_len > q->max_filter_len) {
+    fprintf(stderr, "Error in conv_fft_cc_replan(): input_len and filter_len must be lower than initialized\n");
+    return -1;
+  }
+
+  q->input_len = input_len;
+  q->filter_len = filter_len;
+  q->output_len = input_len+filter_len;
+
+  if (!q->input_fft || !q->filter_fft || !q->output_fft) {
+    return SRSLTE_ERROR;
+  }
+  if (srslte_dft_replan(&q->input_plan,q->output_len)) {
+    fprintf(stderr, "Error initiating input plan\n");
+    return SRSLTE_ERROR;
+  }
+  if (srslte_dft_replan(&q->filter_plan,q->output_len)) {
+    fprintf(stderr, "Error initiating filter plan\n");
+    return SRSLTE_ERROR;
+  }
+  if (srslte_dft_replan(&q->output_plan,q->output_len)) {
+    fprintf(stderr, "Error initiating output plan\n");
+    return SRSLTE_ERROR;
+  }
+  return SRSLTE_SUCCESS;
+}
+
 void srslte_conv_fft_cc_free(srslte_conv_fft_cc_t *q) {
   if (q->input_fft) {
     free(q->input_fft);
diff --git a/lib/src/radio/radio.cc b/lib/src/radio/radio.cc
index 3a5fdf9f1..c3592f168 100644
--- a/lib/src/radio/radio.cc
+++ b/lib/src/radio/radio.cc
@@ -47,7 +47,7 @@ bool radio::init(char *args, char *devname)
   cur_tx_srate = 0; 
   is_start_of_burst = true; 
   
-  // Suppress radio stdout 
+  // Suppress radio stdout
   srslte_rf_suppress_stdout(&rf_device);
   
   tx_adv_auto = true; 
@@ -250,9 +250,13 @@ void radio::save_trace(uint32_t is_eob, srslte_timestamp_t *tx_time) {
   }
 }
 
-void radio::set_rx_freq(float freq)
+void radio::set_freq_offset(double freq) {
+  freq_offset = freq;
+}
+
+void radio::set_rx_freq(double freq)
 {
-  rx_freq = srslte_rf_set_rx_freq(&rf_device, freq);
+  rx_freq = srslte_rf_set_rx_freq(&rf_device, freq+freq_offset);
 }
 
 void radio::set_rx_gain(float gain)
@@ -265,19 +269,19 @@ double radio::set_rx_gain_th(float gain)
   return srslte_rf_set_rx_gain_th(&rf_device, gain);
 }
 
-void radio::set_master_clock_rate(float rate)
+void radio::set_master_clock_rate(double rate)
 {
   srslte_rf_set_master_clock_rate(&rf_device, rate);
 }
 
-void radio::set_rx_srate(float srate)
+void radio::set_rx_srate(double srate)
 {
   srslte_rf_set_rx_srate(&rf_device, srate);
 }
 
-void radio::set_tx_freq(float freq)
+void radio::set_tx_freq(double freq)
 {
-  tx_freq = srslte_rf_set_tx_freq(&rf_device, freq);  
+  tx_freq = srslte_rf_set_tx_freq(&rf_device, freq+freq_offset);
 }
 
 void radio::set_tx_gain(float gain)
@@ -285,12 +289,12 @@ void radio::set_tx_gain(float gain)
   srslte_rf_set_tx_gain(&rf_device, gain);
 }
 
-float radio::get_rx_freq()
+double radio::get_rx_freq()
 {
   return rx_freq;
 }
 
-float radio::get_tx_freq()
+double radio::get_tx_freq()
 {
   return tx_freq; 
 }
@@ -305,7 +309,7 @@ float radio::get_rx_gain()
   return srslte_rf_get_rx_gain(&rf_device);
 }
 
-void radio::set_tx_srate(float srate)
+void radio::set_tx_srate(double srate)
 {
   cur_tx_srate = srslte_rf_set_tx_srate(&rf_device, srate);
   burst_preamble_samples = (uint32_t) (cur_tx_srate * burst_preamble_sec);
@@ -392,8 +396,6 @@ void radio::set_tx_srate(float srate)
   
   // Calculate TX advance in seconds from samples and sampling rate 
   tx_adv_sec = nsamples/cur_tx_srate;
-  
-  printf("Setting TX/RX offset %d samples, %.2f us\n", nsamples, tx_adv_sec*1e6);
 }
 
 void radio::start_rx()
diff --git a/lib/src/radio/radio_multi.cc b/lib/src/radio/radio_multi.cc
index c8de57f46..0bb538c8d 100644
--- a/lib/src/radio/radio_multi.cc
+++ b/lib/src/radio/radio_multi.cc
@@ -14,9 +14,11 @@ bool radio_multi::init_multi(uint32_t nof_rx_antennas, char* args, char* devname
   burst_preamble_samples = 0; 
   burst_preamble_time_rounded = 0; 
   cur_tx_srate = 0; 
-  is_start_of_burst = true; 
-  
-  
+  is_start_of_burst = true;
+
+  // Suppress radio stdout
+  srslte_rf_suppress_stdout(&rf_device);
+
   tx_adv_auto = true; 
   // Set default preamble length each known device
   // We distinguish by device family, maybe we should calibrate per device
diff --git a/lib/src/upper/gw.cc b/lib/src/upper/gw.cc
index 685803a4d..321d8d9e9 100644
--- a/lib/src/upper/gw.cc
+++ b/lib/src/upper/gw.cc
@@ -42,13 +42,14 @@ namespace srslte {
 
 gw::gw()
   :if_up(false)
-{}
+{
+  current_ip_addr = 0;
+}
 
-void gw::init(srsue::pdcp_interface_gw *pdcp_, srsue::rrc_interface_gw *rrc_, srsue::ue_interface *ue_, log *gw_log_, uint32_t lcid_)
+void gw::init(srsue::pdcp_interface_gw *pdcp_, srsue::ue_interface *ue_, log *gw_log_, uint32_t lcid_)
 {
   pool    = byte_buffer_pool::get_instance();
   pdcp    = pdcp_;
-  rrc     = rrc_;
   ue      = ue_;
   gw_log  = gw_log_;
   lcid    = lcid_;
@@ -78,6 +79,8 @@ void gw::stop()
         thread_cancel();
       }
       wait_thread_finish();
+
+      current_ip_addr = 0;
     }
 
     // TODO: tear down TUN device?
@@ -127,38 +130,42 @@ void gw::write_pdu(uint32_t lcid, byte_buffer_t *pdu)
 *******************************************************************************/
 error_t gw::setup_if_addr(uint32_t ip_addr, char *err_str)
 {
-  if(!if_up)
-  {
+  if (ip_addr != current_ip_addr) {
+    if(!if_up)
+    {
       if(init_if(err_str))
       {
         gw_log->error("init_if failed\n");
         return(ERROR_CANT_START);
       }
-  }
+    }
 
-  // Setup the IP address
-  sock                                                   = socket(AF_INET, SOCK_DGRAM, 0);
-  ifr.ifr_addr.sa_family                                 = AF_INET;
-  ((struct sockaddr_in *)&ifr.ifr_addr)->sin_addr.s_addr = htonl(ip_addr);
-  if(0 > ioctl(sock, SIOCSIFADDR, &ifr))
-  {
+    // Setup the IP address
+    sock                                                   = socket(AF_INET, SOCK_DGRAM, 0);
+    ifr.ifr_addr.sa_family                                 = AF_INET;
+    ((struct sockaddr_in *)&ifr.ifr_addr)->sin_addr.s_addr = htonl(ip_addr);
+    if(0 > ioctl(sock, SIOCSIFADDR, &ifr))
+    {
       err_str = strerror(errno);
       gw_log->debug("Failed to set socket address: %s\n", err_str);
       close(tun_fd);
       return(ERROR_CANT_START);
-  }
-  ifr.ifr_netmask.sa_family                                 = AF_INET;
-  ((struct sockaddr_in *)&ifr.ifr_netmask)->sin_addr.s_addr = inet_addr("255.255.255.0");
-  if(0 > ioctl(sock, SIOCSIFNETMASK, &ifr))
-  {
+    }
+    ifr.ifr_netmask.sa_family                                 = AF_INET;
+    ((struct sockaddr_in *)&ifr.ifr_netmask)->sin_addr.s_addr = inet_addr("255.255.255.0");
+    if(0 > ioctl(sock, SIOCSIFNETMASK, &ifr))
+    {
       err_str = strerror(errno);
       gw_log->debug("Failed to set socket netmask: %s\n", err_str);
       close(tun_fd);
       return(ERROR_CANT_START);
-  }
+    }
 
-  // Setup a thread to receive packets from the TUN device
-  start(GW_THREAD_PRIO);
+    current_ip_addr = ip_addr;
+
+    // Setup a thread to receive packets from the TUN device
+    start(GW_THREAD_PRIO);
+  }
 
   return(ERROR_NONE);
 }
@@ -250,11 +257,6 @@ void gw::run_thread()
           {
             gw_log->info_hex(pdu->msg, pdu->N_bytes, "TX PDU");
 
-            while(run_enable && (!rrc->rrc_connected() || !rrc->have_drb())) {
-              rrc->rrc_connect();
-              usleep(1000);
-            }
-            
             if (!run_enable) {
               break;
             }
diff --git a/srsenb/hdr/enb.h b/srsenb/hdr/enb.h
index b28d12ab4..8b847302e 100644
--- a/srsenb/hdr/enb.h
+++ b/srsenb/hdr/enb.h
@@ -138,42 +138,53 @@ typedef struct {
   Main UE class
 *******************************************************************************/
 
+//#define LOG_STDOUT
+
 class enb
-    :public enb_metrics_interface
-{
+    :public enb_metrics_interface {
 public:
-  static enb* get_instance(void);
+  static enb *get_instance(void);
+
   static void cleanup(void);
 
   bool init(all_args_t *args_);
+
   void stop();
+
   void start_plot();
-  
+
   static void rf_msg(srslte_rf_error_t error);
+
   void handle_rf_msg(srslte_rf_error_t error);
 
   // eNodeB metrics interface
   bool get_metrics(enb_metrics_t &m);
 
   void pregenerate_signals(bool enable);
-  
+
 
 private:
   static enb *instance;
+
   enb();
+
   virtual ~enb();
 
-  srslte::radio      radio;
-  srsenb::phy        phy;
-  srsenb::mac        mac;
-  srslte::mac_pcap   mac_pcap;
-  srsenb::rlc        rlc;
-  srsenb::pdcp       pdcp;
-  srsenb::rrc        rrc;
-  srsenb::gtpu       gtpu;
-  srsenb::s1ap       s1ap;
+  srslte::radio radio;
+  srsenb::phy phy;
+  srsenb::mac mac;
+  srslte::mac_pcap mac_pcap;
+  srsenb::rlc rlc;
+  srsenb::pdcp pdcp;
+  srsenb::rrc rrc;
+  srsenb::gtpu gtpu;
+  srsenb::s1ap s1ap;
 
-  srslte::logger_file logger;
+#ifdef LOG_STDOUT
+    srslte::logger_stdout logger;
+#else
+    srslte::logger_file logger;
+#endif
   srslte::log_filter  rf_log;
   std::vector<void*>  phy_log;
   srslte::log_filter  mac_log;
diff --git a/srsenb/hdr/mac/scheduler.h b/srsenb/hdr/mac/scheduler.h
index 66f8998c1..c70ee4247 100644
--- a/srsenb/hdr/mac/scheduler.h
+++ b/srsenb/hdr/mac/scheduler.h
@@ -80,7 +80,8 @@ public:
    ************************************************************/
   
   sched(); 
-  
+  ~sched();
+
   void init(rrc_interface_mac *rrc, srslte::log *log);
   void set_metric(metric_dl *dl_metric, metric_ul *ul_metric);
   int cell_cfg(cell_cfg_t *cell_cfg); 
diff --git a/srsenb/hdr/mac/ue.h b/srsenb/hdr/mac/ue.h
index 5805e5472..b879d040b 100644
--- a/srsenb/hdr/mac/ue.h
+++ b/srsenb/hdr/mac/ue.h
@@ -38,7 +38,7 @@
 
 namespace srsenb {
   
-class ue : public srslte::read_pdu_interface, 
+class ue : public srslte::read_pdu_interface,
            public srslte::pdu_queue::process_callback
 {
 public:
diff --git a/srsenb/hdr/phy/phch_worker.h b/srsenb/hdr/phy/phch_worker.h
index 2f0f80f41..906e8b9d0 100644
--- a/srsenb/hdr/phy/phch_worker.h
+++ b/srsenb/hdr/phy/phch_worker.h
@@ -42,7 +42,8 @@ public:
   
   phch_worker();
   void  init(phch_common *phy, srslte::log *log_h);
-  void  reset(); 
+  void  stop();
+  void  reset();
   
   cf_t *get_buffer_rx();
   void set_time(uint32_t tti, uint32_t tx_mutex_cnt, srslte_timestamp_t tx_time);
@@ -83,7 +84,9 @@ private:
   /* Common objects */  
   srslte::log    *log_h; 
   phch_common    *phy;
-  bool           initiated; 
+  bool           initiated;
+  bool           running;
+
   cf_t          *signal_buffer_rx; 
   cf_t          *signal_buffer_tx; 
   uint32_t       tti_rx, tti_tx, tti_sched_ul, sf_rx, sf_tx, sf_sched_ul, tx_mutex_cnt;
diff --git a/srsenb/src/enb.cc b/srsenb/src/enb.cc
index 1bd7240a2..e2c1c9a29 100644
--- a/srsenb/src/enb.cc
+++ b/srsenb/src/enb.cc
@@ -45,6 +45,7 @@ enb* enb::get_instance(void)
 void enb::cleanup(void)
 {
   srslte_dft_exit();
+  srslte::byte_buffer_pool::cleanup();
   boost::mutex::scoped_lock lock(enb_instance_mutex);
   if(NULL != instance) {
       delete instance;
@@ -61,14 +62,15 @@ enb::enb()
 
 enb::~enb()
 {
-  srslte::byte_buffer_pool::cleanup();
 }
 
 bool enb::init(all_args_t *args_)
 {
   args     = args_;
 
-  logger.init(args->log.filename);
+#ifndef LOG_STDOUT
+    logger.init(args->log.filename);
+#endif
   rf_log.init("RF  ", &logger);
   
   // Create array of pointers to phy_logs 
@@ -87,7 +89,9 @@ bool enb::init(all_args_t *args_)
   s1ap_log.init("S1AP", &logger);
 
   // Init logs
-  logger.log("\n\n");
+#ifndef LOG_STDOUT
+    logger.log("\n\n");
+#endif
   rf_log.set_level(srslte::LOG_LEVEL_INFO);
   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));
diff --git a/srsenb/src/mac/mac.cc b/srsenb/src/mac/mac.cc
index a1e9fb21b..284cef3e7 100644
--- a/srsenb/src/mac/mac.cc
+++ b/srsenb/src/mac/mac.cc
@@ -83,12 +83,16 @@ bool mac::init(mac_args_t *args_, srslte_cell_t *cell_, phy_interface_mac *phy,
     reset();
 
     started = true; 
-  }    
+  }
+
   return started; 
 }
 
 void mac::stop()
 {
+  for (uint32_t i=0;i<ue_db.size();i++) {
+    delete ue_db[i];
+  }
   for (int i=0;i<NOF_BCCH_DLSCH_MSG;i++) {
     srslte_softbuffer_tx_free(&bcch_softbuffer_tx[i]);
   }  
diff --git a/srsenb/src/mac/scheduler.cc b/srsenb/src/mac/scheduler.cc
index 325970b83..79cf3f476 100644
--- a/srsenb/src/mac/scheduler.cc
+++ b/srsenb/src/mac/scheduler.cc
@@ -26,6 +26,12 @@ sched::sched()
   reset();
 }
 
+sched::~sched()
+{
+  srslte_regs_free(&regs);
+  pthread_mutex_destroy(&mutex);
+}
+
 void sched::init(rrc_interface_mac *rrc_, srslte::log* log)
 {
   sched_cfg.pdsch_max_mcs = 28; 
diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index 9a258a1a3..b9a3093c2 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -397,7 +397,7 @@ int sched_ue::generate_format1(dl_harq_proc *h,
     if (fixed_mcs_dl < 0) {
       tbs = alloc_tbs_dl(nof_prb, nof_re, req_bytes, &mcs);
     } else {
-      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_dl), nof_prb);
+      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_dl), nof_prb)/8;
       mcs = fixed_mcs_dl; 
     }
 
@@ -468,7 +468,7 @@ int sched_ue::generate_format0(ul_harq_proc *h,
     if (fixed_mcs_ul < 0) {
       tbs = alloc_tbs_ul(allocation.L, nof_re, req_bytes, &mcs);
     } else {
-      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_ul), allocation.L);
+      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_ul), allocation.L)/8;
       mcs = fixed_mcs_ul;
     }
     
@@ -607,12 +607,12 @@ uint32_t sched_ue::get_required_prb_dl(uint32_t req_bytes, uint32_t nof_ctrl_sym
   
   uint32_t nof_re = 0; 
   int tbs = 0; 
-  for (n=1;n<cell.nof_prb && nbytes < req_bytes;n++) {
+  for (n=1;n<=cell.nof_prb && nbytes < req_bytes;n++) {
     nof_re = srslte_ra_dl_approx_nof_re(cell, n, nof_ctrl_symbols);
     if (fixed_mcs_dl < 0) {
       tbs = alloc_tbs_dl(n, nof_re, 0, &mcs);
     } else {
-      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_dl), n);
+      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_dl), n)/8;
     }
     if (tbs > 0) {
       nbytes = tbs; 
@@ -635,13 +635,13 @@ uint32_t sched_ue::get_required_prb_ul(uint32_t req_bytes)
     return 0; 
   }
   
-  for (n=1;n<cell.nof_prb && nbytes < req_bytes + 4;n++) {
+  for (n=1;n<=cell.nof_prb && nbytes < req_bytes + 4;n++) {
     uint32_t nof_re = (2*(SRSLTE_CP_NSYMB(cell.cp)-1) - N_srs)*n*SRSLTE_NRE;
     int tbs = 0; 
     if (fixed_mcs_ul < 0) {
       tbs = alloc_tbs_ul(n, nof_re, 0, &mcs);
     } else {
-      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_ul), n);
+      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_ul), n)/8;
     }
     if (tbs > 0) {
       nbytes = tbs; 
@@ -814,7 +814,7 @@ int sched_ue::alloc_tbs(uint32_t nof_prb,
   uint32_t max_Qm  = is_ul?4:6; // Allow 16-QAM in PUSCH Only
 
   // TODO: Compute real spectral efficiency based on PUSCH-UCI configuration
-  if (has_pucch) {
+  if (has_pucch && is_ul) {
     cqi-=2;
   }
 
diff --git a/srsenb/src/phy/phch_worker.cc b/srsenb/src/phy/phch_worker.cc
index 73aaa0f54..ab7df91af 100644
--- a/srsenb/src/phy/phch_worker.cc
+++ b/srsenb/src/phy/phch_worker.cc
@@ -50,6 +50,9 @@ using namespace std;
 #include "srsgui/srsgui.h"
 #include <semaphore.h>
 #include <srslte/phy/phch/ra.h>
+#include <srslte/srslte.h>
+#include <srslte/phy/phch/pdsch.h>
+#include <srslte/phy/common/sequence.h>
 
 void init_plots(srsenb::phch_worker *worker);
 pthread_t plot_thread; 
@@ -95,22 +98,29 @@ void phch_worker::init(phch_common* phy_, srslte::log *log_h_)
     fprintf(stderr, "Error allocating memory\n");
     return; 
   }
-  if (srslte_enb_dl_init(&enb_dl, phy->cell)) {
+  if (srslte_enb_dl_init(&enb_dl, phy->cell.nof_prb)) {
     fprintf(stderr, "Error initiating ENB DL\n");
     return;
   }
+  if (srslte_enb_dl_set_cell(&enb_dl, phy->cell)) {
+    fprintf(stderr, "Error initiating ENB DL\n");
+    return;
+  }
+  if (srslte_enb_ul_init(&enb_ul, phy->cell.nof_prb)) {
+    fprintf(stderr, "Error initiating ENB UL\n");
+    return;
+  }
+  if (srslte_enb_ul_set_cell(&enb_ul,
+                              phy->cell,
+                              NULL,
+                              &phy->pusch_cfg,
+                              &phy->hopping_cfg,
+                              &phy->pucch_cfg))
+  {
+    fprintf(stderr, "Error initiating ENB UL\n");
+    return;
+  }
   
-  if (srslte_enb_ul_init(&enb_ul, 
-                          phy->cell, 
-                          NULL, 
-                          &phy->pusch_cfg, 
-                          &phy->hopping_cfg,
-                          &phy->pucch_cfg)) 
-  {
-    fprintf(stderr, "Error initiating ENB DL\n");
-    return;
-  }
-
   /* Setup SI-RNTI in PHY */
   add_rnti(SRSLTE_SIRNTI);
 
@@ -129,12 +139,29 @@ void phch_worker::init(phch_common* phy_, srslte::log *log_h_)
   Info("Worker %d configured cell %d PRB\n", get_id(), phy->cell.nof_prb);
   
   initiated = true; 
-  
+  running   = true;
+
 #ifdef DEBUG_WRITE_FILE
   f = fopen("test.dat", "w");
 #endif
 }
 
+void phch_worker::stop()
+{
+  running = false;
+  pthread_mutex_lock(&mutex);
+
+  srslte_enb_dl_free(&enb_dl);
+  srslte_enb_ul_free(&enb_ul);
+  if (signal_buffer_rx) {
+    free(signal_buffer_rx);
+  }
+  if (signal_buffer_tx) {
+    free(signal_buffer_tx);
+  }
+  pthread_mutex_unlock(&mutex);
+  pthread_mutex_destroy(&mutex);
+}
 void phch_worker::reset() 
 {
   initiated  = false; 
@@ -160,14 +187,14 @@ void phch_worker::set_time(uint32_t tti_, uint32_t tx_mutex_cnt_, srslte_timesta
 
 int phch_worker::add_rnti(uint16_t rnti)
 {
-  
+
   if (srslte_enb_dl_add_rnti(&enb_dl, rnti)) {
     return -1; 
   }
   if (srslte_enb_ul_add_rnti(&enb_ul, rnti)) {
     return -1; 
   }
-  
+
   // Create user 
   ue_db[rnti].rnti = rnti; 
     
@@ -238,9 +265,13 @@ void phch_worker::rem_rnti(uint16_t rnti)
 
 void phch_worker::work_imp()
 {
-  uint32_t sf_ack; 
-  
-  pthread_mutex_lock(&mutex); 
+  uint32_t sf_ack;
+
+  if (!running) {
+    return;
+  }
+
+  pthread_mutex_lock(&mutex);
   
   mac_interface_phy::ul_sched_t *ul_grants = phy->ul_grants;
   mac_interface_phy::dl_sched_t *dl_grants = phy->dl_grants; 
@@ -255,10 +286,10 @@ void phch_worker::work_imp()
     ue_db[rnti].has_grant_tti = -1; 
   }
 
-  // Process UL signal 
+  // Process UL signal
   srslte_enb_ul_fft(&enb_ul, signal_buffer_rx);
 
-  // Decode pending UL grants for the tti they were scheduled 
+  // Decode pending UL grants for the tti they were scheduled
   decode_pusch(ul_grants[sf_rx].sched_grants, ul_grants[sf_rx].nof_grants, sf_rx);
   
   // Decode remaining PUCCH ACKs not associated with PUSCH transmission and SR signals
@@ -327,7 +358,7 @@ void phch_worker::work_imp()
   }
 #endif
 
-unlock: 
+unlock:
   pthread_mutex_unlock(&mutex); 
 
 }
@@ -659,8 +690,10 @@ int phch_worker::encode_pdsch(srslte_enb_dl_pdsch_t *grants, uint32_t nof_grants
                              phy_grant.mcs[0].tbs/8, phy_grant.mcs[0].idx, grants[i].grant.rv_idx, tti_tx);
       }
       srslte_softbuffer_tx_t *sb[SRSLTE_MAX_CODEWORDS] = {grants[i].softbuffer, NULL};
-      uint8_t *d[SRSLTE_MAX_CODEWORDS] = {grants[i].data, NULL};
-      if (srslte_enb_dl_put_pdsch(&enb_dl, &phy_grant, sb, rnti, grants[i].grant.rv_idx, sf_idx, d))
+      uint8_t                 *d[SRSLTE_MAX_CODEWORDS] = {grants[i].data, NULL};
+      int                     rv[SRSLTE_MAX_CODEWORDS] = {grants[i].grant.rv_idx, 0};
+
+      if (srslte_enb_dl_put_pdsch(&enb_dl, &phy_grant, sb, rnti, rv, sf_idx, d, SRSLTE_MIMO_TYPE_SINGLE_ANTENNA, 0))
       {
         fprintf(stderr, "Error putting PDSCH %d\n",i);
         return SRSLTE_ERROR; 
diff --git a/srsenb/src/phy/phy.cc b/srsenb/src/phy/phy.cc
index bc4f3173e..c0d0a73c9 100644
--- a/srsenb/src/phy/phy.cc
+++ b/srsenb/src/phy/phy.cc
@@ -134,6 +134,9 @@ void phy::stop()
 {  
   tx_rx.stop();  
   workers_common.stop();
+  for (uint32_t i=0;i<nof_workers;i++) {
+    workers[i].stop();
+  }
   workers_pool.stop();
   prach.stop();
 }
diff --git a/srsenb/src/phy/prach_worker.cc b/srsenb/src/phy/prach_worker.cc
index 15b42c374..b4bd7da3f 100644
--- a/srsenb/src/phy/prach_worker.cc
+++ b/srsenb/src/phy/prach_worker.cc
@@ -24,6 +24,7 @@
  *
  */
 
+#include "srslte/srslte.h"
 #include "phy/prach_worker.h"
 
 namespace srsenb {
@@ -48,7 +49,7 @@ int prach_worker::init(srslte_cell_t *cell_, srslte_prach_cfg_t *prach_cfg_, mac
   srslte_prach_set_detect_factor(&prach, 60);    
 
   nof_sf = (uint32_t) ceilf(prach.T_tot*1000); 
-  
+
   signal_buffer_rx = (cf_t*) srslte_vec_malloc(sizeof(cf_t)*nof_sf*SRSLTE_SF_LEN_PRB(cell.nof_prb));
   if (!signal_buffer_rx) {
     perror("malloc");
@@ -66,6 +67,11 @@ int prach_worker::init(srslte_cell_t *cell_, srslte_prach_cfg_t *prach_cfg_, mac
 
 void prach_worker::stop()
 {
+  srslte_prach_free(&prach);
+
+  if (signal_buffer_rx) {
+    free(signal_buffer_rx);
+  }
   pthread_mutex_lock(&mutex);
   processed_tti = 99999; 
   running = false; 
@@ -109,7 +115,7 @@ int prach_worker::run_tti(uint32_t tti_rx)
 {
   if (srslte_prach_tti_opportunity(&prach, tti_rx, -1)) 
   {
-    // Detect possible PRACHs 
+    // Detect possible PRACHs
     if (srslte_prach_detect_offset(&prach,
                                    prach_cfg.freq_offset,
                                    &signal_buffer_rx[prach.N_cp],
diff --git a/srsenb/src/upper/rrc.cc b/srsenb/src/upper/rrc.cc
index 44b3d9a89..f803500f2 100644
--- a/srsenb/src/upper/rrc.cc
+++ b/srsenb/src/upper/rrc.cc
@@ -26,6 +26,7 @@
 
 #include <srslte/interfaces/sched_interface.h>
 #include <srslte/asn1/liblte_rrc.h>
+#include <upper/rrc.h>
 #include "srslte/asn1/liblte_mme.h"
 #include "upper/rrc.h"
 
@@ -149,6 +150,7 @@ uint32_t rrc::generate_sibs()
     srslte_bit_pack_vector(bitbuffer.msg, sib_buffer[i].msg, bitbuffer.N_bits);
     sib_buffer[i].N_bytes = (bitbuffer.N_bits-1)/8+1;
   }
+  free(msg);
   return nof_messages; 
 }
 
@@ -604,23 +606,32 @@ void rrc::run_thread()
     if (p.pdu) {
       rrc_log->info_hex(p.pdu->msg, p.pdu->N_bytes, "Rx %s PDU", rb_id_text[p.lcid]);
     }
-    switch(p.lcid)
-    {
-    case RB_ID_SRB0:
-      parse_ul_ccch(p.rnti, p.pdu);
-      break;
-    case RB_ID_SRB1:
-    case RB_ID_SRB2:
-      parse_ul_dcch(p.rnti, p.lcid, p.pdu);
-      break;
-    case LCID_REM_USER:
-      usleep(10000);
-      rem_user(p.rnti);
-      break;
-    default:
-      rrc_log->error("Rx PDU with invalid bearer id: %s", p.lcid);
-      break;
+    pthread_mutex_lock(&user_mutex);
+    if (users.count(p.rnti) == 1) {
+      switch(p.lcid)
+      {
+        case RB_ID_SRB0:
+          parse_ul_ccch(p.rnti, p.pdu);
+          break;
+        case RB_ID_SRB1:
+        case RB_ID_SRB2:
+          parse_ul_dcch(p.rnti, p.lcid, p.pdu);
+          break;
+        case LCID_REM_USER:
+          pthread_mutex_unlock(&user_mutex);
+          usleep(10000);
+          rem_user(p.rnti);
+          pthread_mutex_lock(&user_mutex);
+          break;
+        default:
+          rrc_log->error("Rx PDU with invalid bearer id: %s", p.lcid);
+          break;
+      }
+    } else {
+      printf("Discarting rnti=0x%xn", p.rnti);
+      rrc_log->warning("Discarting PDU for removed rnti=0x%x\n", p.rnti);
     }
+    pthread_mutex_unlock(&user_mutex);
   }
 }
 void rrc::activity_monitor::run_thread()
diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index 963551150..e897ac76f 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -166,8 +166,17 @@ private:
     }
 
     void new_grant_dl(Tgrant grant, Taction *action) {
-      for (uint32_t tb = 0; tb < grant.phy_grant.dl.nof_tb; tb++) {
-        subproc[tb].new_grant_dl(grant, action);
+      /* Fill action structure */
+      bzero(action, sizeof(Taction));
+      action->default_ack = false;
+      action->generate_ack = true;
+      action->decode_enabled = false;
+
+      /* For each subprocess... */
+      for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+        if (grant.tb_en[tb]) {
+          subproc[tb].new_grant_dl(grant, action);
+        }
       }
     }
 
@@ -188,6 +197,12 @@ private:
         bzero(&cur_grant, sizeof(Tgrant));
       }
 
+      ~dl_tb_process() {
+        if (is_initiated) {
+          srslte_softbuffer_rx_free(&softbuffer);
+        }
+      }
+
       bool init(uint32_t pid_, dl_harq_entity *parent, uint32_t tb_idx) {
         tid = tb_idx;
         if (srslte_softbuffer_rx_init(&softbuffer, 110)) {
@@ -235,12 +250,6 @@ private:
         grant.last_tti = cur_grant.tti;
         memcpy(&cur_grant, &grant, sizeof(Tgrant));
 
-        // Fill action structure
-        bzero(action, sizeof(Taction));
-        action->default_ack = ack;
-        action->generate_ack = true;
-        action->decode_enabled = false;
-
         // If data has not yet been successfully decoded
         if (!ack) {
 
@@ -262,6 +271,7 @@ private:
 
         } else {
           Warning("DL PID %d: Received duplicate TB. Discarting and retransmitting ACK\n", pid);
+          action->phy_grant.dl.tb_en[tid] = false;
         }
 
         if (pid == HARQ_BCCH_PID || harq_entity->timers_db->get(TIME_ALIGNMENT)->is_expired()) {
@@ -295,20 +305,18 @@ private:
               harq_entity->pcap->write_dl_crnti(payload_buffer_ptr, cur_grant.n_bytes[tid], cur_grant.rnti, ack,
                                                 cur_grant.tti);
             }
-            if (ack) {
-              if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP) {
-                Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (Temporal C-RNTI)\n",
-                      cur_grant.n_bytes[tid]);
-                harq_entity->demux_unit->push_pdu_temp_crnti(payload_buffer_ptr, cur_grant.n_bytes[tid]);
-              } else {
-                Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit\n", cur_grant.n_bytes[tid]);
-                harq_entity->demux_unit->push_pdu(pid * SRSLTE_MAX_TB + tid, payload_buffer_ptr, cur_grant.n_bytes[tid],
-                                                  cur_grant.tti);
+            if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP) {
+              Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (Temporal C-RNTI)\n",
+                    cur_grant.n_bytes[tid]);
+              harq_entity->demux_unit->push_pdu_temp_crnti(payload_buffer_ptr, cur_grant.n_bytes[tid]);
+            } else {
+              Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit\n", cur_grant.n_bytes[tid]);
+              harq_entity->demux_unit->push_pdu(pid * SRSLTE_MAX_TB + tid, payload_buffer_ptr, cur_grant.n_bytes[tid],
+                                                cur_grant.tti);
 
-                // Compute average number of retransmissions per packet
-                harq_entity->average_retx = SRSLTE_VEC_CMA((float) n_retx, harq_entity->average_retx,
-                                                           harq_entity->nof_pkts++);
-              }
+              // Compute average number of retransmissions per packet
+              harq_entity->average_retx = SRSLTE_VEC_CMA((float) n_retx, harq_entity->average_retx,
+                                                         harq_entity->nof_pkts++);
             }
           }
         } else {
diff --git a/srsue/hdr/mac/mac.h b/srsue/hdr/mac/mac.h
index f2785e288..51bb467f2 100644
--- a/srsue/hdr/mac/mac.h
+++ b/srsue/hdr/mac/mac.h
@@ -153,7 +153,6 @@ private:
   
   // pointer to MAC PCAP object
   srslte::mac_pcap* pcap;
-  bool signals_pregenerated;
   bool is_first_ul_grant;
 
 
diff --git a/srsue/hdr/mac/proc_ra.h b/srsue/hdr/mac/proc_ra.h
index 6863371c0..c1bdf3644 100644
--- a/srsue/hdr/mac/proc_ra.h
+++ b/srsue/hdr/mac/proc_ra.h
@@ -71,6 +71,9 @@ class ra_proc : public srslte::timer_callback
       rar_grant_tti             = 0; 
       msg3_flushed              = false; 
     };
+
+    ~ra_proc();
+
     void init(phy_interface_mac *phy_h, 
               rrc_interface_mac *rrc_, 
               srslte::log *log_h, 
diff --git a/srsue/hdr/mac/ul_harq.h b/srsue/hdr/mac/ul_harq.h
index 952b9f002..e57af77ba 100644
--- a/srsue/hdr/mac/ul_harq.h
+++ b/srsue/hdr/mac/ul_harq.h
@@ -163,9 +163,20 @@ private:
       is_initiated = false;
       is_grant_configured = false;
       tti_last_tx = 0;
+      payload_buffer = NULL;
       bzero(&cur_grant, sizeof(Tgrant));
     }
 
+    ~ul_harq_process()
+    {
+      if (is_initiated) {
+        if (payload_buffer) {
+          free(payload_buffer);
+        }
+        srslte_softbuffer_tx_free(&softbuffer);
+      }
+    }
+
     bool init(uint32_t pid_, ul_harq_entity *parent)
     {
       if (srslte_softbuffer_tx_init(&softbuffer, 110)) {
@@ -358,7 +369,8 @@ private:
         current_irv = 0;
         is_msg3 = is_msg3_;
         Info("UL %d:  New TX%s, RV=%d, TBS=%d, RNTI=%d\n",
-             pid, is_msg3?" for Msg3":"", get_rv(), cur_grant.n_bytes[0], cur_grant.rnti);
+             pid, is_msg3?" for Msg3":"", get_rv(), cur_grant.n_bytes[0],
+             is_msg3?harq_entity->rntis->temp_rnti:cur_grant.rnti);
         generate_tx(tti_tx, action);
       }
     }
diff --git a/srsue/hdr/phy/phch_recv.h b/srsue/hdr/phy/phch_recv.h
index 838d49d30..784d99abc 100644
--- a/srsue/hdr/phy/phch_recv.h
+++ b/srsue/hdr/phy/phch_recv.h
@@ -45,18 +45,24 @@ class phch_recv : public thread
 {
 public:
   phch_recv();
+  ~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);
   void stop();
   void set_agc_enable(bool enable);
 
-  void     resync_sfn(); 
-  
+  void     resync_sfn();
+
+  void    set_earfcn(std::vector<uint32_t> earfcn);
+
+  bool    stop_sync();
+  void    cell_search_start();
+  void    cell_search_next();
+  bool    cell_select(uint32_t earfcn, srslte_cell_t cell);
+
   uint32_t get_current_tti();
   
-  void    sync_start(); 
-  void    sync_stop();
   bool    status_is_sync();
 
   void    set_time_adv_sec(float time_adv_sec);
@@ -65,42 +71,65 @@ public:
   const static int MUTEX_X_WORKER = 4; 
 
 private:
-  
+
+  std::vector<uint32_t> earfcn;
+
   void   set_ue_sync_opts(srslte_ue_sync_t *q); 
   void   run_thread();
-  int    sync_sfn();
-  
+
+  void   set_sampling_rate();
+  bool   set_frequency();
+
+  void   cell_search_inc();
+
+  bool init_cell();
+  void free_cell();
+
   bool   running; 
   
-  srslte::radio_multi        *radio_h;
+  srslte::radio_multi  *radio_h;
   mac_interface_phy    *mac;
-  rrc_interface_phy *rrc;
+  rrc_interface_phy    *rrc;
   srslte::log          *log_h;
   srslte::thread_pool  *workers_pool;
   phch_common          *worker_com;
   prach                *prach_buffer;
-  
+
+  // Structures for Cell Camp
   srslte_ue_sync_t    ue_sync;
   srslte_ue_mib_t     ue_mib;
-  
+
+  // Structures for Cell Search
+  srslte_ue_cellsearch_t cs;
+  srslte_ue_mib_sync_t   ue_mib_sync;
+
   uint32_t      nof_rx_antennas;
 
-  cf_t *sf_buffer_sfn[SRSLTE_MAX_PORTS]; 
+  cf_t *sf_buffer[SRSLTE_MAX_PORTS];
 
   // Sync metrics
   sync_metrics_t metrics;
 
   enum {
-    IDLE, CELL_SEARCH, SYNCING, SYNC_DONE
-  } phy_state; 
+    IDLE = 0,
+    CELL_SEARCH,
+    CELL_SELECT,
+    CELL_MEASURE,
+    CELL_CAMP
+  } phy_state;
 
-  srslte_cell_t cell; 
+  bool is_in_idle;
+
+  enum {
+    SRATE_NONE=0, SRATE_FIND, SRATE_CAMP
+  } srate_mode;
+
+  srslte_cell_t cell;
   bool          cell_is_set;
   bool          is_sfn_synched; 
   bool          started; 
   float         time_adv_sec;
-  bool          radio_is_streaming;
-  uint32_t      tti; 
+  uint32_t      tti;
   bool          do_agc;
   
   float         last_gain;
@@ -108,13 +137,24 @@ private:
   uint32_t      nof_tx_mutex;
   uint32_t      tx_mutex_cnt;
 
+  uint32_t      current_earfcn;
+
   uint32_t      sync_sfn_cnt;
-  const static uint32_t SYNC_SFN_TIMEOUT = 5000;
+  const static uint32_t SYNC_SFN_TIMEOUT = 200;
   float ul_dl_factor;
-  
-  bool          cell_search(int force_N_id_2 = -1);
-  bool          init_cell();
-  void          free_cell();
+  int cur_earfcn_index;
+  bool cell_search_in_progress;
+  uint32_t measure_cnt;
+  float    measure_rsrp;
+  srslte_ue_dl_t ue_dl_measure;
+
+
+  const static int RSRP_MEASURE_NOF_FRAMES = 5;
+
+  int    cell_sync_sfn();
+  int    cell_meas_rsrp();
+  bool   cell_search(int force_N_id_2 = -1);
+  bool   set_cell();
 };
 
 } // namespace srsue
diff --git a/srsue/hdr/phy/phch_worker.h b/srsue/hdr/phy/phch_worker.h
index 7b196f948..e6fce4f02 100644
--- a/srsue/hdr/phy/phch_worker.h
+++ b/srsue/hdr/phy/phch_worker.h
@@ -42,11 +42,13 @@ class phch_worker : public srslte::thread_pool::worker
 public:
   
   phch_worker();
+  ~phch_worker();
   void  reset(); 
   void  set_common(phch_common *phy);
-  bool  init_cell(srslte_cell_t cell);
-  void  free_cell();
-  
+  bool  init(uint32_t max_prb);
+
+  bool  set_cell(srslte_cell_t cell);
+
   /* Functions used by main PHY thread */
   cf_t* get_buffer(uint32_t antenna_idx);
   void  set_tti(uint32_t tti, uint32_t tx_tti); 
@@ -95,7 +97,7 @@ private:
   void set_uci_sr();
   void set_uci_periodic_cqi();
   void set_uci_aperiodic_cqi();
-  void set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], uint32_t nof_tb);
+  void set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], bool tb_en[SRSLTE_MAX_CODEWORDS]);
   bool srs_is_ready_to_send();
   float set_power(float tx_power);
   void setup_tx_gain();
@@ -111,14 +113,15 @@ private:
   
   /* Common objects */  
   phch_common    *phy;
-  srslte_cell_t  cell; 
-  bool           cell_initiated; 
+  srslte_cell_t  cell;
+  bool           mem_initiated;
+  bool           cell_initiated;
   cf_t          *signal_buffer[SRSLTE_MAX_PORTS]; 
   uint32_t       tti; 
   uint32_t       tx_tti;
   bool           pregen_enabled;
   uint32_t       last_dl_pdcch_ncce;
-  bool           rnti_is_set; 
+  bool           rnti_is_set;
   
   /* Objects for DL */
   srslte_ue_dl_t ue_dl; 
diff --git a/srsue/hdr/phy/phy.h b/srsue/hdr/phy/phy.h
index b478bbcdd..c8d88ee44 100644
--- a/srsue/hdr/phy/phy.h
+++ b/srsue/hdr/phy/phy.h
@@ -46,6 +46,7 @@ typedef _Complex float cf_t;
 class phy
     : public phy_interface_mac
     , public phy_interface_rrc
+    , public thread
 {
 public:
   phy();
@@ -57,6 +58,9 @@ public:
   
   void stop();
 
+  void wait_initialize();
+  bool is_initiated();
+
   void set_agc_enable(bool enabled);
 
   void get_metrics(phy_metrics_t &m);
@@ -69,18 +73,22 @@ public:
   void enable_pregen_signals(bool enable); 
   
   void start_trace();
-  void write_trace(std::string filename); 
-  
+  void write_trace(std::string filename);
+
+  void set_earfcn(std::vector<uint32_t> earfcns);
+
   /********** RRC INTERFACE ********************/
   void    reset();
-  bool    status_is_sync();
   void    configure_ul_params(bool pregen_disabled = false);
-  void    resync_sfn(); 
-  
+  void    resync_sfn();
+  void    cell_search_start();
+  void    cell_search_next();
+  bool    cell_select(uint32_t earfcn, srslte_cell_t phy_cell);
+
   /********** MAC INTERFACE ********************/
   /* Functions to synchronize with a cell */
-  void    sync_start(); 
-  void    sync_stop();
+  bool    sync_status(); // this is also RRC interface
+  bool    sync_stop();
 
   /* Sets a C-RNTI allowing the PHY to pregenerate signals if necessary */
   void set_crnti(uint16_t rnti);
@@ -127,7 +135,10 @@ public:
   void    start_plot();
     
 private:
-    
+
+  void run_thread();
+
+  bool     initiated;
   uint32_t nof_workers; 
   
   const static int MAX_WORKERS         = 4;
@@ -136,8 +147,10 @@ private:
   const static int SF_RECV_THREAD_PRIO = 1;
   const static int WORKERS_THREAD_PRIO = 0; 
   
-  srslte::radio_multi   *radio_handler;
-  srslte::log           *log_h;
+  srslte::radio_multi      *radio_handler;
+  srslte::log              *log_h;
+  srsue::mac_interface_phy *mac;
+  srsue::rrc_interface_phy *rrc;
 
   srslte::thread_pool      workers_pool;
   std::vector<phch_worker> workers;
diff --git a/srsue/hdr/phy/prach.h b/srsue/hdr/phy/prach.h
index 694353926..67ccc0c94 100644
--- a/srsue/hdr/phy/prach.h
+++ b/srsue/hdr/phy/prach.h
@@ -45,14 +45,15 @@ namespace srsue {
       
       args              = NULL; 
       config            = NULL; 
-      initiated         = false; 
-      signal_buffer     = NULL; 
+      signal_buffer     = NULL;
       transmitted_tti   = 0; 
-      target_power_dbm  = 0; 
+      target_power_dbm  = 0;
+      mem_initiated     = false;
+      cell_initiated    = false;
     }
-    void           init(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT *config, phy_args_t *args, srslte::log *log_h);
-    bool           init_cell(srslte_cell_t cell);
-    void           free_cell();
+    ~prach();
+    void           init(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT *config, uint32_t max_prb, phy_args_t *args, srslte::log *log_h);
+    bool           set_cell(srslte_cell_t cell);
     bool           prepare_to_send(uint32_t preamble_idx, int allowed_subframe = -1, float target_power_dbm = -1);
     bool           is_ready_to_send(uint32_t current_tti);
     int            tx_tti();
@@ -70,7 +71,8 @@ namespace srsue {
     srslte::log   *log_h;
     int            preamble_idx;  
     int            allowed_subframe; 
-    bool           initiated;   
+    bool           mem_initiated;
+    bool           cell_initiated;
     uint32_t       len; 
     cf_t          *buffer[64]; 
     srslte_prach_t prach_obj; 
diff --git a/srsue/hdr/ue.h b/srsue/hdr/ue.h
index c69dbddeb..11565ec00 100644
--- a/srsue/hdr/ue.h
+++ b/srsue/hdr/ue.h
@@ -57,6 +57,8 @@
 
 namespace srsue {
 
+//#define LOG_STDOUT
+
 /*******************************************************************************
   Main UE class
 *******************************************************************************/
@@ -71,7 +73,7 @@ public:
   void stop();
   bool is_attached();
   void start_plot();
-  
+
   static void rf_msg(srslte_rf_error_t error);
   void handle_rf_msg(srslte_rf_error_t error);
 
@@ -79,10 +81,10 @@ public:
   bool get_metrics(ue_metrics_t &m);
 
   void pregenerate_signals(bool enable);
-  
+
   // Testing
-  void test_con_restablishment(); 
-  
+  void test_con_restablishment();
+
 
 private:
   virtual ~ue();
@@ -98,7 +100,11 @@ private:
   srslte::gw         gw;
   srsue::usim        usim;
 
-  srslte::logger_file logger;
+#ifdef LOG_STDOUT
+    srslte::logger_stdout logger;
+#else
+    srslte::logger_file logger;
+#endif
   srslte::log_filter  rf_log;
   srslte::log_filter  phy_log;
   srslte::log_filter  mac_log;
diff --git a/srsue/hdr/ue_base.h b/srsue/hdr/ue_base.h
index 4720d8062..b105b2088 100644
--- a/srsue/hdr/ue_base.h
+++ b/srsue/hdr/ue_base.h
@@ -52,15 +52,17 @@ namespace srsue {
 *******************************************************************************/
 
 typedef struct {
+  uint32_t      dl_earfcn;
   float         dl_freq;
   float         ul_freq;
+  float         freq_offset;
   float         rx_gain;
   float         tx_gain;
-  uint32_t      nof_rx_ant; 
-  std::string   device_name; 
-  std::string   device_args; 
-  std::string   time_adv_nsamples; 
-  std::string   burst_preamble; 
+  uint32_t      nof_rx_ant;
+  std::string   device_name;
+  std::string   device_args;
+  std::string   time_adv_nsamples;
+  std::string   burst_preamble;
 }rf_args_t;
 
 typedef struct {
diff --git a/srsue/hdr/upper/nas.h b/srsue/hdr/upper/nas.h
index 32afd52bb..0f9b36bf8 100644
--- a/srsue/hdr/upper/nas.h
+++ b/srsue/hdr/upper/nas.h
@@ -39,16 +39,16 @@ using srslte::byte_buffer_t;
 namespace srsue {
 
 // EMM states (3GPP 24.302 v10.0.0)
-typedef enum{
-    EMM_STATE_NULL = 0,
-    EMM_STATE_DEREGISTERED,
-    EMM_STATE_REGISTERED_INITIATED,
-    EMM_STATE_REGISTERED,
-    EMM_STATE_SERVICE_REQUEST_INITIATED,
-    EMM_STATE_DEREGISTERED_INITIATED,
-    EMM_STATE_TAU_INITIATED,
-    EMM_STATE_N_ITEMS,
-}emm_state_t;
+typedef enum {
+  EMM_STATE_NULL = 0,
+  EMM_STATE_DEREGISTERED,
+  EMM_STATE_REGISTERED_INITIATED,
+  EMM_STATE_REGISTERED,
+  EMM_STATE_SERVICE_REQUEST_INITIATED,
+  EMM_STATE_DEREGISTERED_INITIATED,
+  EMM_STATE_TAU_INITIATED,
+  EMM_STATE_N_ITEMS,
+} emm_state_t;
 static const char emm_state_text[EMM_STATE_N_ITEMS][100] = {"NULL",
                                                             "DEREGISTERED",
                                                             "REGISTERED INITIATED",
@@ -57,9 +57,13 @@ static const char emm_state_text[EMM_STATE_N_ITEMS][100] = {"NULL",
                                                             "DEREGISTERED INITIATED",
                                                             "TRACKING AREA UPDATE INITIATED"};
 
+typedef enum {
+  PLMN_NOT_SELECTED = 0,
+  PLMN_SELECTED
+} plmn_selection_state_t;
+
 class nas
-    :public nas_interface_rrc
-{
+  : public nas_interface_rrc, public nas_interface_ue {
 public:
   nas();
   void init(usim_interface_nas  *usim_,
@@ -72,71 +76,103 @@ public:
   emm_state_t get_state();
 
   // RRC interface
-  void      notify_connection_setup();
-  void      write_pdu(uint32_t lcid, byte_buffer_t *pdu);
-  uint32_t  get_ul_count();
-  bool      is_attached();
-  bool      get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi);
-  
+  void notify_connection_setup();
+
+  void write_pdu(uint32_t lcid, byte_buffer_t *pdu);
+
+  uint32_t get_ul_count();
+
+  bool is_attached();
+
+  bool get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi);
+
+  void plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code);
+
+  // UE interface
+  void attach_request();
+
+  void deattach_request();
+
 private:
   srslte::byte_buffer_pool *pool;
-  srslte::log        *nas_log;
-  rrc_interface_nas  *rrc;
+  srslte::log *nas_log;
+  rrc_interface_nas *rrc;
   usim_interface_nas *usim;
-  gw_interface_nas   *gw;
+  gw_interface_nas *gw;
+
   srslte::srslte_nas_config_t cfg;
 
-  emm_state_t        state;
-   
+  emm_state_t state;
+
+  plmn_selection_state_t plmn_selection;
+  LIBLTE_RRC_PLMN_IDENTITY_STRUCT current_plmn;
+  LIBLTE_RRC_PLMN_IDENTITY_STRUCT home_plmn;
+
+  std::vector<LIBLTE_RRC_PLMN_IDENTITY_STRUCT > known_plmns;
+
   // Save short MAC
-  
+
   // Identifiers
   LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT guti;
-  bool                                 is_guti_set;
+  bool is_guti_set;
 
   uint32_t ip_addr;
-  uint8_t  eps_bearer_id;
+  uint8_t eps_bearer_id;
 
-  uint8_t  transaction_id;
+  uint8_t transaction_id;
 
   // NAS counters - incremented for each security-protected message recvd/sent
   uint32_t count_ul;
   uint32_t count_dl;
 
   // Security
-  uint8_t  ksi;
-  uint8_t  k_nas_enc[32];
-  uint8_t  k_nas_int[32];
+  uint8_t ksi;
+  uint8_t k_nas_enc[32];
+  uint8_t k_nas_int[32];
 
   srslte::CIPHERING_ALGORITHM_ID_ENUM cipher_algo;
   srslte::INTEGRITY_ALGORITHM_ID_ENUM integ_algo;
 
-  void integrity_generate(uint8_t  *key_128,
-                          uint32_t  count,
-                          uint8_t   rb_id,
-                          uint8_t   direction,
-                          uint8_t  *msg,
-                          uint32_t  msg_len,
-                          uint8_t  *mac);
+  void integrity_generate(uint8_t *key_128,
+                          uint32_t count,
+                          uint8_t rb_id,
+                          uint8_t direction,
+                          uint8_t *msg,
+                          uint32_t msg_len,
+                          uint8_t *mac);
+
   void integrity_check();
+
   void cipher_encrypt();
+
   void cipher_decrypt();
 
   // Parsers
   void parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu);
+
   void parse_attach_reject(uint32_t lcid, byte_buffer_t *pdu);
+
   void parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu);
+
   void parse_authentication_reject(uint32_t lcid, byte_buffer_t *pdu);
+
   void parse_identity_request(uint32_t lcid, byte_buffer_t *pdu);
+
   void parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu);
+
   void parse_service_reject(uint32_t lcid, byte_buffer_t *pdu);
+
   void parse_esm_information_request(uint32_t lcid, byte_buffer_t *pdu);
+
   void parse_emm_information(uint32_t lcid, byte_buffer_t *pdu);
 
   // Senders
   void send_attach_request();
+
   void send_identity_response();
+
   void send_service_request();
+
   void send_esm_information_response();
 
   void gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg);
diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index 9cbdd5201..81514b559 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -35,6 +35,7 @@
 #include "srslte/common/common.h"
 #include "srslte/interfaces/ue_interfaces.h"
 #include "srslte/common/security.h"
+#include "srslte/common/threads.h"
 
 #include <map>
 
@@ -47,107 +48,155 @@ static std::string rb_id_str[] = {"SRB0", "SRB1", "SRB2",
                                     "DRB4","DRB5","DRB6",
                                     "DRB7","DRB8"};
 
-
 class rrc
   :public rrc_interface_nas
   ,public rrc_interface_phy
   ,public rrc_interface_mac
-  ,public rrc_interface_gw
   ,public rrc_interface_pdcp
   ,public rrc_interface_rlc
   ,public srslte::timer_callback
+  ,public thread
 {
 public:
   rrc();
-  void init(phy_interface_rrc     *phy_,
-            mac_interface_rrc     *mac_,
-            rlc_interface_rrc     *rlc_,
-            pdcp_interface_rrc    *pdcp_,
-            nas_interface_rrc     *nas_,
-            usim_interface_rrc    *usim_,
+
+  void init(phy_interface_rrc *phy_,
+            mac_interface_rrc *mac_,
+            rlc_interface_rrc *rlc_,
+            pdcp_interface_rrc *pdcp_,
+            nas_interface_rrc *nas_,
+            usim_interface_rrc *usim_,
             srslte::mac_interface_timers *mac_timers_,
-            srslte::log           *rrc_log_);
+            srslte::log *rrc_log_);
+
   void stop();
 
   rrc_state_t get_state();
-  void set_ue_category(int category); 
+
+  void set_ue_category(int category);
 
   // Timeout callback interface
   void timer_expired(uint32_t timeout_id);
 
-  void test_con_restablishment(); 
-  void liblte_rrc_log(char* str);
-  
+  void test_con_restablishment();
+
+  void liblte_rrc_log(char *str);
+
 private:
-  srslte::byte_buffer_pool  *pool;
-  srslte::log          *rrc_log;
-  phy_interface_rrc    *phy;
-  mac_interface_rrc    *mac;
-  rlc_interface_rrc    *rlc;
-  pdcp_interface_rrc   *pdcp;
-  nas_interface_rrc    *nas;
-  usim_interface_rrc   *usim;
+  srslte::byte_buffer_pool *pool;
+  srslte::log *rrc_log;
+  phy_interface_rrc *phy;
+  mac_interface_rrc *mac;
+  rlc_interface_rrc *rlc;
+  pdcp_interface_rrc *pdcp;
+  nas_interface_rrc *nas;
+  usim_interface_rrc *usim;
 
-  srslte::bit_buffer_t  bit_buf;
+  srslte::bit_buffer_t bit_buf;
 
-  pthread_mutex_t       mutex;
-  
-  rrc_state_t           state;
-  uint8_t               transaction_id;
-  bool                  drb_up;
+  pthread_mutex_t mutex;
 
-  uint8_t               k_rrc_enc[32];
-  uint8_t               k_rrc_int[32];
-  uint8_t               k_up_enc[32];
-  uint8_t               k_up_int[32];   // Not used: only for relay nodes (3GPP 33.401 Annex A.7)
+  rrc_state_t state;
+  uint8_t transaction_id;
+  bool drb_up;
+
+  // timeouts in ms
+
+  uint32_t connecting_timeout;
+  static const uint32_t RRC_CONNECTING_TIMEOUT = 1000;
+
+  uint32_t plmn_select_timeout;
+  static const uint32_t RRC_PLMN_SELECT_TIMEOUT = 10000;
+
+  uint32_t select_cell_timeout;
+  static const uint32_t RRC_SELECT_CELL_TIMEOUT = 2000;
+
+  uint8_t k_rrc_enc[32];
+  uint8_t k_rrc_int[32];
+  uint8_t k_up_enc[32];
+  uint8_t k_up_int[32];   // Not used: only for relay nodes (3GPP 33.401 Annex A.7)
 
   srslte::CIPHERING_ALGORITHM_ID_ENUM cipher_algo;
   srslte::INTEGRITY_ALGORITHM_ID_ENUM integ_algo;
 
-  LIBLTE_RRC_MIB_STRUCT                                 mib;
-  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT               sib1;
-  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT               sib2;
+  std::map<uint32_t, LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT> srbs;
+  std::map<uint32_t, LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT> drbs;
 
-  std::map<uint32_t, LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT>  srbs;
-  std::map<uint32_t, LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT>  drbs;
+  LIBLTE_RRC_DL_CCCH_MSG_STRUCT dl_ccch_msg;
+  LIBLTE_RRC_DL_DCCH_MSG_STRUCT dl_dcch_msg;
 
-  LIBLTE_RRC_DL_CCCH_MSG_STRUCT                         dl_ccch_msg;
-  LIBLTE_RRC_DL_DCCH_MSG_STRUCT                         dl_dcch_msg;
-
-  pthread_t             sib_search_thread;
-
-  // RRC constants and timers 
+  // RRC constants and timers
   srslte::mac_interface_timers *mac_timers;
-  uint32_t n310_cnt, N310; 
-  uint32_t n311_cnt, N311; 
+  uint32_t n310_cnt, N310;
+  uint32_t n311_cnt, N311;
   uint32_t t301, t310, t311;
-  uint32_t safe_reset_timer; 
+  uint32_t safe_reset_timer;
   int ue_category;
-    
-  
+
+  typedef struct {
+    uint32_t earfcn;
+    srslte_cell_t phy_cell;
+    float    rsrp;
+    bool     has_valid_sib1;
+    bool     has_valid_sib2;
+    bool     in_sync;
+    LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT sib1;
+    LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT sib2;
+  } cell_t;
+
+  std::vector<cell_t> known_cells;
+  cell_t *current_cell;
+
+
+  typedef enum {
+    SI_ACQUIRE_IDLE = 0,
+    SI_ACQUIRE_SIB1,
+    SI_ACQUIRE_SIB2
+  } si_acquire_state_t;
+
+  si_acquire_state_t si_acquire_state;
+
+  void select_next_cell_in_plmn();
+  LIBLTE_RRC_PLMN_IDENTITY_STRUCT selected_plmn_id;
+  int last_selected_cell;
+
+  bool thread_running;
+  void run_thread();
+
   // NAS interface
   void write_sdu(uint32_t lcid, byte_buffer_t *sdu);
+
   uint16_t get_mcc();
+
   uint16_t get_mnc();
+
   void enable_capabilities();
+  void plmn_search();
+  void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id);
 
   // PHY interface
   void in_sync();
+
   void out_of_sync();
+  void cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp);
 
   // MAC interface
   void release_pucch_srs();
-  void ra_problem(); 
+
+  void ra_problem();
 
   // GW interface
-  bool rrc_connected();
-  void rrc_connect(); 
+  bool is_connected();
+
   bool have_drb();
 
   // PDCP interface
   void write_pdu(uint32_t lcid, byte_buffer_t *pdu);
+
   void write_pdu_bcch_bch(byte_buffer_t *pdu);
+
   void write_pdu_bcch_dlsch(byte_buffer_t *pdu);
+
   void write_pdu_pcch(byte_buffer_t *pdu);
 
   // Radio bearers
@@ -175,31 +224,31 @@ private:
   }
 
   // RLC interface
-  void max_retx_attempted();
+  void          max_retx_attempted();
 
   // Senders
-  void send_con_request();
-  void send_con_restablish_request(); 
-  void send_con_restablish_complete();
-  void send_con_setup_complete(byte_buffer_t *nas_msg);
-  void send_ul_info_transfer(uint32_t lcid, byte_buffer_t *sdu);
-  void send_security_mode_complete(uint32_t lcid, byte_buffer_t *pdu);
-  void send_rrc_con_reconfig_complete(uint32_t lcid, byte_buffer_t *pdu);
-  void send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu);
+  void          send_con_request();
+  void          send_con_restablish_request();
+  void          send_con_restablish_complete();
+  void          send_con_setup_complete(byte_buffer_t *nas_msg);
+  void          send_ul_info_transfer(uint32_t lcid, byte_buffer_t *sdu);
+  void          send_security_mode_complete(uint32_t lcid, byte_buffer_t *pdu);
+  void          send_rrc_con_reconfig_complete(uint32_t lcid, byte_buffer_t *pdu);
+  void          send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu);
 
   // Parsers
-  void parse_dl_ccch(byte_buffer_t *pdu);
-  void parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu);
-  void parse_dl_info_transfer(uint32_t lcid, byte_buffer_t *pdu);
+  void          parse_dl_ccch(byte_buffer_t *pdu);
+  void          parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu);
+  void          parse_dl_info_transfer(uint32_t lcid, byte_buffer_t *pdu);
 
   // Helpers
-  void          reset_ue(); 
+  void          reset_ue();
   void          rrc_connection_release();
   void          radio_link_failure(); 
   static void*  start_sib_thread(void *rrc_);
   void          sib_search();
   uint32_t      sib_start_tti(uint32_t tti, uint32_t period, uint32_t x);
-  void          apply_sib2_configs();
+  void          apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2);
   void          handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup);
   void          handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup);
   void          handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig, byte_buffer_t *pdu);
diff --git a/srsue/hdr/upper/rrc_common.h b/srsue/hdr/upper/rrc_common.h
index 692a20cb3..40edfb467 100644
--- a/srsue/hdr/upper/rrc_common.h
+++ b/srsue/hdr/upper/rrc_common.h
@@ -29,21 +29,22 @@
 
 namespace srsue {
 
+
 // RRC states (3GPP 36.331 v10.0.0)
-typedef enum{
-    RRC_STATE_IDLE = 0,
-    RRC_STATE_SIB1_SEARCH,
-    RRC_STATE_SIB2_SEARCH,
-    RRC_STATE_WAIT_FOR_CON_SETUP,
-    RRC_STATE_COMPLETING_SETUP,
-    RRC_STATE_RRC_CONNECTED,
-    RRC_STATE_N_ITEMS,
-}rrc_state_t;
+typedef enum {
+  RRC_STATE_IDLE = 0,
+  RRC_STATE_PLMN_SELECTION,
+  RRC_STATE_CELL_SELECTING,
+  RRC_STATE_CELL_SELECTED,
+  RRC_STATE_CONNECTING,
+  RRC_STATE_CONNECTED,
+  RRC_STATE_N_ITEMS,
+} rrc_state_t;
 static const char rrc_state_text[RRC_STATE_N_ITEMS][100] = {"IDLE",
-                                                            "SIB1_SEARCH",
-                                                            "SIB2_SEARCH",
-                                                            "WAIT FOR CON SETUP",
-                                                            "COMPLETING SETUP",
+                                                            "PLMN SELECTION",
+                                                            "CELL SELECTION",
+                                                            "CONNECTING",
+                                                            "CONNECTED",
                                                             "RRC CONNECTED"};
 
 } // namespace srsue
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index 6028ce7e8..f28e43553 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -49,7 +49,6 @@ mac::mac() : ttisync(10240),
 {
   started = false;  
   pcap    = NULL;   
-  signals_pregenerated = false;  
   bzero(&metrics, sizeof(mac_metrics_t));
 }
   
@@ -87,6 +86,8 @@ bool mac::init(phy_interface_mac *phy, rlc_interface_mac *rlc, rrc_interface_mac
 
 void mac::stop()
 {
+  srslte_softbuffer_rx_free(&pch_softbuffer);
+
   started = false;   
   ttisync.increase();
   upper_timers_thread.thread_cancel();
@@ -132,32 +133,30 @@ void mac::reset()
   phy_h->pdcch_dl_search_reset();
   phy_h->pdcch_ul_search_reset();
   
-  signals_pregenerated = false; 
-  is_first_ul_grant = true;   
+  is_first_ul_grant = true;
   
   bzero(&uernti, sizeof(ue_rnti_t));
 }
 
 void mac::run_thread() {
   int cnt=0;
-  
-  Info("Waiting PHY to synchronize with cell\n");  
-  phy_h->sync_start();
-  while(!phy_h->get_current_tti() && started) {
-    usleep(50000);
-  }
-  Debug("Setting ttysync to %d\n", phy_h->get_current_tti());
-  ttisync.set_producer_cntr(phy_h->get_current_tti());
-     
+
   while(started) {
 
-    /* Warning: Here order of invocation of procedures is important!! */
-    ttisync.wait();
-    tti = phy_h->get_current_tti();
-    
-    if (started) {
-      log_h->step(tti);
+    while (!phy_h->sync_status() && started) {
+      usleep(5000);
+      if (phy_h->sync_status()) {
+        Debug("Setting ttysync to %d\n", phy_h->get_current_tti());
+        ttisync.set_producer_cntr(phy_h->get_current_tti());
+      }
+    }
 
+    if (started && phy_h->sync_status()) {
+      /* Warning: Here order of invocation of procedures is important!! */
+      ttisync.wait();
+      tti = phy_h->get_current_tti();
+
+      log_h->step(tti);
       timers_db.step_all();
       
       // Step all procedures 
@@ -181,18 +180,6 @@ void mac::run_thread() {
         ra_procedure.start_mac_order();
       }
       ra_procedure.step(tti);
-      
-      if (ra_procedure.is_successful() && !signals_pregenerated) {
-
-        // Configure PHY to look for UL C-RNTI grants
-        phy_h->pdcch_ul_search(SRSLTE_RNTI_USER, uernti.crnti);
-        phy_h->pdcch_dl_search(SRSLTE_RNTI_USER, uernti.crnti);
-        
-        // Pregenerate UL signals and C-RNTI scrambling sequences
-        Debug("Pre-computing C-RNTI scrambling sequences for C-RNTI=0x%x\n", uernti.crnti);
-        phy_h->set_crnti(uernti.crnti);
-        signals_pregenerated = true; 
-      }      
     }
   }  
 }
@@ -490,7 +477,7 @@ void mac::upper_timers::reset()
 /********************************************************
  *
  * Class that runs a thread to process DL MAC PDUs from
- * DEMU unit
+ * DEMUX unit
  *
  *******************************************************/
 mac::pdu_process::pdu_process(demux *demux_unit_)
diff --git a/srsue/src/mac/proc_ra.cc b/srsue/src/mac/proc_ra.cc
index 98fed02fc..28a2b47e3 100644
--- a/srsue/src/mac/proc_ra.cc
+++ b/srsue/src/mac/proc_ra.cc
@@ -73,6 +73,10 @@ void ra_proc::init(phy_interface_mac* phy_h_,
   reset();
 }
 
+ra_proc::~ra_proc() {
+  srslte_softbuffer_rx_free(&softbuffer_rar);
+}
+
 void ra_proc::reset() {
   state = IDLE;
   msg3_transmitted = false;
@@ -479,6 +483,12 @@ void ra_proc::step_completition() {
     mux_unit->msg3_flush();
     msg3_flushed = true; 
   }
+  // Configure PHY to look for UL C-RNTI grants
+  phy_h->pdcch_ul_search(SRSLTE_RNTI_USER, rntis->crnti);
+  phy_h->pdcch_dl_search(SRSLTE_RNTI_USER, rntis->crnti);
+
+  phy_h->set_crnti(rntis->crnti);
+
   msg3_transmitted = false;  
   state = COMPLETION_DONE;
 }
diff --git a/srsue/src/main.cc b/srsue/src/main.cc
index 16e0cb59f..f095a60c4 100644
--- a/srsue/src/main.cc
+++ b/srsue/src/main.cc
@@ -61,8 +61,8 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
   // Command line or config file options
   bpo::options_description common("Configuration options");
   common.add_options()
-    ("rf.dl_freq", bpo::value<float>(&args->rf.dl_freq)->default_value(2680000000), "Downlink centre frequency")
-    ("rf.ul_freq", bpo::value<float>(&args->rf.ul_freq)->default_value(2560000000), "Uplink centre frequency")
+    ("rf.dl_earfcn", bpo::value<uint32_t>(&args->rf.dl_earfcn)->default_value(3400), "Downlink EARFCN")
+    ("rf.freq_offset", bpo::value<float>(&args->rf.freq_offset)->default_value(0), "(optional) Frequency offset")
     ("rf.rx_gain", bpo::value<float>(&args->rf.rx_gain)->default_value(-1), "Front-end receiver gain")
     ("rf.tx_gain", bpo::value<float>(&args->rf.tx_gain)->default_value(-1), "Front-end transmitter gain")
     ("rf.nof_rx_ant", bpo::value<uint32_t>(&args->rf.nof_rx_ant)->default_value(1), "Number of RX antennas")
@@ -348,7 +348,6 @@ int main(int argc, char *argv[])
   all_args_t args;
   parse_args(&args, argc, argv);
 
-
   srsue_instance_type_t type = LTE;
   ue_base *ue = ue_base::get_instance(type);
   if (!ue) {
diff --git a/srsue/src/phy/phch_common.cc b/srsue/src/phy/phch_common.cc
index 3c3987863..c65ec7c77 100644
--- a/srsue/src/phy/phch_common.cc
+++ b/srsue/src/phy/phch_common.cc
@@ -186,7 +186,9 @@ void phch_common::set_dl_rnti(srslte_rnti_type_t type, uint16_t rnti_value, int
   dl_rnti_type  = type;
   dl_rnti_start = tti_start;
   dl_rnti_end   = tti_end;
-  Debug("Set DL rnti: start=%d, end=%d, value=0x%x\n", tti_start, tti_end, rnti_value);  
+  if (log_h) {
+    Debug("Set DL rnti: start=%d, end=%d, value=0x%x\n", tti_start, tti_end, rnti_value);
+  }
 }
 
 void phch_common::reset_pending_ack(uint32_t tti) {
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index 251bfd144..5b725d49f 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -25,6 +25,7 @@
  */
 
 #include <unistd.h>
+#include <srslte/srslte.h>
 #include "srslte/srslte.h"
 #include "srslte/common/log.h"
 #include "phy/phch_worker.h"
@@ -37,70 +38,14 @@
 #define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
 
 namespace srsue {
- 
 
-phch_recv::phch_recv() { 
-  running = false; 
-}
-
-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, int sync_cpu_affinity)
-{
-  radio_h      = _radio_handler;
-  log_h        = _log_h;     
-  mac          = _mac;
-  rrc          = _rrc; 
-  workers_pool = _workers_pool;
-  worker_com   = _worker_com;
-  prach_buffer = _prach_buffer; 
-  nof_rx_antennas = nof_rx_antennas_;
-
-  tx_mutex_cnt = 0; 
-  running      = true; 
-  phy_state    = IDLE; 
-  time_adv_sec = 0; 
-  cell_is_set  = false; 
-  sync_sfn_cnt = 0; 
-  
-  for (uint32_t i=0;i<nof_rx_antennas;i++) {
-    sf_buffer_sfn[i] = (cf_t*) srslte_vec_malloc(sizeof(cf_t)*3*SRSLTE_SF_LEN_PRB(100));
-  }
-  
-  nof_tx_mutex = MUTEX_X_WORKER*workers_pool->get_nof_workers();
-  worker_com->set_nof_mutex(nof_tx_mutex);
-  if(sync_cpu_affinity < 0){
-    start(prio);
-  } else {
-    start_cpu(prio, sync_cpu_affinity);
-  }
-  
-  
-}
-
-void phch_recv::stop() {
-  running = false; 
-  wait_thread_finish();
-  for (uint32_t i=0;i<nof_rx_antennas;i++) {
-    if (sf_buffer_sfn[i]) {
-      free(sf_buffer_sfn[i]);
-    }
-  }
-}
-
-void phch_recv::set_agc_enable(bool enable)
-{
-  do_agc = enable;
-}
-
-int radio_recv_wrapper_cs(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *rx_time)
-{
-  srslte::radio_multi *radio_h = (srslte::radio_multi*) h;
+int radio_recv_wrapper_cs(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *rx_time) {
+  srslte::radio_multi *radio_h = (srslte::radio_multi *) h;
   if (radio_h->rx_now(data, nsamples, rx_time)) {
-    int offset = nsamples-radio_h->get_tti_len();
-    if (abs(offset)<10 && offset != 0) {
+    int offset = nsamples - radio_h->get_tti_len();
+    if (abs(offset) < 10 && offset != 0) {
       radio_h->tx_offset(offset);
-    } else if (nsamples<10) {
+    } else if (nsamples < 10) {
       radio_h->tx_offset(nsamples);
     }
     return nsamples;
@@ -110,393 +55,644 @@ int radio_recv_wrapper_cs(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsampl
 }
 
 double callback_set_rx_gain(void *h, double gain) {
-  srslte::radio_multi *radio_handler = (srslte::radio_multi*) h;
+  srslte::radio_multi *radio_handler = (srslte::radio_multi *) h;
   return radio_handler->set_rx_gain_th(gain);
 }
 
+
+phch_recv::phch_recv() {
+  bzero(&cell, sizeof(srslte_cell_t));
+  running = false;
+}
+
+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,
+                     int sync_cpu_affinity) {
+  radio_h = _radio_handler;
+  log_h = _log_h;
+  mac = _mac;
+  rrc = _rrc;
+  workers_pool = _workers_pool;
+  worker_com = _worker_com;
+  prach_buffer = _prach_buffer;
+  nof_rx_antennas = nof_rx_antennas_;
+
+  tx_mutex_cnt = 0;
+  running = true;
+  phy_state = IDLE;
+  time_adv_sec = 0;
+  cell_is_set = false;
+  sync_sfn_cnt = 0;
+  srate_mode = SRATE_NONE;
+  cell_search_in_progress = false;
+  current_earfcn = 0;
+
+  for (uint32_t i = 0; i < nof_rx_antennas; i++) {
+    sf_buffer[i] = (cf_t *) srslte_vec_malloc(sizeof(cf_t) * 3 * SRSLTE_SF_LEN_PRB(100));
+  }
+
+
+  if (srslte_ue_cellsearch_init_multi(&cs, 5, radio_recv_wrapper_cs, nof_rx_antennas,
+                                      radio_h)) {
+    Error("SYNC:  Initiating UE cell search\n");
+    return;
+  }
+
+  srslte_ue_cellsearch_set_nof_valid_frames(&cs, 2);
+
+  // Set options defined in expert section
+  set_ue_sync_opts(&cs.ue_sync);
+
+  if (do_agc) {
+    srslte_ue_sync_start_agc(&cs.ue_sync, callback_set_rx_gain, last_gain);
+  }
+
+  if (srslte_ue_dl_init(&ue_dl_measure, SRSLTE_MAX_PRB, nof_rx_antennas)) {
+    Error("SYNC:  Initiating ue_dl_measure\n");
+    return;
+  }
+
+  if (srslte_ue_mib_init(&ue_mib, SRSLTE_MAX_PRB)) {
+    Error("SYNC:  Initiating UE MIB decoder\n");
+    return;
+  }
+
+  if (srslte_ue_sync_init_multi(&ue_sync, SRSLTE_MAX_PRB, false, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
+    Error("SYNC:  Initiating ue_sync\n");
+    return;
+  }
+
+  if (srslte_ue_mib_sync_init_multi(&ue_mib_sync, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
+    Error("SYNC:  Initiating UE MIB synchronization\n");
+    return;
+  }
+
+  nof_tx_mutex = MUTEX_X_WORKER * workers_pool->get_nof_workers();
+  worker_com->set_nof_mutex(nof_tx_mutex);
+  if (sync_cpu_affinity < 0) {
+    start(prio);
+  } else {
+    start_cpu(prio, sync_cpu_affinity);
+  }
+}
+
+phch_recv::~phch_recv() {
+  for (uint32_t i = 0; i < nof_rx_antennas; i++) {
+    if (sf_buffer[i]) {
+      free(sf_buffer[i]);
+    }
+  }
+  srslte_ue_sync_free(&ue_sync);
+  srslte_ue_dl_free(&ue_dl_measure);
+  srslte_ue_mib_free(&ue_mib);
+  srslte_ue_mib_sync_free(&ue_mib_sync);
+  srslte_ue_cellsearch_free(&cs);
+}
+
+void phch_recv::stop() {
+
+  running = false;
+  wait_thread_finish();
+}
+
+void phch_recv::set_agc_enable(bool enable) {
+  do_agc = enable;
+}
+
 void phch_recv::set_time_adv_sec(float _time_adv_sec) {
- time_adv_sec = _time_adv_sec;
+  time_adv_sec = _time_adv_sec;
 }
 
 void phch_recv::set_ue_sync_opts(srslte_ue_sync_t *q) {
   if (worker_com->args->cfo_integer_enabled) {
-    srslte_ue_sync_cfo_i_detec_en(q, true); 
+    srslte_ue_sync_cfo_i_detec_en(q, true);
   }
-  
-  float cfo_tol = worker_com->args->cfo_correct_tol_hz; 
-  srslte_cfo_set_tol(&q->strack.cfocorr, cfo_tol/(15000*q->fft_size));
-  srslte_cfo_set_tol(&q->sfind.cfocorr, cfo_tol/(15000*q->fft_size));
 
-  int time_correct_period = worker_com->args->time_correct_period; 
+  srslte_ue_sync_set_cfo_tol(q, worker_com->args->cfo_correct_tol_hz);
+
+  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);     
+    srslte_ue_sync_set_sample_offset_correct_period(q, time_correct_period);
   }
-  
-  sss_alg_t sss_alg = SSS_FULL; 
+
+  sss_alg_t sss_alg = SSS_FULL;
   if (!worker_com->args->sss_algorithm.compare("diff")) {
     sss_alg = SSS_DIFF;
   } else if (!worker_com->args->sss_algorithm.compare("partial")) {
     sss_alg = SSS_PARTIAL_3;
-  } else if (!worker_com->args->sss_algorithm.compare("full")){
-    sss_alg = SSS_FULL; 
+  } else if (!worker_com->args->sss_algorithm.compare("full")) {
+    sss_alg = SSS_FULL;
   } else {
-    Warning("Invalid SSS algorithm %s. Using 'full'\n", worker_com->args->sss_algorithm.c_str());     
+    Warning("SYNC:  Invalid SSS algorithm %s. Using 'full'\n", worker_com->args->sss_algorithm.c_str());
   }
-  srslte_sync_set_sss_algorithm(&q->strack, (sss_alg_t) sss_alg); 
-  srslte_sync_set_sss_algorithm(&q->sfind, (sss_alg_t) sss_alg);  
+  srslte_sync_set_sss_algorithm(&q->strack, (sss_alg_t) sss_alg);
+  srslte_sync_set_sss_algorithm(&q->sfind, (sss_alg_t) sss_alg);
 }
 
-bool phch_recv::init_cell() {
+bool phch_recv::set_cell() {
   cell_is_set = false;
-  if (!srslte_ue_mib_init(&ue_mib, cell)) 
-  {
-    if (!srslte_ue_sync_init_multi(&ue_sync, cell, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) 
-    {
-
-      // Set options defined in expert section 
-      set_ue_sync_opts(&ue_sync); 
-      
-      for (uint32_t i=0;i<workers_pool->get_nof_workers();i++) {
-        if (!((phch_worker*) workers_pool->get_worker(i))->init_cell(cell)) {
-          Error("Error setting cell: initiating PHCH worker\n");
-          return false; 
-        }
-      }
-      radio_h->set_tti_len(SRSLTE_SF_LEN_PRB(cell.nof_prb));
-      if (do_agc) {
-        srslte_ue_sync_start_agc(&ue_sync, callback_set_rx_gain, last_gain);    
-      }
-      srslte_ue_sync_set_cfo(&ue_sync, cellsearch_cfo);
-      cell_is_set = true;                             
-    } else {
-      Error("Error setting cell: initiating ue_sync");      
-    }
-  } else {
-    Error("Error setting cell: initiating ue_mib\n"); 
-  }      
-  return cell_is_set; 
-}
-
-void phch_recv::free_cell()
-{
-  if (cell_is_set) {
-    for (uint32_t i=0;i<workers_pool->get_nof_workers();i++) {
-      ((phch_worker*) workers_pool->get_worker(i))->free_cell();
-    }
-    prach_buffer->free_cell();
+  if (srslte_ue_mib_set_cell(&ue_mib, cell)) {
+    Error("SYNC:  Setting cell: initiating ue_mib\n");
+    return false;
   }
-}
-
-
-bool phch_recv::cell_search(int force_N_id_2) 
-{
-  uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
-  uint8_t bch_payload_bits[SRSLTE_BCH_PAYLOAD_LEN/8];
-  
-  srslte_ue_cellsearch_result_t found_cells[3];
-  srslte_ue_cellsearch_t        cs; 
-
-  bzero(found_cells, 3*sizeof(srslte_ue_cellsearch_result_t));
-
-  log_h->console("Searching for cell...\n");
-  if (srslte_ue_cellsearch_init_multi(&cs, SRSLTE_DEFAULT_MAX_FRAMES_PSS, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
-    Error("Initiating UE cell search\n");
-    return false; 
+  if (srslte_ue_sync_set_cell(&ue_sync, cell)) {
+    Error("SYNC:  Setting cell: initiating ue_sync");
+    return false;
   }
-  
-  srslte_ue_cellsearch_set_nof_valid_frames(&cs, SRSLTE_DEFAULT_NOF_VALID_PSS_FRAMES);
-  
-  // Set options defined in expert section 
-  set_ue_sync_opts(&cs.ue_sync); 
-  
+
+  // Set options defined in expert section
+  set_ue_sync_opts(&ue_sync);
+
+  if (srslte_ue_dl_set_cell(&ue_dl_measure, cell)) {
+    Error("SYNC:  Setting cell: initiating ue_dl_measure\n");
+    return false;
+  }
+
+  worker_com->set_cell(cell);
+
+  for (uint32_t i = 0; i < workers_pool->get_nof_workers(); i++) {
+    if (!((phch_worker *) workers_pool->get_worker(i))->set_cell(cell)) {
+      Error("SYNC:  Setting cell: initiating PHCH worker\n");
+      return false;
+    }
+  }
+  radio_h->set_tti_len(SRSLTE_SF_LEN_PRB(cell.nof_prb));
   if (do_agc) {
-    srslte_ue_sync_start_agc(&cs.ue_sync, callback_set_rx_gain, last_gain);
+    srslte_ue_sync_start_agc(&ue_sync, callback_set_rx_gain, last_gain);
+  }
+  cell_is_set = true;
+
+  return cell_is_set;
+}
+
+bool phch_recv::cell_search(int force_N_id_2) {
+  uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
+
+  srslte_ue_cellsearch_result_t found_cells[3];
+
+  bzero(&cell, sizeof(srslte_cell_t));
+  bzero(found_cells, 3 * sizeof(srslte_ue_cellsearch_result_t));
+
+  if (srate_mode != SRATE_FIND) {
+    srate_mode = SRATE_FIND;
+    radio_h->set_rx_srate(1.92e6);
   }
-  
-  radio_h->set_rx_srate(1.92e6);
   radio_h->start_rx();
-  
+
   /* Find a cell in the given N_id_2 or go through the 3 of them to find the strongest */
   uint32_t max_peak_cell = 0;
-  int ret = SRSLTE_ERROR; 
-  
+  int ret = SRSLTE_ERROR;
+
+  Info("SYNC:  Searching for cell...\n");
+  printf("."); fflush(stdout);
+
   if (force_N_id_2 >= 0 && force_N_id_2 < 3) {
     ret = srslte_ue_cellsearch_scan_N_id_2(&cs, force_N_id_2, &found_cells[force_N_id_2]);
     max_peak_cell = force_N_id_2;
   } else {
-    ret = srslte_ue_cellsearch_scan(&cs, found_cells, &max_peak_cell); 
+    ret = srslte_ue_cellsearch_scan(&cs, found_cells, &max_peak_cell);
   }
 
   last_gain = srslte_agc_get_gain(&cs.ue_sync.agc);
 
-  radio_h->stop_rx();
-  srslte_ue_cellsearch_free(&cs);
-  
   if (ret < 0) {
-    Error("Error decoding MIB: Error searching PSS\n");
+    radio_h->stop_rx();
+    Error("SYNC:  Error decoding MIB: Error searching PSS\n");
     return false;
   } else if (ret == 0) {
-    Error("Error decoding MIB: Could not find any PSS in this frequency\n");
+    radio_h->stop_rx();
+    Info("SYNC:  Could not find any cell in this frequency\n");
     return false;
   }
-    
-  // Save result 
-  cell.id   = found_cells[max_peak_cell].cell_id;
-  cell.cp   = found_cells[max_peak_cell].cp; 
+  // Save result
+  cell.id = found_cells[max_peak_cell].cell_id;
+  cell.cp = found_cells[max_peak_cell].cp;
   cellsearch_cfo = found_cells[max_peak_cell].cfo;
-  
-  log_h->console("Found CELL ID: %d CP: %s, CFO: %.1f KHz.\nTrying to decode MIB...\n", 
-                 cell.id, srslte_cp_string(cell.cp), cellsearch_cfo/1000);
-  
-  srslte_ue_mib_sync_t ue_mib_sync; 
 
-  if (srslte_ue_mib_sync_init_multi(&ue_mib_sync, cell.id, cell.cp, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
-    Error("Initiating UE MIB synchronization\n");
-    return false; 
+  printf("\n");
+  Info("SYNC:  PSS/SSS detected: PCI=%d, CFO=%.1f KHz, CP=%s\n",
+       cell.id, cellsearch_cfo/1000, srslte_cp_string(cell.cp));
+
+  if (srslte_ue_mib_sync_set_cell(&ue_mib_sync, cell.id, cell.cp)) {
+    Error("SYNC:  Setting UE MIB cell\n");
+    return false;
   }
-  
-  // Set options defined in expert section 
-  set_ue_sync_opts(&ue_mib_sync.ue_sync); 
+
+  // Set options defined in expert section
+  set_ue_sync_opts(&ue_mib_sync.ue_sync);
 
   if (do_agc) {
-    srslte_ue_sync_start_agc(&ue_mib_sync.ue_sync, callback_set_rx_gain, last_gain);    
+    srslte_ue_sync_start_agc(&ue_mib_sync.ue_sync, callback_set_rx_gain, last_gain);
   }
 
+  srslte_ue_sync_reset(&ue_mib_sync.ue_sync);
   srslte_ue_sync_set_cfo(&ue_mib_sync.ue_sync, cellsearch_cfo);
 
   /* Find and decode MIB */
-  uint32_t sfn; 
-  int sfn_offset; 
-  radio_h->start_rx();
-  ret = srslte_ue_mib_sync_decode(&ue_mib_sync, 
-                                  SRSLTE_DEFAULT_MAX_FRAMES_PBCH, 
-                                  bch_payload, &cell.nof_ports, &sfn_offset); 
+  int sfn_offset;
+  ret = srslte_ue_mib_sync_decode(&ue_mib_sync,
+                                  40,
+                                  bch_payload, &cell.nof_ports, &sfn_offset);
   radio_h->stop_rx();
   last_gain = srslte_agc_get_gain(&ue_mib_sync.ue_sync.agc);
   cellsearch_cfo = srslte_ue_sync_get_cfo(&ue_mib_sync.ue_sync);
-  srslte_ue_mib_sync_free(&ue_mib_sync);
+
+  srslte_ue_sync_reset(&ue_sync);
+  srslte_ue_sync_set_cfo(&ue_sync, cellsearch_cfo);
 
   if (ret == 1) {
     srslte_pbch_mib_unpack(bch_payload, &cell, NULL);
-    worker_com->set_cell(cell);
-    srslte_cell_fprint(stdout, &cell, 0);
 
-    srslte_bit_pack_vector(bch_payload, bch_payload_bits, SRSLTE_BCH_PAYLOAD_LEN);
-    mac->bch_decoded_ok(bch_payload_bits, SRSLTE_BCH_PAYLOAD_LEN/8);
-    return true;     
+    fprintf(stdout, "Found Cell:  PCI=%d, PRB=%d, Ports=%d, CFO=%.1f KHz\n",
+         cell.id, cell.nof_prb, cell.nof_ports, cellsearch_cfo/1000);
+
+    Info("SYNC:  MIB Decoded: PCI=%d, PRB=%d, Ports=%d, CFO=%.1f KHz\n",
+         cell.id, cell.nof_prb, cell.nof_ports, cellsearch_cfo/1000);
+
+    return true;
   } else {
-    Warning("Error decoding MIB: Error decoding PBCH\n");      
+    Warning("SYNC:  Found PSS but could not decode PBCH\n");
     return false;
   }
 }
 
 
-int phch_recv::sync_sfn(void) {
-  
-  int ret = SRSLTE_ERROR; 
+int phch_recv::cell_sync_sfn(void) {
+
+  int ret = SRSLTE_ERROR;
   uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
 
   srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
-  ret = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer_sfn);
+  ret = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer);
   if (ret < 0) {
-    Error("Error calling ue_sync_get_buffer");      
+    Error("SYNC:  Error calling ue_sync_get_buffer");
     return -1;
   }
-    
+
   if (ret == 1) {
     if (srslte_ue_sync_get_sfidx(&ue_sync) == 0) {
-      int sfn_offset=0;
-      Info("SYNC:  Decoding MIB...\n");
-      int n = srslte_ue_mib_decode(&ue_mib, sf_buffer_sfn[0], bch_payload, NULL, &sfn_offset);
+      int sfn_offset = 0;
+      Info("SYNC:  Trying to decode MIB... SNR=%.1f dB\n",
+           10*log10(srslte_chest_dl_get_snr(&ue_mib.chest)));
+      int n = srslte_ue_mib_decode(&ue_mib, sf_buffer[0], bch_payload, NULL, &sfn_offset);
       if (n < 0) {
-        Error("Error decoding MIB while synchronising SFN");      
-        return -1; 
-      } else if (n == SRSLTE_UE_MIB_FOUND) {  
-        uint32_t sfn; 
+        Error("SYNC:  Error decoding MIB while synchronising SFN");
+        return -1;
+      } else if (n == SRSLTE_UE_MIB_FOUND) {
+        uint32_t sfn;
         srslte_pbch_mib_unpack(bch_payload, &cell, &sfn);
 
-        sfn = (sfn + sfn_offset)%1024;         
-        tti = sfn*10;
-        
+        sfn = (sfn+sfn_offset)%1024;
+        tti = sfn * 10;
+
         srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
         Info("SYNC:  DONE, TTI=%d, sfn_offset=%d\n", tti, sfn_offset);
         srslte_ue_mib_reset(&ue_mib);
         return 1;
       }
-    }    
+    }
   } else {
     Debug("SYNC:  PSS/SSS not found...\n");
   }
   return 0;
 }
 
-void phch_recv::resync_sfn() {
-  sync_sfn_cnt = 0; 
-  phy_state = SYNCING;
+int phch_recv::cell_meas_rsrp() {
+
+  uint32_t cfi = 0;
+
+  tti = (tti+1) % 10240;
+  log_h->step(tti);
+
+  uint32_t sf_idx = tti%10;
+
+  int sync_res = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer);
+  if (sync_res == 1) {
+    if (srslte_ue_dl_decode_fft_estimate(&ue_dl_measure, sf_buffer, sf_idx, &cfi)) {
+      log_h->error("SYNC:  Measuring RSRP: Estimating channel\n");
+      return -1;
+    }
+    float rsrp   = srslte_chest_dl_get_rsrp(&ue_dl_measure.chest);
+    float snr    = srslte_chest_dl_get_snr(&ue_dl_measure.chest);
+    measure_rsrp = SRSLTE_VEC_CMA(rsrp, measure_rsrp, measure_cnt);
+    measure_cnt++;
+    log_h->info("SYNC:  Measuring RSRP %d/%d, sf_idx=%d, RSRP=%.1f dBm, SNR=%.1f dB\n",
+                measure_cnt, RSRP_MEASURE_NOF_FRAMES, sf_idx, 10 * log10(rsrp / 1000), 10*log10(snr));
+    if (measure_cnt >= RSRP_MEASURE_NOF_FRAMES) {
+      return 1;
+    }
+  } else {
+    log_h->error("SYNC:  Measuring RSRP: Sync error\n");
+    return -1;
+  }
+
+  return 0;
 }
 
-void phch_recv::run_thread()
+void phch_recv::resync_sfn() {
+  radio_h->stop_rx();
+  radio_h->start_rx();
+  srslte_ue_mib_reset(&ue_mib);
+  Info("SYNC:  Starting SFN synchronization\n");
+  sync_sfn_cnt = 0;
+  phy_state = CELL_SELECT;
+}
+
+void phch_recv::set_earfcn(std::vector<uint32_t> earfcn) {
+  this->earfcn = earfcn;
+}
+
+bool phch_recv::stop_sync() {
+  Info("SYNC:  Going to IDLE\n");
+  phy_state = IDLE;
+  int cnt=0;
+  while(!is_in_idle && cnt<100) {
+    usleep(10000);
+    cnt++;
+  }
+  return is_in_idle;
+}
+
+void phch_recv::cell_search_inc()
 {
-  int sync_res; 
+  cur_earfcn_index++;
+  Info("SYNC:  Cell Search idx %d/%d\n", cur_earfcn_index, earfcn.size());
+  if (cur_earfcn_index >= 0) {
+    if (cur_earfcn_index >= (int) earfcn.size() - 1) {
+      cur_earfcn_index = 0;
+    }
+  }
+  if (current_earfcn != earfcn[cur_earfcn_index]) {
+    current_earfcn = earfcn[cur_earfcn_index];
+    set_frequency();
+  }
+}
+
+void phch_recv::cell_search_next() {
+  if (cell_search_in_progress) {
+    cell_search_in_progress = false;
+    if (!stop_sync()) {
+      log_h->warning("SYNC:  Couldn't stop PHY\n");
+    }
+    cell_search_inc();
+    phy_state = CELL_SEARCH;
+    cell_search_in_progress = true;
+  }
+}
+
+void phch_recv::cell_search_start() {
+  if (earfcn.size() > 0) {
+    cell_search_in_progress = true;
+    cur_earfcn_index = -1;
+    cell_search_next();
+    log_h->info("SYNC:  Starting Cell Search procedure in %d EARFCNs...\n", earfcn.size());
+  } else {
+    log_h->info("SYNC:  Empty EARFCN list. Stopping cell search...\n");
+    log_h->console("Empty EARFCN list. Stopping cell search...\n");
+  }
+}
+
+bool phch_recv::cell_select(uint32_t earfcn, srslte_cell_t cell) {
+
+  // Check if we are already camping in this cell
+  if (earfcn == current_earfcn && this->cell.id == cell.id) {
+    log_h->info("Cell Select: Already in cell EARFCN=%d\n", earfcn);
+    cell_search_in_progress = false;
+    if (srate_mode != SRATE_CAMP) {
+      set_sampling_rate();
+    }
+    if (phy_state < CELL_SELECT) {
+      resync_sfn();
+    }
+    return true;
+  } else {
+
+    cell_search_in_progress = false;
+
+    if (!stop_sync()) {
+      log_h->warning("Still not in idle\n");
+    }
+
+    current_earfcn = earfcn;
+
+    if (set_frequency()) {
+      this->cell = cell;
+      log_h->info("Cell Select: Configuring cell...\n");
+
+      if (set_cell()) {
+        log_h->info("Cell Select: Synchronizing on cell...\n");
+
+        resync_sfn();
+
+        usleep(500000); // Time offset we set start_rx to start receveing samples
+        return true;
+      } else {
+        log_h->error("Cell Select: Configuring cell in EARFCN=%d, PCI=%d\n", earfcn, cell.id);
+      }
+    }
+    return false;
+  }
+}
+
+bool phch_recv::set_frequency()
+{
+  double dl_freq = 1e6*srslte_band_fd(current_earfcn);
+  double ul_freq = 1e6*srslte_band_fu(srslte_band_ul_earfcn(current_earfcn));
+  if (dl_freq > 0 && ul_freq > 0) {
+    log_h->info("SYNC:  Set DL EARFCN=%d, f_dl=%.1f MHz, f_ul=%.1f MHz\n",
+                current_earfcn, dl_freq / 1e6, ul_freq / 1e6);
+
+    log_h->console("Searching cell in DL EARFCN=%d, f_dl=%.1f MHz, f_ul=%.1f MHz\n",
+                current_earfcn, dl_freq / 1e6, ul_freq / 1e6);
+
+    radio_h->set_rx_freq(dl_freq);
+    radio_h->set_tx_freq(ul_freq);
+    ul_dl_factor = radio_h->get_tx_freq()/radio_h->get_rx_freq();
+
+    srslte_ue_sync_reset(&ue_sync);
+
+    return true;
+  } else {
+    log_h->error("SYNC:  Cell Search: Invalid EARFCN=%d\n", current_earfcn);
+    return false;
+  }
+}
+
+void phch_recv::set_sampling_rate()
+{
+  float srate = (float) srslte_sampling_freq_hz(cell.nof_prb);
+
+  Info("SYNC:  Setting sampling rate %.2f MHz\n", srate/1000000);
+
+  if (30720 % ((int) srate / 1000) == 0) {
+    radio_h->set_master_clock_rate(30.72e6);
+  } else {
+    radio_h->set_master_clock_rate(23.04e6);
+  }
+  srate_mode = SRATE_CAMP;
+  radio_h->set_rx_srate(srate);
+  radio_h->set_tx_srate(srate);
+}
+
+void phch_recv::run_thread() {
+  int sync_res;
   phch_worker *worker = NULL;
   cf_t *buffer[SRSLTE_MAX_PORTS];
-  while(running) {
-    switch(phy_state) {
+  phy_state  = IDLE;
+  is_in_idle = true;
+
+  while (running) {
+    if (phy_state != IDLE) {
+      is_in_idle = false;
+      Debug("SYNC:  state=%d\n", phy_state);
+    }
+    switch (phy_state) {
       case CELL_SEARCH:
-        if (cell_search()) {
-          log_h->console("Initializating cell configuration...\n");
-          init_cell();
-          float srate = (float) srslte_sampling_freq_hz(cell.nof_prb); 
-          
-          if (30720%((int) srate/1000) == 0) {
-            radio_h->set_master_clock_rate(30.72e6);        
-          } else {
-            radio_h->set_master_clock_rate(23.04e6);        
+        if (cell_search() && cell_search_in_progress) {
+          if (!srslte_cell_isvalid(&cell)) {
+            Error("SYNC:  Detected invalid cell\n");
+            phy_state = IDLE;
+            break;
+          }
+          if (set_cell()) {
+            set_sampling_rate();
+            resync_sfn();
           }
-          
-          log_h->console("Setting Sampling frequency %.2f MHz\n", (float) srate/1000000);          
-          radio_h->set_rx_srate(srate);
-          radio_h->set_tx_srate(srate);
 
-          ul_dl_factor = radio_h->get_tx_freq()/radio_h->get_rx_freq();
-
-          Info("SYNC:  Cell found. Synchronizing...\n");
-          phy_state = SYNCING;
-          sync_sfn_cnt = 0; 
-          srslte_ue_mib_reset(&ue_mib);
+        } else {
+          if (cell_search_in_progress) {
+            cell_search_inc();
+          }
         }
         break;
-      case SYNCING:
-        
+      case CELL_SELECT:
+
         srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
-        
-        if (!radio_is_streaming) {
-          // Start streaming
-          radio_h->start_rx();
-          radio_is_streaming = true; 
-        }
-          
-        switch(sync_sfn()) {
+
+        switch (cell_sync_sfn()) {
           default:
-            log_h->console("Going IDLE\n");
-            phy_state = IDLE; 
-            break; 
+            log_h->console("SYNC:  Going IDLE\n");
+            phy_state = IDLE;
+            break;
           case 1:
             srslte_ue_sync_set_agc_period(&ue_sync, 20);
-            phy_state = SYNC_DONE;  
-            break;        
+            if (!cell_search_in_progress) {
+              phy_state = CELL_CAMP;
+              log_h->info("Sync OK. Camping on cell PCI=%d...\n", cell.id);
+            } else {
+              measure_cnt  = 0;
+              measure_rsrp = 0;
+              phy_state    = CELL_MEASURE;
+            }
+            break;
           case 0:
-            break;        
-        } 
+            break;
+        }
         sync_sfn_cnt++;
         if (sync_sfn_cnt >= SYNC_SFN_TIMEOUT) {
-          sync_sfn_cnt = 0; 
-          radio_h->stop_rx();
-          radio_is_streaming = false; 
-          log_h->console("Timeout while synchronizing SFN\n");
-          log_h->warning("Timeout while synchronizing SFN\n");
+          sync_sfn_cnt = 0;
+          phy_state = CELL_SEARCH;
+          log_h->warning("SYNC:  Timeout while synchronizing SFN\n");
         }
-       break;
-      case SYNC_DONE:        
-        tti = (tti+1)%10240;        
-        worker = (phch_worker*) workers_pool->wait_worker(tti);
-        sync_res = 0; 
-        if (worker) {       
-          for (uint32_t i=0;i<nof_rx_antennas;i++) {
+        break;
+      case CELL_MEASURE:
+        switch(cell_meas_rsrp()) {
+          case 1:
+            log_h->info("SYNC:  Measured OK. Camping on cell PCI=%d...\n", cell.id);
+            phy_state = CELL_CAMP;
+            rrc->cell_found(earfcn[cur_earfcn_index], cell, 10*log10(measure_rsrp/1000));
+            break;
+          case 0:
+            break;
+          default:
+            log_h->error("SYNC:  Getting RSRP cell measurement.\n");
+            cell_search_next();
+        }
+        break;
+      case CELL_CAMP:
+        tti = (tti+1) % 10240;
+        worker = (phch_worker *) workers_pool->wait_worker(tti);
+        if (worker) {
+          for (uint32_t i = 0; i < nof_rx_antennas; i++) {
             buffer[i] = worker->get_buffer(i);
           }
 
-          sync_res = srslte_ue_sync_zerocopy_multi(&ue_sync, buffer); 
+          sync_res = srslte_ue_sync_zerocopy_multi(&ue_sync, buffer);
           if (sync_res == 1) {
-            
+
             log_h->step(tti);
 
-            Debug("Worker %d synchronized\n", worker->get_id());
-            
+            Debug("SYNC:  Worker %d synchronized\n", worker->get_id());
+
             metrics.sfo = srslte_ue_sync_get_sfo(&ue_sync);
             metrics.cfo = srslte_ue_sync_get_cfo(&ue_sync);
-            worker->set_cfo(ul_dl_factor*metrics.cfo/15000);
+            worker->set_cfo(ul_dl_factor * metrics.cfo / 15000);
             worker_com->set_sync_metrics(metrics);
-    
-            float sample_offset = (float) srslte_ue_sync_get_sfo(&ue_sync)/1000; 
+
+            float sample_offset = (float) srslte_ue_sync_get_sfo(&ue_sync) / 1000;
             worker->set_sample_offset(sample_offset);
-            
-            /* Compute TX time: Any transmission happens in TTI+4 thus advance 4 ms the reception time */
-            srslte_timestamp_t rx_time, tx_time, tx_time_prach; 
-            srslte_ue_sync_get_last_timestamp(&ue_sync, &rx_time); 
+
+            /* Compute TX time: Any transmission happens in TTI4 thus advance 4 ms the reception time */
+            srslte_timestamp_t rx_time, tx_time, tx_time_prach;
+            srslte_ue_sync_get_last_timestamp(&ue_sync, &rx_time);
             srslte_timestamp_copy(&tx_time, &rx_time);
             srslte_timestamp_add(&tx_time, 0, 4e-3 - time_adv_sec);
             worker->set_tx_time(tx_time);
-            
-            Debug("Settting TTI=%d, tx_mutex=%d to worker %d\n", tti, tx_mutex_cnt, worker->get_id());
-            worker->set_tti(tti, tx_mutex_cnt);
-            tx_mutex_cnt = (tx_mutex_cnt+1)%nof_tx_mutex;
 
-            // Check if we need to TX a PRACH 
+            Debug("SYNC:  Setting TTI=%d, tx_mutex=%d to worker %d\n", tti, tx_mutex_cnt, worker->get_id());
+            worker->set_tti(tti, tx_mutex_cnt);
+            tx_mutex_cnt = (tx_mutex_cnt+1) % nof_tx_mutex;
+
+            // Check if we need to TX a PRACH
             if (prach_buffer->is_ready_to_send(tti)) {
               srslte_timestamp_copy(&tx_time_prach, &rx_time);
-              srslte_timestamp_add(&tx_time_prach, 0, prach::tx_advance_sf*1e-3);
-              prach_buffer->send(radio_h, ul_dl_factor*metrics.cfo/15000, worker_com->pathloss, tx_time_prach);
-              radio_h->tx_end();            
+              srslte_timestamp_add(&tx_time_prach, 0, prach::tx_advance_sf * 1e-3);
+              prach_buffer->send(radio_h, ul_dl_factor * metrics.cfo / 15000, worker_com->pathloss, tx_time_prach);
+              radio_h->tx_end();
               worker_com->p0_preamble = prach_buffer->get_p0_preamble();
-              worker_com->cur_radio_power = SRSLTE_MIN(SRSLTE_PC_MAX, worker_com->pathloss + worker_com->p0_preamble);
-            }            
-            workers_pool->start_worker(worker);             
+              worker_com->cur_radio_power = SRSLTE_MIN(SRSLTE_PC_MAX, worker_com->pathloss+worker_com->p0_preamble);
+            }
+            workers_pool->start_worker(worker);
             // Notify RRC in-sync every 1 frame
-            if ((tti%10) == 0) {
+            if ((tti % 10) == 0) {
               rrc->in_sync();
-              log_h->debug("Sending in-sync to RRC\n");
+              log_h->debug("SYNC:  Sending in-sync to RRC\n");
             }
           } else {
-            log_h->console("Sync error.\n");
-            log_h->error("Sync error. Sending out-of-sync to RRC\n");
+            log_h->error("SYNC:  Sync error. Sending out-of-sync to RRC\n");
             // Notify RRC of out-of-sync frame
             rrc->out_of_sync();
             worker->release();
-            worker_com->reset_ul();            
-            phy_state = SYNCING;
+            worker_com->reset_ul();
           }
         } else {
           // wait_worker() only returns NULL if it's being closed. Quit now to avoid unnecessary loops here
-          running = false; 
+          running = false;
         }
         break;
       case IDLE:
+        if (!is_in_idle) {
+          radio_h->stop_rx();
+        }
+        is_in_idle = true;
         usleep(1000);
         break;
     }
   }
 }
 
-uint32_t phch_recv::get_current_tti()
-{
-  return tti; 
+uint32_t phch_recv::get_current_tti() {
+  return tti;
 }
 
-bool phch_recv::status_is_sync()
-{
-  return phy_state == SYNC_DONE;
+bool phch_recv::status_is_sync() {
+  return phy_state == CELL_CAMP;
 }
 
-void phch_recv::get_current_cell(srslte_cell_t* cell_)
-{
+void phch_recv::get_current_cell(srslte_cell_t *cell_) {
   if (cell_) {
     memcpy(cell_, &cell, sizeof(srslte_cell_t));
   }
 }
-
-void phch_recv::sync_start()
-{
-  radio_h->set_master_clock_rate(30.72e6);        
-  phy_state = CELL_SEARCH;
-}
-
-void phch_recv::sync_stop()
-{
-  free_cell();
-  radio_h->stop_rx();
-  radio_is_streaming = false; 
-  phy_state = IDLE; 
-}
-
 }
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 4618f2d16..0290c0745 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -40,8 +40,8 @@
 #ifdef ENABLE_GUI
 #include "srsgui/srsgui.h"
 #include <semaphore.h>
-#include <srslte/srslte.h>
-#include <srslte/interfaces/ue_interfaces.h>
+#include "srslte/srslte.h"
+#include "srslte/interfaces/ue_interfaces.h"
 
 void init_plots(srsue::phch_worker *worker);
 pthread_t plot_thread; 
@@ -60,15 +60,31 @@ phch_worker::phch_worker() : tr_exec(10240)
 {
   phy = NULL; 
   bzero(signal_buffer, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
-  
+
+  mem_initiated   = false;
   cell_initiated  = false; 
   pregen_enabled  = false; 
-  trace_enabled   = false; 
-  
+  trace_enabled   = false;
+
   reset();  
 }
 
-void phch_worker::reset() 
+
+phch_worker::~phch_worker()
+{
+  if (mem_initiated) {
+    for (uint32_t i=0;i<phy->args->nof_rx_ant;i++) {
+      if (signal_buffer[i]) {
+        free(signal_buffer[i]);
+      }
+    }
+    srslte_ue_dl_free(&ue_dl);
+    srslte_ue_ul_free(&ue_ul);
+    mem_initiated = false;
+  }
+}
+
+void phch_worker::reset()
 {
   bzero(&dl_metrics, sizeof(dl_metrics_t));
   bzero(&ul_metrics, sizeof(ul_metrics_t));
@@ -89,48 +105,56 @@ void phch_worker::set_common(phch_common* phy_)
 {
   phy = phy_;   
 }
-    
-bool phch_worker::init_cell(srslte_cell_t cell_)
+
+bool phch_worker::init(uint32_t max_prb)
 {
-  memcpy(&cell, &cell_, sizeof(srslte_cell_t));
-  
-  // ue_sync in phy.cc requires a buffer for 3 subframes 
+  // ue_sync in phy.cc requires a buffer for 3 subframes
   for (uint32_t i=0;i<phy->args->nof_rx_ant;i++) {
-    signal_buffer[i] = (cf_t*) srslte_vec_malloc(3 * sizeof(cf_t) * SRSLTE_SF_LEN_PRB(cell.nof_prb));
+    signal_buffer[i] = (cf_t*) srslte_vec_malloc(3 * sizeof(cf_t) * SRSLTE_SF_LEN_PRB(max_prb));
     if (!signal_buffer[i]) {
       Error("Allocating memory\n");
-      return false; 
+      return false;
     }
   }
 
-  if (srslte_ue_dl_init(&ue_dl, cell, phy->args->nof_rx_ant)) {
+  if (srslte_ue_dl_init(&ue_dl, max_prb, phy->args->nof_rx_ant)) {
     Error("Initiating UE DL\n");
-    return false; 
+    return false;
   }
-  
-  if (srslte_ue_ul_init(&ue_ul, cell)) {  
+
+  if (srslte_ue_ul_init(&ue_ul, max_prb)) {
     Error("Initiating UE UL\n");
-    return false; 
+    return false;
   }
+
   srslte_ue_ul_set_normalization(&ue_ul, true);
   srslte_ue_ul_set_cfo_enable(&ue_ul, true);
-    
-  cell_initiated = true; 
-  
-  return true; 
+
+  mem_initiated = true;
+
+  return true;
 }
 
-void phch_worker::free_cell()
+bool phch_worker::set_cell(srslte_cell_t cell_)
 {
-  if (cell_initiated) {
-    for (uint32_t i=0;i<phy->args->nof_rx_ant;i++) {
-      if (signal_buffer[i]) {
-        free(signal_buffer[i]);
-      }      
+  if (cell.id != cell_.id || !cell_initiated) {
+    memcpy(&cell, &cell_, sizeof(srslte_cell_t));
+
+    if (srslte_ue_dl_set_cell(&ue_dl, cell)) {
+      Error("Initiating UE DL\n");
+      return false;
     }
-    srslte_ue_dl_free(&ue_dl);
-    srslte_ue_ul_free(&ue_ul);
+
+    if (srslte_ue_ul_set_cell(&ue_ul, cell)) {
+      Error("Initiating UE UL\n");
+      return false;
+    }
+    srslte_ue_ul_set_normalization(&ue_ul, true);
+    srslte_ue_ul_set_cfo_enable(&ue_ul, true);
+
+    cell_initiated = true;
   }
+  return true;
 }
 
 cf_t* phch_worker::get_buffer(uint32_t antenna_idx)
@@ -216,17 +240,37 @@ void phch_worker::work_imp()
                       dl_mac_grant.pid, dl_ack);
       }
       if (dl_action.generate_ack_callback && dl_action.decode_enabled) {
-
-        // NOTE: Currently hard-coded to 1st TB only
-        for (uint32_t tb = 0; tb < 1; tb++) {
-          phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
-          dl_ack[tb] = dl_action.generate_ack_callback(dl_action.generate_ack_callback_arg);
-          Debug("Calling generate ACK callback for TB %d returned=%d\n", tb, dl_ack[tb]);
+        for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+          if (dl_mac_grant.tb_en[tb]) {
+            phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
+            dl_ack[tb] = dl_action.generate_ack_callback(dl_action.generate_ack_callback_arg);
+            Debug("Calling generate ACK callback for TB %d returned=%d\n", tb, dl_ack[tb]);
+          }
         }
       }
       Debug("dl_ack={%d, %d}, generate_ack=%d\n", dl_ack[0], dl_ack[1], dl_action.generate_ack);
       if (dl_action.generate_ack) {
-        set_uci_ack(dl_ack, dl_mac_grant.phy_grant.dl.nof_tb);
+        set_uci_ack(dl_ack, dl_mac_grant.tb_en);
+      }
+
+      /* Select Rank Indicator by computing Condition Number */
+      if (phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_3) {
+        float cn = 0.0f;
+        srslte_ue_dl_ri_select(&ue_dl, &uci_data.uci_ri, &cn);
+        uci_data.uci_ri_len = 1;
+      } else if (phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_4){
+        float sinr = 0.0f;
+        uint8 packed_pmi = 0;
+        srslte_ue_dl_ri_pmi_select(&ue_dl, &uci_data.uci_ri, &packed_pmi, &sinr);
+        srslte_bit_unpack_vector(&packed_pmi, uci_data.uci_pmi, 2);
+        uci_data.uci_ri_len = 1;
+        if (uci_data.uci_ri == 0) {
+          uci_data.uci_pmi_len = 2;
+          uci_data.uci_dif_cqi_len = 0;
+        } else {
+          uci_data.uci_pmi_len = 1;
+          uci_data.uci_dif_cqi_len = 3;
+        }
       }
     }
   }
@@ -292,8 +336,10 @@ void phch_worker::work_imp()
     if (dl_mac_grant.rnti_type == SRSLTE_RNTI_PCH) {
       phy->mac->pch_decoded_ok(dl_mac_grant.n_bytes[0]);
     } else {
-      for (uint32_t tb = 0; tb < dl_mac_grant.phy_grant.dl.nof_tb; tb++) {
-        phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
+      for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+        if (dl_mac_grant.tb_en[tb]) {
+          phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
+        }
       }
     }
   }
@@ -392,7 +438,7 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
       Error("Converting DCI message to DL grant\n");
       return false;   
     }
-    
+
     /* Fill MAC grant structure */
     grant->ndi[0] = dci_unpacked.ndi;
     grant->ndi[1] = dci_unpacked.ndi_1;
@@ -405,7 +451,15 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
     grant->rnti = dl_rnti;
     grant->rnti_type = type;
     grant->last_tti = 0;
-    
+    grant->tb_en[0] = dci_unpacked.tb_en[0];
+    grant->tb_en[1] = dci_unpacked.tb_en[1];
+    grant->tb_cw_swap = dci_unpacked.tb_cw_swap; // FIXME: tb_cw_swap not supported
+
+    if (grant->tb_cw_swap) {
+      Info("tb_cw_swap = true\n");
+      printf("tb_cw_swap = true\n");
+    }
+
     last_dl_pdcch_ncce = srslte_ue_dl_get_ncce(&ue_dl);
 
     char hexstr[16];
@@ -432,8 +486,8 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
   srslte_mimo_type_t mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
   int ret = SRSLTE_SUCCESS;
 
-  for (uint32_t tb = 0; tb < grant->nof_tb; tb++) {
-    if (rv[tb] < 0 || rv[tb] > 3) {
+  for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    if (grant->tb_en[tb] && (rv[tb] < 0 || rv[tb] > 3)) {
       valid_config = false;
       Error("Wrong RV (%d) for TB index %d", rv[tb], tb);
     }
@@ -452,22 +506,22 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
       }
       break;
     case LIBLTE_RRC_TRANSMISSION_MODE_3:
-      if (grant->nof_tb == 1) {
+      if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 1) {
         mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
-      } else if (grant->nof_tb == 2) {
+      } else if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
         mimo_type = SRSLTE_MIMO_TYPE_CDD;
       } else {
-        Error("Wrong number of transport blocks (%d) for TM3\n", grant->nof_tb);
+        Error("Wrong number of transport blocks (%d) for TM3\n", SRSLTE_RA_DL_GRANT_NOF_TB(grant));
         valid_config = false;
       }
       break;
     case LIBLTE_RRC_TRANSMISSION_MODE_4:
-      if (grant->nof_tb == 1) {
+      if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 1) {
         mimo_type = (grant->pinfo == 0) ? SRSLTE_MIMO_TYPE_TX_DIVERSITY : SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
-      } else if (grant->nof_tb == 2) {
+      } else if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
         mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
       } else {
-        Error("Wrong number of transport blocks (%d) for TM4\n", grant->nof_tb);
+        Error("Wrong number of transport blocks (%d) for TM4\n", SRSLTE_RA_DL_GRANT_NOF_TB(grant));
         valid_config = false;
       }
     break;
@@ -505,7 +559,7 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
           srslte_pdsch_set_max_noi(&ue_dl.pdsch, phy->args->pdsch_max_its);
         }
 
-        
+
   #ifdef LOG_EXECTIME
         struct timeval t[3];
         gettimeofday(&t[1], NULL);
@@ -513,7 +567,7 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
         ret = srslte_pdsch_decode(&ue_dl.pdsch, &ue_dl.pdsch_cfg, softbuffers, ue_dl.sf_symbols_m,
                                   ue_dl.ce_m, noise_estimate, rnti, payload, acks);
         if (ret) {
-          Error("Decoding PDSCH");
+          Error("ERROR: Decoding PDSCH\n");
         }
   #ifdef LOG_EXECTIME
         gettimeofday(&t[2], NULL);
@@ -521,17 +575,15 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
         snprintf(timestr, 64, ", dec_time=%4d us", (int) t[0].tv_usec);
   #endif
 
-        Info("PDSCH: l_crb=%2d, harq=%d, nof_tb=%d, tbs={%d, %d}, mcs={%d, %d}, rv={%d, %d}, crc={%s, %s}, snr=%.1f dB, n_iter=%d%s\n",
-             grant->nof_prb, harq_pid,
-             grant->nof_tb, grant->mcs[0].tbs / 8, grant->mcs[1].tbs / 8, grant->mcs[0].idx, grant->mcs[1].idx,
-             rv[0], rv[1], acks[0] ? "OK" : "KO", acks[1] ? "OK" : "KO",
-             10 * log10(srslte_chest_dl_get_snr(&ue_dl.chest)),
-             srslte_pdsch_last_noi(&ue_dl.pdsch),
-             timestr);
+        Info(
+            "PDSCH: l_crb=%2d, harq=%d, scheme=%s, tb_en={%s, %s}, tbs={%d, %d}, mcs={%d, %d}, rv={%d, %d}, crc={%s, %s}, snr=%.1f dB, n_iter=%d, %s\n",
+            grant->nof_prb, harq_pid, srslte_mimotype2str(mimo_type), grant->tb_en[0] ? "on" : "off",
+            grant->tb_en[1] ? "on" : "off", grant->mcs[0].tbs / 8, grant->mcs[1].tbs / 8, grant->mcs[0].idx,
+            grant->mcs[1].idx, rv[0], rv[1], acks[0] ? "OK" : "KO", acks[1] ? "OK" : "KO",
+            10 * log10(srslte_chest_dl_get_snr(&ue_dl.chest)),
+            srslte_pdsch_last_noi(&ue_dl.pdsch),
+            timestr);
 
-        //printf("tti=%d, cfo=%f\n", tti, cfo*15000);
-        //srslte_vec_save_file("pdsch", signal_buffer, sizeof(cf_t)*SRSLTE_SF_LEN_PRB(cell.nof_prb));
-        
         // Store metrics
         dl_metrics.mcs    = grant->mcs[0].idx;
       } else {
@@ -585,7 +637,7 @@ bool phch_worker::decode_pdcch_ul(mac_interface_phy::mac_grant_t* grant)
     {
       Error("Converting RAR message to UL grant\n");
       return false; 
-    } 
+    }
     grant->rnti_type = SRSLTE_RNTI_TEMP;
     grant->is_from_rar = true; 
     grant->has_cqi_request = false; // In contention-based Random Access CQI request bit is reserved
@@ -660,23 +712,25 @@ void phch_worker::reset_uci()
   bzero(&uci_data, sizeof(srslte_uci_data_t));
 }
 
-  void phch_worker::set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], uint32_t nof_tb) {
-    if (nof_tb > 0) {
-      uci_data.uci_ack = (uint8_t) ((ack[0]) ? 1 : 0);
-    }
-
-    if (nof_tb > 1) {
-      uci_data.uci_ack_2 = (uint8_t) ((ack[1]) ? 1 : 0);
-    }
-
-    if (nof_tb > 2) {
-      Error("Number of transport blocks is not supported");
-    }
-
-    uci_data.uci_ack_len = nof_tb;
-
+void phch_worker::set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], bool tb_en[SRSLTE_MAX_CODEWORDS])
+{
+  uint32_t nof_tb = 0;
+  if (tb_en[0]) {
+    uci_data.uci_ack = (uint8_t) ((ack[0]) ? 1 : 0);
+    nof_tb = 1;
+  } else {
+    uci_data.uci_ack = 1;
   }
 
+  if (tb_en[1]) {
+    uci_data.uci_ack_2 = (uint8_t) ((ack[1]) ? 1 : 0);
+    nof_tb = 2;
+  }
+
+  uci_data.uci_ack_len = nof_tb;
+
+}
+
 void phch_worker::set_uci_sr()
 {
   uci_data.scheduling_request = false; 
@@ -698,7 +752,16 @@ void phch_worker::set_uci_periodic_cqi()
   int cqi_max       = phy->args->cqi_max;
   
   if (period_cqi.configured && rnti_is_set) {
-    if (srslte_cqi_send(period_cqi.pmi_idx, (tti+4)%10240)) {
+    if (period_cqi.ri_idx_present && srslte_ri_send(period_cqi.pmi_idx, period_cqi.ri_idx, (tti+4)%10240)) {
+      if (uci_data.uci_ri_len) {
+        uci_data.uci_cqi[0] = uci_data.uci_ri;
+        uci_data.uci_cqi_len = uci_data.uci_ri_len;
+        uci_data.uci_ri_len = 0;
+        uci_data.uci_dif_cqi_len = 0;
+        uci_data.uci_pmi_len = 0;
+        Info("PUCCH: Periodic RI=%d\n", uci_data.uci_cqi[0]);
+      }
+    } else if (srslte_cqi_send(period_cqi.pmi_idx, (tti+4)%10240)) {
       srslte_cqi_value_t cqi_report;
       if (period_cqi.format_is_subband) {
         // TODO: Implement subband periodic reports
@@ -813,11 +876,12 @@ void phch_worker::encode_pusch(srslte_ra_ul_grant_t *grant, uint8_t *payload, ui
   snprintf(timestr, 64, ", total_time=%4d us", (int) logtime_start[0].tv_usec);
 #endif
 
-  Info("PUSCH: tti_tx=%d, n_prb=%d, rb_start=%d, tbs=%d, mod=%d, mcs=%d, rv_idx=%d, ack=%s, cfo=%.1f Hz%s\n", 
+  Info("PUSCH: tti_tx=%d, n_prb=%d, rb_start=%d, tbs=%d, mod=%d, mcs=%d, rv_idx=%d, ack=%s, ri=%s, cfo=%.1f Hz%s\n",
          (tti+4)%10240,
          grant->L_prb, grant->n_prb[0], 
          grant->mcs.tbs/8, grant->mcs.mod, grant->mcs.idx, rv,
          uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",
+         uci_data.uci_ri_len>0?(uci_data.uci_ri?"1":"0"):"no",
          cfo*15000, timestr);
 
   // Store metrics
@@ -859,10 +923,13 @@ void phch_worker::encode_pucch()
   float tx_power = srslte_ue_ul_pucch_power(&ue_ul, phy->pathloss, ue_ul.last_pucch_format, uci_data.uci_cqi_len, uci_data.uci_ack_len);
   float gain = set_power(tx_power);  
   
-  Info("PUCCH: power=%.2f dBm, tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s, sr=%s, cfo=%.1f Hz%s\n", 
+  Info("PUCCH: power=%.2f dBm, tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s%s, ri=%s, sr=%s, cfo=%.1f Hz%s\n",
          tx_power, (tti+4)%10240, 
          last_dl_pdcch_ncce, ue_ul.pucch.last_n_pucch, ue_ul.pucch.last_n_prb, 
-       uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",uci_data.scheduling_request?"yes":"no", 
+       uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",
+       uci_data.uci_ack_len>1?(uci_data.uci_ack_2?"1":"0"):"",
+       uci_data.uci_ri_len>0?(uci_data.uci_ri?"1":"0"):"no",
+       uci_data.scheduling_request?"yes":"no",
          cfo*15000, timestr);        
   }   
   
@@ -992,12 +1059,19 @@ void phch_worker::set_ul_params(bool pregen_disabled)
   /* CQI configuration */
   bzero(&period_cqi, sizeof(srslte_cqi_periodic_cfg_t));
   period_cqi.configured        = dedicated->cqi_report_cnfg.report_periodic_setup_present;
-  period_cqi.pmi_idx           = dedicated->cqi_report_cnfg.report_periodic.pmi_cnfg_idx; 
+  period_cqi.pmi_idx           = dedicated->cqi_report_cnfg.report_periodic.pmi_cnfg_idx;
   period_cqi.simul_cqi_ack     = dedicated->cqi_report_cnfg.report_periodic.simult_ack_nack_and_cqi;
   period_cqi.format_is_subband = dedicated->cqi_report_cnfg.report_periodic.format_ind_periodic ==
                                  LIBLTE_RRC_CQI_FORMAT_INDICATOR_PERIODIC_SUBBAND_CQI;
   period_cqi.subband_size      = dedicated->cqi_report_cnfg.report_periodic.format_ind_periodic_subband_k;
-  
+
+  if (dedicated->cqi_report_cnfg.report_periodic.ri_cnfg_idx_present) {
+    period_cqi.ri_idx = dedicated->cqi_report_cnfg.report_periodic.ri_cnfg_idx;
+    period_cqi.ri_idx_present = true;
+  } else {
+    period_cqi.ri_idx_present = false;
+  }
+
   /* SR configuration */
   I_sr                         = dedicated->sched_request_cnfg.sr_cnfg_idx;
   
@@ -1043,7 +1117,7 @@ int phch_worker::read_ce_abs(float *ce_abs) {
   bzero(ce_abs, sizeof(float)*sz);
   int g = (sz - 12*cell.nof_prb)/2;
   for (i = 0; i < 12*cell.nof_prb; i++) {
-    ce_abs[g+i] = 20 * log10(cabs(ue_dl.ce[0][i]));
+    ce_abs[g+i] = 20 * log10f(cabsf(ue_dl.ce_m[0][0][i]));
     if (isinf(ce_abs[g+i])) {
       ce_abs[g+i] = -80;
     }
diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc
index dea66346b..d3933df13 100644
--- a/srsue/src/phy/phy.cc
+++ b/srsue/src/phy/phy.cc
@@ -95,44 +95,63 @@ bool phy::check_args(phy_args_t *args)
   return true; 
 }
 
-bool phy::init(srslte::radio_multi* radio_handler_, mac_interface_phy *mac, rrc_interface_phy *rrc, 
-               srslte::log *log_h_, phy_args_t *phy_args)
-{
+bool phy::init(srslte::radio_multi* radio_handler, mac_interface_phy *mac, rrc_interface_phy *rrc,
+               srslte::log *log_h, phy_args_t *phy_args) {
 
   mlockall(MCL_CURRENT | MCL_FUTURE);
-  
-  n_ta = 0; 
-  log_h = log_h_; 
-  radio_handler = radio_handler_;
-  
+
+  n_ta = 0;
+  this->log_h         = log_h;
+  this->radio_handler = radio_handler;
+  this->mac           = mac;
+  this->rrc           = rrc;
+
   if (!phy_args) {
-    args = &default_args; 
+    args = &default_args;
     set_default_args(args);
   } else {
     args = phy_args;
   }
-  
+
   if (!check_args(args)) {
-    return false; 
+    return false;
   }
-  
-  nof_workers = args->nof_phy_threads; 
-  
+
+  nof_workers = args->nof_phy_threads;
+
+  initiated = false;
+  start();
+  return true;
+}
+
+// Initializes PHY in a thread
+void phy::run_thread() {
+
+  prach_buffer.init(&config.common.prach_cnfg, SRSLTE_MAX_PRB, args, log_h);
+  workers_common.init(&config, args, log_h, radio_handler, mac);
+
   // Add workers to workers pool and start threads
   for (uint32_t i=0;i<nof_workers;i++) {
     workers[i].set_common(&workers_common);
-    workers_pool.init_worker(i, &workers[i], WORKERS_THREAD_PRIO, args->worker_cpu_mask);    
+    workers[i].init(SRSLTE_MAX_PRB);
+    workers_pool.init_worker(i, &workers[i], WORKERS_THREAD_PRIO, args->worker_cpu_mask);
   }
-  prach_buffer.init(&config.common.prach_cnfg, args, log_h);
-  workers_common.init(&config, args, log_h, radio_handler, mac);
-  
+
   // 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);
 
   // Disable UL signal pregeneration until the attachment 
   enable_pregen_signals(false);
 
-  return true; 
+  initiated = true;
+}
+
+void phy::wait_initialize() {
+  wait_thread_finish();
+}
+
+bool phy::is_initiated() {
+  return initiated;
 }
 
 void phy::set_agc_enable(bool enabled)
@@ -190,7 +209,7 @@ void phy::configure_prach_params()
     Debug("Configuring PRACH parameters\n");
     srslte_cell_t cell; 
     sf_recv.get_current_cell(&cell);
-    if (!prach_buffer.init_cell(cell)) {
+    if (!prach_buffer.set_cell(cell)) {
       Error("Configuring PRACH parameters\n");
     } 
   } else {
@@ -201,11 +220,28 @@ void phy::configure_prach_params()
 void phy::configure_ul_params(bool pregen_disabled)
 {
   Info("PHY:   Configuring UL parameters\n");
-  for (uint32_t i=0;i<nof_workers;i++) {
-    workers[i].set_ul_params(pregen_disabled);
+  if (is_initiated()) {
+    for (uint32_t i=0;i<nof_workers;i++) {
+      workers[i].set_ul_params(pregen_disabled);
+    }
   }
 }
 
+void phy::cell_search_start()
+{
+  sf_recv.cell_search_start();
+}
+
+void phy::cell_search_next()
+{
+  sf_recv.cell_search_next();
+}
+
+bool phy::cell_select(uint32_t earfcn, srslte_cell_t phy_cell)
+{
+  return sf_recv.cell_select(earfcn, phy_cell);
+}
+
 float phy::get_phr()
 {
   float phr = radio_handler->get_max_tx_power() - workers_common.cur_pusch_power; 
@@ -281,23 +317,23 @@ int phy::sr_last_tx_tti()
   return workers_common.sr_last_tx_tti;
 }
 
-bool phy::status_is_sync()
-{
-  return sf_recv.status_is_sync();
-}
-
 void phy::resync_sfn() {
   sf_recv.resync_sfn();
 }
 
-void phy::sync_start()
+void phy::set_earfcn(vector< uint32_t > earfcns)
 {
-  sf_recv.sync_start();
+  sf_recv.set_earfcn(earfcns);
 }
 
-void phy::sync_stop()
+bool phy::sync_status()
 {
-  sf_recv.sync_stop();
+  return sf_recv.status_is_sync();
+}
+
+bool phy::sync_stop()
+{
+  return sf_recv.stop_sync();
 }
 
 void phy::set_rar_grant(uint32_t tti, uint8_t grant_payload[SRSLTE_RAR_GRANT_LEN])
diff --git a/srsue/src/phy/prach.cc b/srsue/src/phy/prach.cc
index 62980c623..7c693fe7a 100644
--- a/srsue/src/phy/prach.cc
+++ b/srsue/src/phy/prach.cc
@@ -41,14 +41,13 @@
 
 namespace srsue {
  
-  
-void prach::free_cell() 
-{
-  if (initiated) {
+
+prach::~prach() {
+  if (mem_initiated) {
     for (int i=0;i<64;i++) {
       if (buffer[i]) {
-        free(buffer[i]);    
-      }      
+        free(buffer[i]);
+      }
     }
     if (signal_buffer) {
       free(signal_buffer);
@@ -58,84 +57,103 @@ void prach::free_cell()
   }
 }
 
-void prach::init(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT *config_, phy_args_t *args_, srslte::log* log_h_)
+void prach::init(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT *config_, uint32_t max_prb, phy_args_t *args_, srslte::log* log_h_)
 {
-  log_h  = log_h_; 
-  config = config_; 
-  args   = args_; 
+  log_h  = log_h_;
+  config = config_;
+  args   = args_;
+
+  for (int i=0;i<64;i++) {
+    buffer[i] = (cf_t*) srslte_vec_malloc(SRSLTE_PRACH_MAX_LEN*sizeof(cf_t));
+    if(!buffer[i]) {
+      perror("malloc");
+      return;
+    }
+  }
+  if (srslte_cfo_init(&cfo_h, SRSLTE_PRACH_MAX_LEN)) {
+    fprintf(stderr, "PRACH: Error initiating CFO\n");
+    return;
+  }
+  srslte_cfo_set_tol(&cfo_h, 0);
+  signal_buffer = (cf_t*) srslte_vec_malloc(SRSLTE_PRACH_MAX_LEN*sizeof(cf_t));
+  if (!signal_buffer) {
+    perror("malloc");
+    return;
+  }
+  if (srslte_prach_init(&prach_obj, srslte_symbol_sz(max_prb))) {
+    Error("Initiating PRACH library\n");
+    return;
+  }
+  mem_initiated = true;
 }
 
-bool prach::init_cell(srslte_cell_t cell_)
+bool prach::set_cell(srslte_cell_t cell_)
 {
-  // TODO: Check if other PRACH parameters changed
-  if (cell_.id != cell.id || !initiated) {
-    if (initiated) {
-      free_cell();
-    }
-    cell = cell_; 
-    preamble_idx = -1; 
+  if (mem_initiated) {
+    // TODO: Check if other PRACH parameters changed
+    if (cell_.id != cell.id || !cell_initiated) {
+      memcpy(&cell, &cell_, sizeof(srslte_cell_t));
+      preamble_idx = -1;
 
-    uint32_t configIdx      = config->prach_cnfg_info.prach_config_index;
-    uint32_t rootSeq        = config->root_sequence_index;
-    uint32_t zeroCorrConfig = config->prach_cnfg_info.zero_correlation_zone_config;
-    uint32_t freq_offset    = config->prach_cnfg_info.prach_freq_offset;
-    bool     highSpeed      = config->prach_cnfg_info.high_speed_flag; 
-    
-    if (6 + freq_offset > cell.nof_prb) {
-      log_h->console("Error no space for PRACH: frequency offset=%d, N_rb_ul=%d\n", freq_offset, cell.nof_prb);
-      log_h->error("Error no space for PRACH: frequency offset=%d, N_rb_ul=%d\n", freq_offset, cell.nof_prb);
-      return false; 
-    }
-    
-    if (srslte_prach_init(&prach_obj, srslte_symbol_sz(cell.nof_prb), 
-                          configIdx, rootSeq, highSpeed, zeroCorrConfig))
-    {
-      Error("Initiating PRACH library\n");
-      return false; 
-    }
-    
-    len = prach_obj.N_seq + prach_obj.N_cp;
-    for (int i=0;i<64;i++) {
-      buffer[i] = (cf_t*) srslte_vec_malloc(len*sizeof(cf_t));
-      if(!buffer[i]) {
-        return false; 
-      }    
-      if(srslte_prach_gen(&prach_obj, i, freq_offset, buffer[i])) {
-        Error("Generating PRACH preamble %d\n", i);
+      uint32_t configIdx      = config->prach_cnfg_info.prach_config_index;
+      uint32_t rootSeq        = config->root_sequence_index;
+      uint32_t zeroCorrConfig = config->prach_cnfg_info.zero_correlation_zone_config;
+      uint32_t freq_offset    = config->prach_cnfg_info.prach_freq_offset;
+      bool     highSpeed      = config->prach_cnfg_info.high_speed_flag;
+
+      if (6 + freq_offset > cell.nof_prb) {
+        log_h->console("Error no space for PRACH: frequency offset=%d, N_rb_ul=%d\n", freq_offset, cell.nof_prb);
+        log_h->error("Error no space for PRACH: frequency offset=%d, N_rb_ul=%d\n", freq_offset, cell.nof_prb);
         return false;
       }
+
+      Info("PRACH: configIdx=%d, rootSequence=%d, zeroCorrelationConfig=%d, freqOffset=%d\n",
+            configIdx, rootSeq, zeroCorrConfig, freq_offset);
+
+      if (srslte_prach_set_cell(&prach_obj, srslte_symbol_sz(cell.nof_prb),
+                                 configIdx, rootSeq, highSpeed, zeroCorrConfig)) {
+        Error("Initiating PRACH library\n");
+        return false;
+      }
+      for (int i=0;i<64;i++) {
+        if(srslte_prach_gen(&prach_obj, i, freq_offset, buffer[i])) {
+          Error("Generating PRACH preamble %d\n", i);
+          return false;
+        }
+      }
+
+      len = prach_obj.N_seq + prach_obj.N_cp;
+      transmitted_tti = -1;
+      cell_initiated = true;
     }
-    srslte_cfo_init(&cfo_h, len);
-    srslte_cfo_set_tol(&cfo_h, 0);
-    signal_buffer = (cf_t*) srslte_vec_malloc(len*sizeof(cf_t)); 
-    initiated = signal_buffer?true:false; 
-    transmitted_tti = -1; 
-    Debug("PRACH Initiated %s\n", initiated?"OK":"KO");
+    return true;
+  } else {
+    fprintf(stderr, "PRACH: Error must call init() first\n");
+    return false;
   }
-  return initiated;  
 }
 
 bool prach::prepare_to_send(uint32_t preamble_idx_, int allowed_subframe_, float target_power_dbm_)
 {
-  if (initiated && preamble_idx_ < 64) {
+  if (cell_initiated && preamble_idx_ < 64) {
     preamble_idx = preamble_idx_;
     target_power_dbm = target_power_dbm_;
     allowed_subframe = allowed_subframe_; 
     transmitted_tti = -1; 
-    Debug("PRACH prepare to send preamble %d\n", preamble_idx);
+    Debug("PRACH: prepare to send preamble %d\n", preamble_idx);
     return true; 
   } else {
-    if (!initiated) {
-      Error("PRACH not initiated\n");
+    if (!cell_initiated) {
+      Error("PRACH: Cell not configured\n");
     } else if (preamble_idx_ >= 64) {
-      Error("Invalid preamble %d\n", preamble_idx_);
+      Error("PRACH: Invalid preamble %d\n", preamble_idx_);
     }
     return false; 
   }
 }
 
 bool prach::is_ready_to_send(uint32_t current_tti_) {
-  if (initiated && preamble_idx >= 0 && preamble_idx < 64) {
+  if (cell_initiated && preamble_idx >= 0 && preamble_idx < 64) {
     // consider the number of subframes the transmission must be anticipated 
     uint32_t current_tti = (current_tti_ + tx_advance_sf)%10240;
     if (srslte_prach_tti_opportunity(&prach_obj, current_tti, allowed_subframe)) {
@@ -195,9 +213,9 @@ void prach::send(srslte::radio *radio_handler, float cfo, float pathloss, srslte
   
   Info("PRACH: Transmitted preamble=%d, CFO=%.2f KHz, tx_time=%f\n", 
        preamble_idx, cfo*15, tx_time.frac_secs);
-  preamble_idx = -1; 
+  preamble_idx = -1;
 
-  radio_handler->set_tx_gain(old_gain);    
+  radio_handler->set_tx_gain(old_gain);
   Debug("Restoring TX gain to %.0f dB\n", old_gain);  
 }
   
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index 0110a1a14..55e0c7d92 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -51,8 +51,10 @@ ue::~ue()
 bool ue::init(all_args_t *args_)
 {
   args     = args_;
-  
-  logger.init(args->log.filename);
+
+#ifndef LOG_STDOUT
+    logger.init(args->log.filename);
+#endif
   rf_log.init("RF  ", &logger);
   phy_log.init("PHY ", &logger, true);
   mac_log.init("MAC ", &logger, true);
@@ -64,7 +66,9 @@ bool ue::init(all_args_t *args_)
   usim_log.init("USIM", &logger);
 
   // Init logs
+#ifndef LOG_STDOUT
   logger.log("\n\n");
+#endif
   rf_log.set_level(srslte::LOG_LEVEL_INFO);
   phy_log.set_level(level(args->log.phy_level));
   mac_log.set_level(level(args->log.mac_level));
@@ -97,7 +101,11 @@ bool ue::init(all_args_t *args_)
   }
   
   // Init layers
-  
+
+  // PHY initis in background, start before radio
+  args->expert.phy.nof_rx_ant = args->rf.nof_rx_ant;
+  phy.init(&radio, &mac, &rrc, &phy_log, &args->expert.phy);
+
   /* Start Radio */
   char *dev_name = NULL;
   if (args->rf.device_name.compare("auto")) {
@@ -128,15 +136,13 @@ bool ue::init(all_args_t *args_)
   radio.set_manual_calibration(&args->rf_cal);
 
   // Set PHY options
-  args->expert.phy.nof_rx_ant = args->rf.nof_rx_ant;
 
   if (args->rf.tx_gain > 0) {
     args->expert.phy.ul_pwr_ctrl_en = false; 
   } else {
     args->expert.phy.ul_pwr_ctrl_en = true; 
   }
-  phy.init(&radio, &mac, &rrc, &phy_log, &args->expert.phy);
-  
+
   if (args->rf.rx_gain < 0) {
     radio.start_agc(false);    
     radio.set_tx_rx_gain_offset(10);
@@ -154,23 +160,31 @@ bool ue::init(all_args_t *args_)
   }
 
   radio.register_error_handler(rf_msg);
-
-  radio.set_rx_freq(args->rf.dl_freq);
-  radio.set_tx_freq(args->rf.ul_freq);
-
-  phy_log.console("Setting frequency: DL=%.1f Mhz, UL=%.1f MHz\n", args->rf.dl_freq/1e6, args->rf.ul_freq/1e6);
+  radio.set_freq_offset(args->rf.freq_offset);
 
   mac.init(&phy, &rlc, &rrc, &mac_log);
   rlc.init(&pdcp, &rrc, this, &rlc_log, &mac, 0 /* RB_ID_SRB0 */);
-
   pdcp.init(&rlc, &rrc, &gw, &pdcp_log, 0 /* RB_ID_SRB0 */, SECURITY_DIRECTION_UPLINK);
 
+  nas.init(&usim, &rrc, &gw, &nas_log, 1 /* RB_ID_SRB1 */);
+  gw.init(&pdcp, this, &gw_log, 3 /* RB_ID_DRB1 */);
+
+  usim.init(&args->usim, &usim_log);
+
   rrc.init(&phy, &mac, &rlc, &pdcp, &nas, &usim, &mac, &rrc_log);
   rrc.set_ue_category(atoi(args->expert.ue_cateogry.c_str()));
-  
-  nas.init(&usim, &rrc, &gw, &nas_log, 1 /* RB_ID_SRB1 */);
-  gw.init(&pdcp, &rrc, this, &gw_log, 3 /* RB_ID_DRB1 */);
-  usim.init(&args->usim, &usim_log);
+
+  // Currently EARFCN list is set to only one frequency as indicated in ue.conf
+  std::vector<uint32_t> earfcn_list;
+  earfcn_list.push_back(args->rf.dl_earfcn);
+  phy.set_earfcn(earfcn_list);
+
+  printf("Waiting PHY to initialize...\n");
+  phy.wait_initialize();
+  phy.configure_ul_params();
+
+  printf("...\n");
+  nas.attach_request();
 
   started = true;
   return true;
@@ -236,7 +250,7 @@ bool ue::get_metrics(ue_metrics_t &m)
   rf_metrics.rf_error = false; // Reset error flag
 
   if(EMM_STATE_REGISTERED == nas.get_state()) {
-    if(RRC_STATE_RRC_CONNECTED == rrc.get_state()) {
+    if(RRC_STATE_CONNECTED == rrc.get_state()) {
       phy.get_metrics(m.phy);
       mac.get_metrics(m.mac);
       rlc.get_metrics(m.rlc);
diff --git a/srsue/src/upper/nas.cc b/srsue/src/upper/nas.cc
index 1595a2cec..f3b3708af 100644
--- a/srsue/src/upper/nas.cc
+++ b/srsue/src/upper/nas.cc
@@ -25,116 +25,153 @@
  */
 
 
+#include "srslte/asn1/liblte_rrc.h"
 #include "upper/nas.h"
+#include "srslte/common/bcd_helpers.h"
 
 using namespace srslte;
 
-namespace srsue{
+namespace srsue {
 
 nas::nas()
-  :state(EMM_STATE_DEREGISTERED)
-  ,is_guti_set(false)
-  ,ip_addr(0)
-  ,eps_bearer_id(0)
-  ,count_ul(0)
-  ,count_dl(0)
-{}
+  : state(EMM_STATE_DEREGISTERED), plmn_selection(PLMN_SELECTED), is_guti_set(false), ip_addr(0), eps_bearer_id(0),
+    count_ul(0), count_dl(0) {}
 
 void nas::init(usim_interface_nas *usim_,
-               rrc_interface_nas  *rrc_,
-               gw_interface_nas   *gw_,
-               srslte::log        *nas_log_,
+               rrc_interface_nas *rrc_,
+               gw_interface_nas *gw_,
+               srslte::log *nas_log_,
                srslte::srslte_nas_config_t cfg_)
 {
-  pool    = byte_buffer_pool::get_instance();
-  usim    = usim_;
-  rrc     = rrc_;
-  gw      = gw_;
+  pool = byte_buffer_pool::get_instance();
+  usim = usim_;
+  rrc = rrc_;
+  gw = gw_;
   nas_log = nas_log_;
+  state = EMM_STATE_DEREGISTERED;
+  plmn_selection = PLMN_NOT_SELECTED;
+  home_plmn.mcc = 61441; // This is 001
+  home_plmn.mnc = 65281; // This is 01
   cfg     = cfg_;
 }
 
-void nas::stop()
-{}
+void nas::stop() {}
 
-emm_state_t nas::get_state()
-{
+emm_state_t nas::get_state() {
   return state;
 }
 
+/*******************************************************************************
+UE interface
+*******************************************************************************/
+void nas::attach_request() {
+  nas_log->info("Attach Request\n");
+  if (state == EMM_STATE_DEREGISTERED) {
+    state = EMM_STATE_REGISTERED_INITIATED;
+    if (plmn_selection == PLMN_NOT_SELECTED) {
+      nas_log->info("Starting PLMN Search...\n");
+      rrc->plmn_search();
+    } else if (plmn_selection == PLMN_SELECTED) {
+      nas_log->info("Selecting PLMN %s\n", plmn_id_to_c_str(current_plmn).c_str());
+      rrc->plmn_select(current_plmn);
+    }
+  } else {
+    nas_log->info("Attach request ignored. State = %s\n", emm_state_text[state]);
+  }
+}
+
+void nas::deattach_request() {
+  state = EMM_STATE_DEREGISTERED_INITIATED;
+  nas_log->info("Dettach request not supported\n");
+}
 
 /*******************************************************************************
-  RRC interface
+RRC interface
 *******************************************************************************/
 
-bool nas::is_attached()
-{
+void nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) {
+
+  // Store PLMN if not registered
+  for (uint32_t i=0;i<known_plmns.size();i++) {
+    if (plmn_id.mcc == known_plmns[i].mcc && plmn_id.mnc == known_plmns[i].mnc) {
+      nas_log->info("Detected known PLMN %s\n", plmn_id_to_c_str(plmn_id).c_str());
+      if (plmn_id.mcc == home_plmn.mcc && plmn_id.mnc == home_plmn.mnc) {
+        rrc->plmn_select(plmn_id);
+      }
+      return;
+    }
+  }
+  nas_log->info("Found PLMN:  Id=%s, TAC=%d\n", plmn_id_to_c_str(plmn_id).c_str(),
+                tracking_area_code);
+  nas_log->console("Found PLMN:  Id=%s, TAC=%d\n", plmn_id_to_c_str(plmn_id).c_str(),
+                tracking_area_code);
+  if (plmn_id.mcc == home_plmn.mcc && plmn_id.mnc == home_plmn.mnc) {
+    rrc->plmn_select(plmn_id);
+  }
+}
+
+bool nas::is_attached() {
   return state == EMM_STATE_REGISTERED;
 }
 
-void nas::notify_connection_setup()
-{
+void nas::notify_connection_setup() {
   nas_log->debug("State = %s\n", emm_state_text[state]);
-  if(EMM_STATE_DEREGISTERED == state) {
+  if (EMM_STATE_REGISTERED_INITIATED == state) {
     send_attach_request();
   } else {
     send_service_request();
   }
 }
 
-void nas::write_pdu(uint32_t lcid, byte_buffer_t *pdu)
-{
+void nas::write_pdu(uint32_t lcid, byte_buffer_t *pdu) {
   uint8 pd;
   uint8 msg_type;
 
   nas_log->info_hex(pdu->msg, pdu->N_bytes, "DL %s PDU", rrc->get_rb_name(lcid).c_str());
 
   // Parse the message
-  liblte_mme_parse_msg_header((LIBLTE_BYTE_MSG_STRUCT*)pdu, &pd, &msg_type);
-  switch(msg_type)
-  {
-  case LIBLTE_MME_MSG_TYPE_ATTACH_ACCEPT:
+  liblte_mme_parse_msg_header((LIBLTE_BYTE_MSG_STRUCT *) pdu, &pd, &msg_type);
+  switch (msg_type) {
+    case LIBLTE_MME_MSG_TYPE_ATTACH_ACCEPT:
       parse_attach_accept(lcid, pdu);
       break;
-  case LIBLTE_MME_MSG_TYPE_ATTACH_REJECT:
+    case LIBLTE_MME_MSG_TYPE_ATTACH_REJECT:
       parse_attach_reject(lcid, pdu);
       break;
-  case LIBLTE_MME_MSG_TYPE_AUTHENTICATION_REQUEST:
+    case LIBLTE_MME_MSG_TYPE_AUTHENTICATION_REQUEST:
       parse_authentication_request(lcid, pdu);
       break;
-  case LIBLTE_MME_MSG_TYPE_AUTHENTICATION_REJECT:
+    case LIBLTE_MME_MSG_TYPE_AUTHENTICATION_REJECT:
       parse_authentication_reject(lcid, pdu);
       break;
-  case LIBLTE_MME_MSG_TYPE_IDENTITY_REQUEST:
+    case LIBLTE_MME_MSG_TYPE_IDENTITY_REQUEST:
       parse_identity_request(lcid, pdu);
       break;
-  case LIBLTE_MME_MSG_TYPE_SECURITY_MODE_COMMAND:
+    case LIBLTE_MME_MSG_TYPE_SECURITY_MODE_COMMAND:
       parse_security_mode_command(lcid, pdu);
       break;
-  case LIBLTE_MME_MSG_TYPE_SERVICE_REJECT:
+    case LIBLTE_MME_MSG_TYPE_SERVICE_REJECT:
       parse_service_reject(lcid, pdu);
       break;
-  case LIBLTE_MME_MSG_TYPE_ESM_INFORMATION_REQUEST:
+    case LIBLTE_MME_MSG_TYPE_ESM_INFORMATION_REQUEST:
       parse_esm_information_request(lcid, pdu);
       break;
-  case LIBLTE_MME_MSG_TYPE_EMM_INFORMATION:
+    case LIBLTE_MME_MSG_TYPE_EMM_INFORMATION:
       parse_emm_information(lcid, pdu);
       break;
-  default:
-      nas_log->error("Not handling NAS message with MSG_TYPE=%02X\n",msg_type);
+    default:
+      nas_log->error("Not handling NAS message with MSG_TYPE=%02X\n", msg_type);
       pool->deallocate(pdu);
       break;
   }
 }
 
-uint32_t  nas::get_ul_count()
-{
+uint32_t nas::get_ul_count() {
   return count_ul;
 }
 
-bool      nas::get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi)
-{
-  if(is_guti_set) {
+bool nas::get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi) {
+  if (is_guti_set) {
     s_tmsi->mmec   = guti.mme_code;
     s_tmsi->m_tmsi = guti.m_tmsi;
     return true;
@@ -144,113 +181,94 @@ bool      nas::get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi)
 }
 
 /*******************************************************************************
-  Security
+Security
 *******************************************************************************/
 
-void nas::integrity_generate(uint8_t  *key_128,
-                             uint32_t  count,
-                             uint8_t   rb_id,
-                             uint8_t   direction,
-                             uint8_t  *msg,
-                             uint32_t  msg_len,
-                             uint8_t  *mac)
-{
-  switch(integ_algo)
-  {
-  case INTEGRITY_ALGORITHM_ID_EIA0:
-    break;
-  case INTEGRITY_ALGORITHM_ID_128_EIA1:
-    security_128_eia1(key_128,
-                      count,
-                      rb_id,
-                      direction,
-                      msg,
-                      msg_len,
-                      mac);
-    break;
-  case INTEGRITY_ALGORITHM_ID_128_EIA2:
-    security_128_eia2(key_128,
-                      count,
-                      rb_id,
-                      direction,
-                      msg,
-                      msg_len,
-                      mac);
-    break;
-  default:
-    break;
+void nas::integrity_generate(uint8_t *key_128,
+                             uint32_t count,
+                             uint8_t rb_id,
+                             uint8_t direction,
+                             uint8_t *msg,
+                             uint32_t msg_len,
+                             uint8_t *mac) {
+  switch (integ_algo) {
+    case INTEGRITY_ALGORITHM_ID_EIA0:
+      break;
+    case INTEGRITY_ALGORITHM_ID_128_EIA1:
+      security_128_eia1(key_128,
+                        count,
+                        rb_id,
+                        direction,
+                        msg,
+                        msg_len,
+                        mac);
+      break;
+    case INTEGRITY_ALGORITHM_ID_128_EIA2:
+      security_128_eia2(key_128,
+                        count,
+                        rb_id,
+                        direction,
+                        msg,
+                        msg_len,
+                        mac);
+      break;
+    default:
+      break;
   }
 }
 
-void nas::integrity_check()
-{
+void nas::integrity_check() {
 
 }
 
-void nas::cipher_encrypt()
-{
+void nas::cipher_encrypt() {
 
 }
 
-void nas::cipher_decrypt()
-{
+void nas::cipher_decrypt() {
 
 }
 
 
-
 /*******************************************************************************
-  Parsers
+Parsers
 *******************************************************************************/
 
-void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu)
-{
-  LIBLTE_MME_ATTACH_ACCEPT_MSG_STRUCT                                attach_accept;
-  LIBLTE_MME_ACTIVATE_DEFAULT_EPS_BEARER_CONTEXT_REQUEST_MSG_STRUCT  act_def_eps_bearer_context_req;
-  LIBLTE_MME_ATTACH_COMPLETE_MSG_STRUCT                              attach_complete;
-  LIBLTE_MME_ACTIVATE_DEFAULT_EPS_BEARER_CONTEXT_ACCEPT_MSG_STRUCT   act_def_eps_bearer_context_accept;
+void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu) {
+  LIBLTE_MME_ATTACH_ACCEPT_MSG_STRUCT attach_accept;
+  LIBLTE_MME_ACTIVATE_DEFAULT_EPS_BEARER_CONTEXT_REQUEST_MSG_STRUCT act_def_eps_bearer_context_req;
+  LIBLTE_MME_ATTACH_COMPLETE_MSG_STRUCT attach_complete;
+  LIBLTE_MME_ACTIVATE_DEFAULT_EPS_BEARER_CONTEXT_ACCEPT_MSG_STRUCT act_def_eps_bearer_context_accept;
 
   nas_log->info("Received Attach Accept\n");
   count_dl++;
 
-  liblte_mme_unpack_attach_accept_msg((LIBLTE_BYTE_MSG_STRUCT*)pdu, &attach_accept);
+  liblte_mme_unpack_attach_accept_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &attach_accept);
 
-  if(attach_accept.eps_attach_result == LIBLTE_MME_EPS_ATTACH_RESULT_EPS_ONLY)
-  {
+  if (attach_accept.eps_attach_result == LIBLTE_MME_EPS_ATTACH_RESULT_EPS_ONLY) {
     //FIXME: Handle t3412.unit
     //FIXME: Handle tai_list
-    if(attach_accept.guti_present)
-    {
+    if (attach_accept.guti_present) {
       memcpy(&guti, &attach_accept.guti.guti, sizeof(LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT));
       is_guti_set = true;
       // TODO: log message to console
     }
-    if(attach_accept.lai_present)
-    {
+    if (attach_accept.lai_present) {
     }
-    if(attach_accept.ms_id_present)
-    {}
-    if(attach_accept.emm_cause_present)
-    {}
-    if(attach_accept.t3402_present)
-    {}
-    if(attach_accept.t3423_present)
-    {}
-    if(attach_accept.equivalent_plmns_present)
-    {}
-    if(attach_accept.emerg_num_list_present)
-    {}
-    if(attach_accept.eps_network_feature_support_present)
-    {}
-    if(attach_accept.additional_update_result_present)
-    {}
-    if(attach_accept.t3412_ext_present)
-    {}
+    if (attach_accept.ms_id_present) {}
+    if (attach_accept.emm_cause_present) {}
+    if (attach_accept.t3402_present) {}
+    if (attach_accept.t3423_present) {}
+    if (attach_accept.equivalent_plmns_present) {}
+    if (attach_accept.emerg_num_list_present) {}
+    if (attach_accept.eps_network_feature_support_present) {}
+    if (attach_accept.additional_update_result_present) {}
+    if (attach_accept.t3412_ext_present) {}
 
-    liblte_mme_unpack_activate_default_eps_bearer_context_request_msg(&attach_accept.esm_msg, &act_def_eps_bearer_context_req);
+    liblte_mme_unpack_activate_default_eps_bearer_context_request_msg(&attach_accept.esm_msg,
+                                                                      &act_def_eps_bearer_context_req);
 
-    if(LIBLTE_MME_PDN_TYPE_IPV4 == act_def_eps_bearer_context_req.pdn_addr.pdn_type)
-    {
+    if (LIBLTE_MME_PDN_TYPE_IPV4 == act_def_eps_bearer_context_req.pdn_addr.pdn_type) {
       ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[0] << 24;
       ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[1] << 16;
       ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[2] << 8;
@@ -263,28 +281,24 @@ void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu)
                     act_def_eps_bearer_context_req.pdn_addr.addr[3]);
 
       nas_log->console("Network attach successful.   IP: %u.%u.%u.%u\n",
-                    act_def_eps_bearer_context_req.pdn_addr.addr[0],
-                    act_def_eps_bearer_context_req.pdn_addr.addr[1],
-                    act_def_eps_bearer_context_req.pdn_addr.addr[2],
-                    act_def_eps_bearer_context_req.pdn_addr.addr[3]);
-      
+                       act_def_eps_bearer_context_req.pdn_addr.addr[0],
+                       act_def_eps_bearer_context_req.pdn_addr.addr[1],
+                       act_def_eps_bearer_context_req.pdn_addr.addr[2],
+                       act_def_eps_bearer_context_req.pdn_addr.addr[3]);
+
       // Setup GW
       char *err_str = NULL;
-      if(gw->setup_if_addr(ip_addr, err_str))
-      {
+      if (gw->setup_if_addr(ip_addr, err_str)) {
         nas_log->error("Failed to set gateway address - %s\n", err_str);
       }
-    }
-    else
-    {
+    } else {
       nas_log->error("Not handling IPV6 or IPV4V6\n");
       pool->deallocate(pdu);
       return;
     }
     eps_bearer_id = act_def_eps_bearer_context_req.eps_bearer_id;
-    if(act_def_eps_bearer_context_req.transaction_id_present)
-    {
-        transaction_id = act_def_eps_bearer_context_req.proc_transaction_id;
+    if (act_def_eps_bearer_context_req.transaction_id_present) {
+      transaction_id = act_def_eps_bearer_context_req.proc_transaction_id;
     }
 
     //FIXME: Handle the following parameters
@@ -306,20 +320,21 @@ void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu)
 
     // Send EPS bearer context accept and attach complete
     count_ul++;
-    act_def_eps_bearer_context_accept.eps_bearer_id                 = eps_bearer_id;
-    act_def_eps_bearer_context_accept.proc_transaction_id           = transaction_id;
-    act_def_eps_bearer_context_accept.protocol_cnfg_opts_present    = false;
-    liblte_mme_pack_activate_default_eps_bearer_context_accept_msg(&act_def_eps_bearer_context_accept, &attach_complete.esm_msg);
+    act_def_eps_bearer_context_accept.eps_bearer_id = eps_bearer_id;
+    act_def_eps_bearer_context_accept.proc_transaction_id = transaction_id;
+    act_def_eps_bearer_context_accept.protocol_cnfg_opts_present = false;
+    liblte_mme_pack_activate_default_eps_bearer_context_accept_msg(&act_def_eps_bearer_context_accept,
+                                                                   &attach_complete.esm_msg);
     liblte_mme_pack_attach_complete_msg(&attach_complete,
                                         LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY_AND_CIPHERED,
                                         count_ul,
-                                        (LIBLTE_BYTE_MSG_STRUCT*)pdu);
+                                        (LIBLTE_BYTE_MSG_STRUCT *) pdu);
     integrity_generate(&k_nas_int[16],
                        count_ul,
-                       lcid-1,
+                       lcid - 1,
                        SECURITY_DIRECTION_UPLINK,
                        &pdu->msg[5],
-                       pdu->N_bytes-5,
+                       pdu->N_bytes - 5,
                        &pdu->msg[1]);
 
     // Instruct RRC to enable capabilities
@@ -327,21 +342,18 @@ void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu)
 
     nas_log->info("Sending Attach Complete\n");
     rrc->write_sdu(lcid, pdu);
-    
-  }
-  else
-  {
+
+  } else {
     nas_log->info("Not handling attach type %u\n", attach_accept.eps_attach_result);
     state = EMM_STATE_DEREGISTERED;
     pool->deallocate(pdu);
   }
 }
 
-void nas::parse_attach_reject(uint32_t lcid, byte_buffer_t *pdu)
-{
+void nas::parse_attach_reject(uint32_t lcid, byte_buffer_t *pdu) {
   LIBLTE_MME_ATTACH_REJECT_MSG_STRUCT attach_rej;
 
-  liblte_mme_unpack_attach_reject_msg((LIBLTE_BYTE_MSG_STRUCT*)pdu, &attach_rej);
+  liblte_mme_unpack_attach_reject_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &attach_rej);
   nas_log->warning("Received Attach Reject. Cause= %02X\n", attach_rej.emm_cause);
   nas_log->console("Received Attach Reject. Cause= %02X\n", attach_rej.emm_cause);
   state = EMM_STATE_DEREGISTERED;
@@ -349,13 +361,12 @@ void nas::parse_attach_reject(uint32_t lcid, byte_buffer_t *pdu)
   // FIXME: Command RRC to release?
 }
 
-void nas::parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu)
-{
-  LIBLTE_MME_AUTHENTICATION_REQUEST_MSG_STRUCT  auth_req;
+void nas::parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu) {
+  LIBLTE_MME_AUTHENTICATION_REQUEST_MSG_STRUCT auth_req;
   LIBLTE_MME_AUTHENTICATION_RESPONSE_MSG_STRUCT auth_res;
 
   nas_log->info("Received Authentication Request\n");;
-  liblte_mme_unpack_authentication_request_msg((LIBLTE_BYTE_MSG_STRUCT*)pdu, &auth_req);
+  liblte_mme_unpack_authentication_request_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &auth_req);
 
   // Reuse the pdu for the response message
   pdu->reset();
@@ -367,56 +378,49 @@ void nas::parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu)
 
   nas_log->info("MCC=%d, MNC=%d\n", mcc, mnc);
 
-  bool    net_valid;
+  bool net_valid;
   uint8_t res[16];
   usim->generate_authentication_response(auth_req.rand, auth_req.autn, mcc, mnc, &net_valid, res);
 
-  if(net_valid)
-  {
+  if (net_valid) {
     nas_log->info("Network authentication successful\n");
-    for(int i=0; i<8; i++)
-    {
+    for (int i = 0; i < 8; i++) {
       auth_res.res[i] = res[i];
     }
-    liblte_mme_pack_authentication_response_msg(&auth_res, (LIBLTE_BYTE_MSG_STRUCT*)pdu);
+    liblte_mme_pack_authentication_response_msg(&auth_res, (LIBLTE_BYTE_MSG_STRUCT *) pdu);
 
     nas_log->info("Sending Authentication Response\n");
     rrc->write_sdu(lcid, pdu);
-  }
-  else
-  {
+  } else {
     nas_log->warning("Network authentication failure\n");
     nas_log->console("Warning: Network authentication failure\n");
     pool->deallocate(pdu);
   }
 }
 
-void nas::parse_authentication_reject(uint32_t lcid, byte_buffer_t *pdu)
-{
+void nas::parse_authentication_reject(uint32_t lcid, byte_buffer_t *pdu) {
   nas_log->warning("Received Authentication Reject\n");
   pool->deallocate(pdu);
   state = EMM_STATE_DEREGISTERED;
   // FIXME: Command RRC to release?
 }
 
-void nas::parse_identity_request(uint32_t lcid, byte_buffer_t *pdu)
-{
+void nas::parse_identity_request(uint32_t lcid, byte_buffer_t *pdu) {
   nas_log->error("TODO:parse_identity_request\n");
 }
 
-void nas::parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu)
-{
-  bool                                          success;
-  LIBLTE_MME_SECURITY_MODE_COMMAND_MSG_STRUCT   sec_mode_cmd;
-  LIBLTE_MME_SECURITY_MODE_COMPLETE_MSG_STRUCT  sec_mode_comp;
-  LIBLTE_MME_SECURITY_MODE_REJECT_MSG_STRUCT    sec_mode_rej;
+void nas::parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu) {
+  bool success;
+  LIBLTE_MME_SECURITY_MODE_COMMAND_MSG_STRUCT sec_mode_cmd;
+  LIBLTE_MME_SECURITY_MODE_COMPLETE_MSG_STRUCT sec_mode_comp;
+  LIBLTE_MME_SECURITY_MODE_REJECT_MSG_STRUCT sec_mode_rej;
 
   nas_log->info("Received Security Mode Command\n");
-  liblte_mme_unpack_security_mode_command_msg((LIBLTE_BYTE_MSG_STRUCT*)pdu, &sec_mode_cmd);
+  liblte_mme_unpack_security_mode_command_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &sec_mode_cmd);
 
   ksi = sec_mode_cmd.nas_ksi.nas_ksi;
-  cipher_algo = (CIPHERING_ALGORITHM_ID_ENUM)sec_mode_cmd.selected_nas_sec_algs.type_of_eea;
-  integ_algo  = (INTEGRITY_ALGORITHM_ID_ENUM)sec_mode_cmd.selected_nas_sec_algs.type_of_eia;
+  cipher_algo = (CIPHERING_ALGORITHM_ID_ENUM) sec_mode_cmd.selected_nas_sec_algs.type_of_eea;
+  integ_algo = (INTEGRITY_ALGORITHM_ID_ENUM) sec_mode_cmd.selected_nas_sec_algs.type_of_eia;
   // FIXME: Handle nonce_ue, nonce_mme
   // FIXME: Currently only handling ciphering EEA0 (null) and integrity EIA1,EIA2
   // FIXME: Use selected_nas_sec_algs to choose correct algos
@@ -425,17 +429,14 @@ void nas::parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu)
                  ksi, ciphering_algorithm_id_text[cipher_algo], integrity_algorithm_id_text[integ_algo]);
 
 
-  if(CIPHERING_ALGORITHM_ID_EEA0 != cipher_algo ||
-     (INTEGRITY_ALGORITHM_ID_128_EIA2 != integ_algo &&
-      INTEGRITY_ALGORITHM_ID_128_EIA1 != integ_algo) ||
-     sec_mode_cmd.nas_ksi.tsc_flag != LIBLTE_MME_TYPE_OF_SECURITY_CONTEXT_FLAG_NATIVE)
-  {
+  if (CIPHERING_ALGORITHM_ID_EEA0 != cipher_algo ||
+      (INTEGRITY_ALGORITHM_ID_128_EIA2 != integ_algo &&
+       INTEGRITY_ALGORITHM_ID_128_EIA1 != integ_algo) ||
+      sec_mode_cmd.nas_ksi.tsc_flag != LIBLTE_MME_TYPE_OF_SECURITY_CONTEXT_FLAG_NATIVE) {
     sec_mode_rej.emm_cause = LIBLTE_MME_EMM_CAUSE_UE_SECURITY_CAPABILITIES_MISMATCH;
     nas_log->warning("Sending Security Mode Reject due to security capabilities mismatch\n");
     success = false;
-  }
-  else
-  {
+  } else {
     // Generate NAS encryption key and integrity protection key
     usim->generate_nas_keys(k_nas_enc, k_nas_int, cipher_algo, integ_algo);
     nas_log->debug_hex(k_nas_enc, 32, "NAS encryption key - k_nas_enc");
@@ -446,38 +447,35 @@ void nas::parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu)
     uint8_t genMAC[4];
     integrity_generate(&k_nas_int[16],
                        count_dl,
-                       lcid-1,
+                       lcid - 1,
                        SECURITY_DIRECTION_DOWNLINK,
                        &pdu->msg[5],
-                       pdu->N_bytes-5,
+                       pdu->N_bytes - 5,
                        genMAC);
 
     nas_log->info_hex(inMAC, 4, "Incoming PDU MAC:");
     nas_log->info_hex(genMAC, 4, "Generated PDU MAC:");
 
-    bool match=true;
-    for(int i=0;i<4;i++) {
-      if(inMAC[i] != genMAC[i]) {
+    bool match = true;
+    for (int i = 0; i < 4; i++) {
+      if (inMAC[i] != genMAC[i]) {
         match = false;
       }
     }
-    if(!match) {
+    if (!match) {
       sec_mode_rej.emm_cause = LIBLTE_MME_EMM_CAUSE_SECURITY_MODE_REJECTED_UNSPECIFIED;
       nas_log->warning("Sending Security Mode Reject due to integrity check failure\n");
       success = false;
     } else {
 
-      if(sec_mode_cmd.imeisv_req_present && LIBLTE_MME_IMEISV_REQUESTED == sec_mode_cmd.imeisv_req)
-      {
-          sec_mode_comp.imeisv_present = true;
-          sec_mode_comp.imeisv.type_of_id = LIBLTE_MME_MOBILE_ID_TYPE_IMEISV;
-          usim->get_imei_vec(sec_mode_comp.imeisv.imeisv, 15);
-          sec_mode_comp.imeisv.imeisv[14] = 5;
-          sec_mode_comp.imeisv.imeisv[15] = 3;
-      }
-      else
-      {
-          sec_mode_comp.imeisv_present = false;
+      if (sec_mode_cmd.imeisv_req_present && LIBLTE_MME_IMEISV_REQUESTED == sec_mode_cmd.imeisv_req) {
+        sec_mode_comp.imeisv_present = true;
+        sec_mode_comp.imeisv.type_of_id = LIBLTE_MME_MOBILE_ID_TYPE_IMEISV;
+        usim->get_imei_vec(sec_mode_comp.imeisv.imeisv, 15);
+        sec_mode_comp.imeisv.imeisv[14] = 5;
+        sec_mode_comp.imeisv.imeisv[15] = 3;
+      } else {
+        sec_mode_comp.imeisv_present = false;
       }
 
       // Reuse pdu for response
@@ -485,13 +483,13 @@ void nas::parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu)
       liblte_mme_pack_security_mode_complete_msg(&sec_mode_comp,
                                                  LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY_AND_CIPHERED,
                                                  count_ul,
-                                                 (LIBLTE_BYTE_MSG_STRUCT*)pdu);
+                                                 (LIBLTE_BYTE_MSG_STRUCT *) pdu);
       integrity_generate(&k_nas_int[16],
                          count_ul,
-                         lcid-1,
+                         lcid - 1,
                          SECURITY_DIRECTION_UPLINK,
                          &pdu->msg[5],
-                         pdu->N_bytes-5,
+                         pdu->N_bytes - 5,
                          &pdu->msg[1]);
       nas_log->info("Sending Security Mode Complete nas_count_ul=%d, RB=%s\n",
                     count_ul,
@@ -500,44 +498,41 @@ void nas::parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu)
     }
   }
 
-  if(!success) {
+  if (!success) {
     // Reuse pdu for response
     pdu->reset();
-    liblte_mme_pack_security_mode_reject_msg(&sec_mode_rej, (LIBLTE_BYTE_MSG_STRUCT*)pdu);
+    liblte_mme_pack_security_mode_reject_msg(&sec_mode_rej, (LIBLTE_BYTE_MSG_STRUCT *) pdu);
   }
 
   rrc->write_sdu(lcid, pdu);
 }
 
-void nas::parse_service_reject(uint32_t lcid, byte_buffer_t *pdu)
-{
+void nas::parse_service_reject(uint32_t lcid, byte_buffer_t *pdu) {
   nas_log->error("TODO:parse_service_reject\n");
 }
-void nas::parse_esm_information_request(uint32_t lcid, byte_buffer_t *pdu)
-{
+
+void nas::parse_esm_information_request(uint32_t lcid, byte_buffer_t *pdu) {
   nas_log->error("TODO:parse_esm_information_request\n");
 }
-void nas::parse_emm_information(uint32_t lcid, byte_buffer_t *pdu)
-{
+
+void nas::parse_emm_information(uint32_t lcid, byte_buffer_t *pdu) {
   nas_log->error("TODO:parse_emm_information\n");
 }
 
 /*******************************************************************************
-  Senders
+Senders
 *******************************************************************************/
 
-void nas::send_attach_request()
-{
-  LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT  attach_req;
-  byte_buffer_t                  *msg = pool_allocate;
-  u_int32_t                             i;
+void nas::send_attach_request() {
+  LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT attach_req;
+  byte_buffer_t *msg = pool_allocate;
+  u_int32_t i;
 
   attach_req.eps_attach_type = LIBLTE_MME_EPS_ATTACH_TYPE_EPS_ATTACH;
 
-  for(i=0; i<8; i++)
-  {
-      attach_req.ue_network_cap.eea[i] = false;
-      attach_req.ue_network_cap.eia[i] = false;
+  for (i = 0; i < 8; i++) {
+    attach_req.ue_network_cap.eea[i] = false;
+    attach_req.ue_network_cap.eia[i] = false;
   }
   attach_req.ue_network_cap.eea[0] = true; // EEA0 supported
   attach_req.ue_network_cap.eia[0] = true; // EIA0 supported
@@ -571,45 +566,43 @@ void nas::send_attach_request()
   attach_req.old_guti_type_present = false;
 
   // Pack the message
-  liblte_mme_pack_attach_request_msg(&attach_req, (LIBLTE_BYTE_MSG_STRUCT*)msg);
+  liblte_mme_pack_attach_request_msg(&attach_req, (LIBLTE_BYTE_MSG_STRUCT *) msg);
 
   nas_log->info("Sending attach request\n");
   rrc->write_sdu(cfg.lcid, msg);
 }
 
-void nas::gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg)
-{
-    LIBLTE_MME_PDN_CONNECTIVITY_REQUEST_MSG_STRUCT  pdn_con_req;
+void nas::gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg) {
+  LIBLTE_MME_PDN_CONNECTIVITY_REQUEST_MSG_STRUCT pdn_con_req;
 
-    nas_log->info("Generating PDN Connectivity Request\n");
+  nas_log->info("Generating PDN Connectivity Request\n");
 
-    // Set the PDN con req parameters
-    pdn_con_req.eps_bearer_id       = 0x00; // Unassigned bearer ID
-    pdn_con_req.proc_transaction_id = 0x01; // First transaction ID
-    pdn_con_req.pdn_type            = LIBLTE_MME_PDN_TYPE_IPV4;
-    pdn_con_req.request_type        = LIBLTE_MME_REQUEST_TYPE_INITIAL_REQUEST;
+  // Set the PDN con req parameters
+  pdn_con_req.eps_bearer_id = 0x00; // Unassigned bearer ID
+  pdn_con_req.proc_transaction_id = 0x01; // First transaction ID
+  pdn_con_req.pdn_type = LIBLTE_MME_PDN_TYPE_IPV4;
+  pdn_con_req.request_type = LIBLTE_MME_REQUEST_TYPE_INITIAL_REQUEST;
 
-    // Set the optional flags
-    pdn_con_req.esm_info_transfer_flag_present  = false; //FIXME: Check if this is needed
-    pdn_con_req.apn_present                     = false;
-    pdn_con_req.protocol_cnfg_opts_present      = false;
-    pdn_con_req.device_properties_present       = false;
+  // Set the optional flags
+  pdn_con_req.esm_info_transfer_flag_present = false; //FIXME: Check if this is needed
+  pdn_con_req.apn_present = false;
+  pdn_con_req.protocol_cnfg_opts_present = false;
+  pdn_con_req.device_properties_present = false;
 
-    // Pack the message
-    liblte_mme_pack_pdn_connectivity_request_msg(&pdn_con_req, msg);
+  // Pack the message
+  liblte_mme_pack_pdn_connectivity_request_msg(&pdn_con_req, msg);
 }
 
-void nas::send_identity_response(){}
+void nas::send_identity_response() {}
 
-void nas::send_service_request()
-{
+void nas::send_service_request() {
   byte_buffer_t *msg = pool_allocate;
   count_ul++;
 
   // Pack the service request message directly
-  msg->msg[0]  = (LIBLTE_MME_SECURITY_HDR_TYPE_SERVICE_REQUEST << 4) | (LIBLTE_MME_PD_EPS_MOBILITY_MANAGEMENT);
+  msg->msg[0] = (LIBLTE_MME_SECURITY_HDR_TYPE_SERVICE_REQUEST << 4) | (LIBLTE_MME_PD_EPS_MOBILITY_MANAGEMENT);
   msg->N_bytes++;
-  msg->msg[1]  = (ksi & 0x07) << 5;
+  msg->msg[1] = (ksi & 0x07) << 5;
   msg->msg[1] |= count_ul & 0x1F;
   msg->N_bytes++;
 
@@ -630,6 +623,6 @@ void nas::send_service_request()
   rrc->write_sdu(cfg.lcid, msg);
 }
 
-void nas::send_esm_information_response(){}
+void nas::send_esm_information_response() {}
 
 } // namespace srsue
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 8b417f40d..cb7c117a3 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -27,19 +27,23 @@
 
 #include <unistd.h>
 #include <sstream>
-#include <srslte/asn1/liblte_rrc.h>
-
+#include "srslte/asn1/liblte_rrc.h"
 #include "upper/rrc.h"
-#include "srslte/phy/utils/bit.h"
+#include <boost/assign.hpp>
+#include <upper/rrc.h>
 #include "srslte/common/security.h"
 #include "srslte/common/bcd_helpers.h"
-#include "boost/assign.hpp"
 
 #define TIMEOUT_RESYNC_REESTABLISH 100
 
 using namespace srslte;
 
-namespace srsue{
+namespace srsue {
+
+
+/*******************************************************************************
+  Base functions 
+*******************************************************************************/
 
 rrc::rrc()
   :state(RRC_STATE_IDLE)
@@ -48,12 +52,11 @@ rrc::rrc()
 }
 
 static void liblte_rrc_handler(void *ctx, char *str) {
-  rrc *r = (rrc*) ctx; 
+  rrc *r = (rrc *) ctx;
   r->liblte_rrc_log(str);
 }
 
-void rrc::liblte_rrc_log(char* str)
-{
+void rrc::liblte_rrc_log(char *str) {
   if (rrc_log) {
     rrc_log->warning("[ASN]: %s\n", str);
   } else {
@@ -61,115 +64,465 @@ void rrc::liblte_rrc_log(char* str)
   }
 }
 
-void rrc::init(phy_interface_rrc     *phy_,
-               mac_interface_rrc     *mac_,
-               rlc_interface_rrc     *rlc_,
-               pdcp_interface_rrc    *pdcp_,
-               nas_interface_rrc     *nas_,
-               usim_interface_rrc    *usim_,
-               mac_interface_timers  *mac_timers_,
-               srslte::log           *rrc_log_)
-{
-  pool    = byte_buffer_pool::get_instance();
-  phy     = phy_;
-  mac     = mac_;
-  rlc     = rlc_;
-  pdcp    = pdcp_;
-  nas     = nas_;
-  usim    = usim_;
+void rrc::init(phy_interface_rrc *phy_,
+               mac_interface_rrc *mac_,
+               rlc_interface_rrc *rlc_,
+               pdcp_interface_rrc *pdcp_,
+               nas_interface_rrc *nas_,
+               usim_interface_rrc *usim_,
+               mac_interface_timers *mac_timers_,
+               srslte::log *rrc_log_) {
+  pool = byte_buffer_pool::get_instance();
+  phy = phy_;
+  mac = mac_;
+  rlc = rlc_;
+  pdcp = pdcp_;
+  nas = nas_;
+  usim = usim_;
   rrc_log = rrc_log_;
 
   // Use MAC timers
   mac_timers = mac_timers_;
+  state = RRC_STATE_IDLE;
+  si_acquire_state = SI_ACQUIRE_IDLE;
+
+  thread_running = true;
+  start();
+
+  pthread_mutex_init(&mutex, NULL);
+
+  ue_category = SRSLTE_UE_CATEGORY;
   t301 = mac_timers->get_unique_id();
   t310 = mac_timers->get_unique_id();
   t311 = mac_timers->get_unique_id();
   safe_reset_timer = mac_timers->get_unique_id();
 
-
-  pthread_mutex_init(&mutex, NULL); 
-  
-  ue_category = SRSLTE_UE_CATEGORY; 
-  
   transaction_id = 0;
-  
+
   // Register logging handler with liblte_rrc
   liblte_rrc_log_register_handler(this, liblte_rrc_handler);
-  
-  // Set default values for all layers 
+
+  // Set default values for all layers
   set_rrc_default();
   set_phy_default();
   set_mac_default();
 }
 
-void rrc::stop()
-{}
+void rrc::stop() {
+  thread_running = false;
+  wait_thread_finish();
+}
 
-rrc_state_t rrc::get_state()
-{
+rrc_state_t rrc::get_state() {
   return state;
 }
 
-void rrc::set_ue_category(int category)
-{
-  if(category >= 1 && category <= 5) {
-    ue_category = category; 
+void rrc::set_ue_category(int category) {
+  if (category >= 1 && category <= 5) {
+    ue_category = category;
   } else {
     rrc_log->error("Unsupported UE category %d\n", category);
   }
 }
 
 
+
 /*******************************************************************************
-  NAS interface
+*
+*
+*
+* PLMN selection, cell selection/reselection and acquisition of SI procedures
+*
+*
+*
 *******************************************************************************/
 
-void rrc::write_sdu(uint32_t lcid, byte_buffer_t *sdu)
-{
-  rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", rb_id_str[lcid].c_str());
+/*******************************************************************************
+NAS interface
+*******************************************************************************/
 
-  switch(state)
-  {
-  case RRC_STATE_COMPLETING_SETUP:
-    send_con_setup_complete(sdu);
-    break;
-  case RRC_STATE_RRC_CONNECTED:
-    send_ul_info_transfer(lcid, sdu);
-    break;
-  default:
-    rrc_log->error("SDU received from NAS while RRC state = %s", rrc_state_text[state]);
-    break;
+uint16_t rrc::get_mcc() {
+  if (current_cell) {
+    if (current_cell->sib1.N_plmn_ids > 0) {
+      return current_cell->sib1.plmn_id[0].id.mcc;
+    }
+  }
+  return 0;
+}
+
+uint16_t rrc::get_mnc() {
+  if (current_cell) {
+    if (current_cell->sib1.N_plmn_ids > 0) {
+      return current_cell->sib1.plmn_id[0].id.mnc;
+    }
+  }
+  return 0;
+}
+
+void rrc::plmn_search() {
+  rrc_log->info("Starting PLMN search procedure\n");
+  state = RRC_STATE_PLMN_SELECTION;
+  phy->cell_search_start();
+  plmn_select_timeout = 0;
+}
+
+void rrc::plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {
+  rrc_log->info("PLMN %s selected\n", plmn_id_to_c_str(plmn_id).c_str());
+
+
+  // Sort cells according to RSRP
+
+  selected_plmn_id = plmn_id;
+  last_selected_cell = -1;
+  select_cell_timeout = 0;
+
+  state = RRC_STATE_CELL_SELECTING;
+  select_next_cell_in_plmn();
+}
+
+void rrc::select_next_cell_in_plmn() {
+  for (uint32_t i = last_selected_cell + 1; i < known_cells.size(); i++) {
+    for (uint32_t j = 0; j < known_cells[i].sib1.N_plmn_ids; j++) {
+      if (known_cells[i].sib1.plmn_id[j].id.mcc == selected_plmn_id.mcc ||
+          known_cells[i].sib1.plmn_id[j].id.mnc == selected_plmn_id.mnc) {
+        rrc_log->info("Selecting cell PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
+                      known_cells[i].phy_cell.id, known_cells[i].earfcn,
+                      known_cells[i].sib1.cell_id);
+        rrc_log->console("Select cell: PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
+                      known_cells[i].phy_cell.id, known_cells[i].earfcn,
+                      known_cells[i].sib1.cell_id);
+        // Check that cell satisfies S criteria
+        if (known_cells[i].in_sync) { // %% rsrp > S dbm
+          // Try to select Cell
+          if (phy->cell_select(known_cells[i].earfcn, known_cells[i].phy_cell))
+          {
+            last_selected_cell = i;
+            current_cell = &known_cells[i];
+            return;
+          } else {
+            rrc_log->warning("Selecting cell EARFCN=%d, Cell ID=0x%x.\n",
+                           known_cells[i].earfcn, known_cells[i].sib1.cell_id);
+          }
+        }
+      }
+    }
+  }
+  rrc_log->info("No more known cells...\n");
+}
+
+/*******************************************************************************
+PHY interface
+*******************************************************************************/
+
+void rrc::cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
+
+  // find if cell_id-earfcn combination already exists
+  for (uint32_t i = 0; i < known_cells.size(); i++) {
+    if (earfcn == known_cells[i].earfcn && phy_cell.id == known_cells[i].phy_cell.id) {
+      known_cells[i].rsrp    = rsrp;
+      known_cells[i].in_sync = true;
+      current_cell = &known_cells[i];
+      rrc_log->info("Updating cell EARFCN=%d, PCI=%d, RSRP=%.1f dBm\n", known_cells[i].earfcn,
+                    known_cells[i].phy_cell.id, known_cells[i].rsrp);
+
+      if (!known_cells[i].has_valid_sib1) {
+        si_acquire_state = SI_ACQUIRE_SIB1;
+      } else {
+        for (uint32_t i = 0; i < current_cell->sib1.N_plmn_ids; i++) {
+          nas->plmn_found(current_cell->sib1.plmn_id[i].id, current_cell->sib1.tracking_area_code);
+        }
+      }
+      return;
+    }
+  }
+  // add to list of known cells
+  cell_t cell;
+  cell.phy_cell = phy_cell;
+  cell.rsrp = rsrp;
+  cell.earfcn = earfcn;
+  cell.has_valid_sib1 = false;
+  cell.has_valid_sib2 = false;
+  known_cells.push_back(cell);
+
+  // save current cell
+  current_cell = &known_cells.back();
+
+  si_acquire_state = SI_ACQUIRE_SIB1;
+
+  rrc_log->info("New Cell: PCI=%d, PRB=%d, Ports=%d, EARFCN=%d, RSRP=%.1f dBm\n",
+                cell.phy_cell.id, cell.phy_cell.nof_prb, cell.phy_cell.nof_ports,
+                cell.earfcn, cell.rsrp);
+}
+
+// Detection of physical layer problems (5.3.11.1)
+void rrc::out_of_sync() {
+    current_cell->in_sync = false;
+  if (!mac_timers->get(t311)->is_running() && !mac_timers->get(t310)->is_running()) {
+    n310_cnt++;
+    if (n310_cnt == N310) {
+      mac_timers->get(t310)->reset();
+      mac_timers->get(t310)->run();
+      n310_cnt = 0;
+      rrc_log->info("Detected %d out-of-sync from PHY. Starting T310 timer\n", N310);
+    }
   }
 }
 
-uint16_t rrc::get_mcc()
-{
-  if(sib1.N_plmn_ids > 0)
-    return sib1.plmn_id[0].id.mcc;
-  else
-    return 0;
-}
-
-uint16_t rrc::get_mnc()
-{
-  if(sib1.N_plmn_ids > 0)
-    return sib1.plmn_id[0].id.mnc;
-  else
-    return 0;
+// Recovery of physical layer problems (5.3.11.2)
+void rrc::in_sync() {
+  current_cell->in_sync = true;
+  if (mac_timers->get(t310)->is_running()) {
+    n311_cnt++;
+    if (n311_cnt == N311) {
+      mac_timers->get(t310)->stop();
+      n311_cnt = 0;
+      rrc_log->info("Detected %d in-sync from PHY. Stopping T310 timer\n", N311);
+    }
+  }
 }
 
 /*******************************************************************************
-  MAC interface
+PDCP interface
+*******************************************************************************/
+void rrc::write_pdu_bcch_bch(byte_buffer_t *pdu) {
+  pool->deallocate(pdu);
+  if (state == RRC_STATE_PLMN_SELECTION) {
+    // Do we need to do something with BCH?
+    rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH BCH message received.");
+  } else {
+    rrc_log->warning("Received BCCH BCH in incorrect state\n");
+  }
+}
+
+void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu) {
+  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH DLSCH message received.");
+  rrc_log->info("BCCH DLSCH message Stack latency: %ld us\n", pdu->get_latency_us());
+  LIBLTE_RRC_BCCH_DLSCH_MSG_STRUCT dlsch_msg;
+  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
+  bit_buf.N_bits = pdu->N_bytes * 8;
+  pool->deallocate(pdu);
+  liblte_rrc_unpack_bcch_dlsch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &dlsch_msg);
+
+  if (dlsch_msg.N_sibs > 0) {
+    if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1 == dlsch_msg.sibs[0].sib_type && SI_ACQUIRE_SIB1 == si_acquire_state) {
+      mac->bcch_stop_rx();
+
+      // Handle SIB1
+      memcpy(&current_cell->sib1, &dlsch_msg.sibs[0].sib.sib1, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT));
+
+      rrc_log->info("SIB1 received, CellID=%d, si_window=%d, sib2_period=%d\n",
+                    current_cell->sib1.cell_id & 0xfff,
+                    liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length],
+                    liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity]);
+
+
+      // Set TDD Config
+      if (current_cell->sib1.tdd) {
+        phy->set_config_tdd(&current_cell->sib1.tdd_cnfg);
+      }
+
+      current_cell->has_valid_sib1 = true;
+
+      // Send PLMN and TAC to NAS
+      std::stringstream ss;
+      for (uint32_t i = 0; i < current_cell->sib1.N_plmn_ids; i++) {
+        nas->plmn_found(current_cell->sib1.plmn_id[i].id, current_cell->sib1.tracking_area_code);
+      }
+
+      // Jump to next state
+      switch(state) {
+        case RRC_STATE_CELL_SELECTING:
+          si_acquire_state = SI_ACQUIRE_SIB2;
+          break;
+        case RRC_STATE_PLMN_SELECTION:
+          si_acquire_state = SI_ACQUIRE_IDLE;
+          rrc_log->info("SI Acquisition done. Searching next cell...\n");
+          usleep(5000);
+          phy->cell_search_next();
+          break;
+        default:
+          si_acquire_state = SI_ACQUIRE_IDLE;
+      }
+
+    } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2 == dlsch_msg.sibs[0].sib_type &&
+               SI_ACQUIRE_SIB2 == si_acquire_state) {
+      mac->bcch_stop_rx();
+
+      // Handle SIB2
+      memcpy(&current_cell->sib2, &dlsch_msg.sibs[0].sib.sib2, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT));
+      rrc_log->info("SIB2 received\n");
+
+      apply_sib2_configs(&current_cell->sib2);
+
+      current_cell->has_valid_sib2 = true;
+
+      // Jump to next state
+      switch(state) {
+        case RRC_STATE_CELL_SELECTING:
+          si_acquire_state = SI_ACQUIRE_IDLE;
+          state = RRC_STATE_CELL_SELECTED;
+          break;
+        default:
+          si_acquire_state = SI_ACQUIRE_IDLE;
+      }
+    }
+  }
+}
+
+
+// Right now, this thread only controls System Information acquisition procedure
+void rrc::run_thread() {
+  uint32_t tti;
+  uint32_t si_win_start=0, si_win_len=0, last_win_start=0;
+  uint16_t period;
+  uint32_t nof_sib1_trials = 0;
+  const int SIB1_SEARCH_TIMEOUT = 30;
+
+  while (thread_running) {
+    switch (si_acquire_state) {
+      case SI_ACQUIRE_SIB1:
+        // Instruct MAC to look for SIB1
+        tti = mac->get_current_tti();
+        si_win_start = sib_start_tti(tti, 2, 5);
+        if (tti > last_win_start + 10) {
+          last_win_start = si_win_start;
+          mac->bcch_start_rx(si_win_start, 1);
+          rrc_log->debug("Instructed MAC to search for SIB1, win_start=%d, win_len=%d\n",
+                         si_win_start, 1);
+          nof_sib1_trials++;
+          if (nof_sib1_trials >= SIB1_SEARCH_TIMEOUT) {
+            if (state == RRC_STATE_CELL_SELECTING) {
+              select_next_cell_in_plmn();
+              si_acquire_state = SI_ACQUIRE_IDLE;
+            } else if (state == RRC_STATE_PLMN_SELECTION) {
+              phy->cell_search_next();
+            }
+            nof_sib1_trials = 0;
+          }
+        }
+        break;
+      case SI_ACQUIRE_SIB2:
+        // Instruct MAC to look for SIB2 only when selecting a cell
+        tti = mac->get_current_tti();
+        period = liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity];
+        si_win_start = sib_start_tti(tti, period, 0);
+        if (tti > last_win_start + 10) {
+          last_win_start = si_win_start;
+          si_win_len = liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length];
+
+          mac->bcch_start_rx(si_win_start, si_win_len);
+          rrc_log->debug("Instructed MAC to search for SIB2, win_start=%d, win_len=%d\n",
+                         si_win_start, si_win_len);
+        }
+        break;
+      default:
+        break;
+    }
+    switch(state) {
+      case RRC_STATE_IDLE:
+        if (nas->is_attached()) {
+          usleep(10000);
+          rrc_log->info("RRC IDLE: NAS is attached, re-selecting cell...\n");
+          plmn_select(selected_plmn_id);
+        }
+        break;
+      case RRC_STATE_PLMN_SELECTION:
+        plmn_select_timeout++;
+        if (plmn_select_timeout >= RRC_PLMN_SELECT_TIMEOUT) {
+          rrc_log->info("RRC PLMN Search: timeout expired. Searching again\n");
+          sleep(1);
+          rrc_log->console("\nRRC PLMN Search: timeout expired. Searching again\n");
+          plmn_select_timeout = 0;
+          phy->cell_search_start();
+        }
+        break;
+      case RRC_STATE_CELL_SELECTING:
+        if (phy->sync_status()) {
+          if (!current_cell->has_valid_sib1) {
+            si_acquire_state = SI_ACQUIRE_SIB1;
+          } else if (!current_cell->has_valid_sib2) {
+            si_acquire_state = SI_ACQUIRE_SIB2;
+          } else {
+            apply_sib2_configs(&current_cell->sib2);
+            si_acquire_state = SI_ACQUIRE_IDLE;
+            state = RRC_STATE_CELL_SELECTED;
+          }
+        }
+        select_cell_timeout++;
+        if (select_cell_timeout >= RRC_SELECT_CELL_TIMEOUT) {
+          rrc_log->info("RRC Cell Selecting: timeout expired. Starting Cell Search...\n");
+          state = RRC_STATE_PLMN_SELECTION;
+          plmn_select_timeout = 0;
+          phy->cell_search_start();
+        }
+        break;
+      case RRC_STATE_CELL_SELECTED:
+        rrc_log->info("RRC Cell Selected: Sending connection request...\n");
+        send_con_request();
+        state = RRC_STATE_CONNECTING;
+        connecting_timeout = 0;
+        break;
+      case RRC_STATE_CONNECTING:
+        connecting_timeout++;
+        if (connecting_timeout >= RRC_CONNECTING_TIMEOUT) {
+          // Select another cell
+          rrc_log->info("RRC Connecting: timeout expired. Selecting next cell\n");
+          state = RRC_STATE_CELL_SELECTING;
+        }
+        break;
+      case RRC_STATE_CONNECTED:
+        // Take measurements, cell reselection, etc
+        break;
+      default:
+        break;
+    }
+    usleep(1000);
+  }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+/*******************************************************************************
+NAS interface
+*******************************************************************************/
+
+void rrc::write_sdu(uint32_t lcid, byte_buffer_t *sdu) {
+  rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", get_rb_name(lcid).c_str());
+  switch (state) {
+    case RRC_STATE_CONNECTING:
+      send_con_setup_complete(sdu);
+      break;
+    case RRC_STATE_CONNECTED:
+      send_ul_info_transfer(lcid, sdu);
+      break;
+    default:
+      rrc_log->error("SDU received from NAS while RRC state = %s", rrc_state_text[state]);
+      break;
+  }
+}
+
+
+/*******************************************************************************
+MAC interface
 *******************************************************************************/
 /* Reception of PUCCH/SRS release procedure (Section 5.3.13) */
-void rrc::release_pucch_srs()
-{
+void rrc::release_pucch_srs() {
   // Apply default configuration for PUCCH (CQI and SR) and SRS (release)
   set_phy_default_pucch_srs();
-  
+
   // Configure RX signals without pregeneration because default option is release
   phy->configure_ul_params(true);
-  
 }
 
 void rrc::ra_problem() {
@@ -177,192 +530,75 @@ void rrc::ra_problem() {
 }
 
 /*******************************************************************************
-  PHY interface
+GW interface
 *******************************************************************************/
 
-// Detection of physical layer problems (5.3.11.1)
-void rrc::out_of_sync()
-{
-  if (!mac_timers->get(t311)->is_running() && !mac_timers->get(t310)->is_running()) {
-    n310_cnt++;
-    if (n310_cnt == N310) {
-      mac_timers->get(t310)->reset();
-      mac_timers->get(t310)->run();
-      n310_cnt = 0; 
-      rrc_log->info("Detected %d out-of-sync from PHY. Starting T310 timer\n", N310);
-    }
-  }
+bool rrc::is_connected() {
+  return (RRC_STATE_CONNECTED == state);
 }
 
-// Recovery of physical layer problems (5.3.11.2)
-void rrc::in_sync()
-{
-  if (mac_timers->get(t310)->is_running()) {
-    n311_cnt++;
-    if (n311_cnt == N311) {
-      mac_timers->get(t310)->stop();      
-      n311_cnt = 0; 
-      rrc_log->info("Detected %d in-sync from PHY. Stopping T310 timer\n", N311);
-    }
-  }
-}
-
-/*******************************************************************************
-  GW interface
-*******************************************************************************/
-
-bool rrc::rrc_connected()
-{
-  return (RRC_STATE_RRC_CONNECTED == state);
-}
-
-void rrc::rrc_connect() {
-  pthread_mutex_lock(&mutex);
-  if(RRC_STATE_IDLE == state) {
-    rrc_log->info("RRC in IDLE state - sending connection request.\n");
-    state = RRC_STATE_WAIT_FOR_CON_SETUP;
-    send_con_request();
-  }
-  pthread_mutex_unlock(&mutex);
-}
-
-bool rrc::have_drb()
-{
+bool rrc::have_drb() {
   return drb_up;
 }
 
 /*******************************************************************************
-  PDCP interface
+PDCP interface
 *******************************************************************************/
 
-void rrc::write_pdu(uint32_t lcid, byte_buffer_t *pdu)
-{
-  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "TX %s PDU", rb_id_str[lcid].c_str());
+void rrc::write_pdu(uint32_t lcid, byte_buffer_t *pdu) {
+  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "TX %s PDU", get_rb_name(lcid).c_str());
   rrc_log->info("TX PDU Stack latency: %ld us\n", pdu->get_latency_us());
 
-  switch(lcid)
-  {
-  case RB_ID_SRB0:
-    parse_dl_ccch(pdu);
-    break;
-  case RB_ID_SRB1:
-  case RB_ID_SRB2:
-    parse_dl_dcch(lcid, pdu);
-    break;
-  default:
-    rrc_log->error("TX PDU with invalid bearer id: %s", lcid);
-    break;
-  }
-
-}
-
-void rrc::write_pdu_bcch_bch(byte_buffer_t *pdu)
-{
-  // Unpack the MIB
-  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH BCH message received.");
-  rrc_log->info("BCCH BCH message Stack latency: %ld us\n", pdu->get_latency_us());
-  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes*8);
-  bit_buf.N_bits = pdu->N_bytes*8;
-  pool->deallocate(pdu);
-  liblte_rrc_unpack_bcch_bch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &mib);
-  rrc_log->info("MIB received BW=%s MHz\n", liblte_rrc_dl_bandwidth_text[mib.dl_bw]);
-  rrc_log->console("MIB received BW=%s MHz\n", liblte_rrc_dl_bandwidth_text[mib.dl_bw]);
-
-  // Start the SIB search state machine
-  state = RRC_STATE_SIB1_SEARCH;
-  pthread_create(&sib_search_thread, NULL, &rrc::start_sib_thread, this);
-}
-
-void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu)
-{
-  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH DLSCH message received.");
-  rrc_log->info("BCCH DLSCH message Stack latency: %ld us\n", pdu->get_latency_us());
-  LIBLTE_RRC_BCCH_DLSCH_MSG_STRUCT dlsch_msg;
-  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes*8);
-  bit_buf.N_bits = pdu->N_bytes*8;
-  pool->deallocate(pdu);
-  liblte_rrc_unpack_bcch_dlsch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &dlsch_msg);
-
-  if (dlsch_msg.N_sibs > 0) {
-    if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1 == dlsch_msg.sibs[0].sib_type && RRC_STATE_SIB1_SEARCH == state) {
-      // Handle SIB1
-      memcpy(&sib1, &dlsch_msg.sibs[0].sib.sib1, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT));
-      rrc_log->info("SIB1 received, CellID=%d, si_window=%d, sib2_period=%d\n",
-                    sib1.cell_id&0xfff,
-                    liblte_rrc_si_window_length_num[sib1.si_window_length],
-                    liblte_rrc_si_periodicity_num[sib1.sched_info[0].si_periodicity]);
-      std::stringstream ss;
-      for(uint32_t i=0;i<sib1.N_plmn_ids;i++){
-        std::string mcc;
-        std::string mnc;
-        mcc_to_string(sib1.plmn_id[i].id.mcc, &mcc);
-        mnc_to_string(sib1.plmn_id[i].id.mnc, &mnc);
-        ss << " PLMN Id: MCC " << mcc << " MNC " << mnc;
-      }
-      
-      // Set TDD Config 
-      if (dlsch_msg.sibs[0].sib.sib1.tdd) {
-        phy->set_config_tdd(&dlsch_msg.sibs[0].sib.sib1.tdd_cnfg);
-      }
-      
-      rrc_log->console("SIB1 received, CellID=%d, %s\n",
-                       sib1.cell_id&0xfff,
-                       ss.str().c_str());
-      
-      state = RRC_STATE_SIB2_SEARCH;
-      mac->bcch_stop_rx();
-      //TODO: Use all SIB1 info
-
-    } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2 == dlsch_msg.sibs[0].sib_type && RRC_STATE_SIB2_SEARCH == state) {
-      // Handle SIB2
-      memcpy(&sib2, &dlsch_msg.sibs[0].sib.sib2, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT));
-      rrc_log->console("SIB2 received\n");
-      rrc_log->info("SIB2 received\n");
-      state = RRC_STATE_WAIT_FOR_CON_SETUP;
-      mac->bcch_stop_rx();
-      apply_sib2_configs();
-      send_con_request();
-    }
+  switch (lcid) {
+    case RB_ID_SRB0:
+      parse_dl_ccch(pdu);
+      break;
+    case RB_ID_SRB1:
+    case RB_ID_SRB2:
+      parse_dl_dcch(lcid, pdu);
+      break;
+    default:
+      rrc_log->error("TX PDU with invalid bearer id: %s", lcid);
+      break;
   }
 }
 
-void rrc::write_pdu_pcch(byte_buffer_t *pdu)
-{
+void rrc::write_pdu_pcch(byte_buffer_t *pdu) {
   if (pdu->N_bytes > 0 && pdu->N_bytes < SRSLTE_MAX_BUFFER_SIZE_BITS) {
     rrc_log->info_hex(pdu->msg, pdu->N_bytes, "PCCH message received %d bytes\n", pdu->N_bytes);
     rrc_log->info("PCCH message Stack latency: %ld us\n", pdu->get_latency_us());
     rrc_log->console("PCCH message received %d bytes\n", pdu->N_bytes);
-    
+
     LIBLTE_RRC_PCCH_MSG_STRUCT pcch_msg;
-    srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes*8);
-    bit_buf.N_bits = pdu->N_bytes*8;
+    srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
+    bit_buf.N_bits = pdu->N_bytes * 8;
     pool->deallocate(pdu);
-    liblte_rrc_unpack_pcch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &pcch_msg);
-    
+    liblte_rrc_unpack_pcch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &pcch_msg);
+
     if (pcch_msg.paging_record_list_size > LIBLTE_RRC_MAX_PAGE_REC) {
-      pcch_msg.paging_record_list_size = LIBLTE_RRC_MAX_PAGE_REC;     
+      pcch_msg.paging_record_list_size = LIBLTE_RRC_MAX_PAGE_REC;
     }
 
     LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
-    if(!nas->get_s_tmsi(&s_tmsi)) {
+    if (!nas->get_s_tmsi(&s_tmsi)) {
       rrc_log->info("No S-TMSI present in NAS\n");
       return;
     }
-    
+
     LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi_paged;
-    for (uint32_t i=0;i<pcch_msg.paging_record_list_size;i++) {
+    for (uint32_t i = 0; i < pcch_msg.paging_record_list_size; i++) {
       s_tmsi_paged = &pcch_msg.paging_record_list[i].ue_identity.s_tmsi;
-      rrc_log->info("Received paging (%d/%d) for UE 0x%x\n", i+1, pcch_msg.paging_record_list_size,
+      rrc_log->info("Received paging (%d/%d) for UE 0x%x\n", i + 1, pcch_msg.paging_record_list_size,
                     pcch_msg.paging_record_list[i].ue_identity.s_tmsi);
-      rrc_log->console("Received paging (%d/%d) for UE 0x%x\n", i+1, pcch_msg.paging_record_list_size,
-                      pcch_msg.paging_record_list[i].ue_identity.s_tmsi);
-      if(s_tmsi.mmec == s_tmsi_paged->mmec && s_tmsi.m_tmsi == s_tmsi_paged->m_tmsi) {
+      rrc_log->console("Received paging (%d/%d) for UE 0x%x\n", i + 1, pcch_msg.paging_record_list_size,
+                       pcch_msg.paging_record_list[i].ue_identity.s_tmsi);
+      if (s_tmsi.mmec == s_tmsi_paged->mmec && s_tmsi.m_tmsi == s_tmsi_paged->m_tmsi) {
         rrc_log->info("S-TMSI match in paging message\n");
         rrc_log->console("S-TMSI match in paging message\n");
         mac->pcch_stop_rx();
-        if(RRC_STATE_IDLE == state) {
+        if (RRC_STATE_IDLE == state) {
           rrc_log->info("RRC in IDLE state - sending connection request.\n");
-          state = RRC_STATE_WAIT_FOR_CON_SETUP;
+          state = RRC_STATE_CONNECTING;
           send_con_request();
         }
       }
@@ -371,29 +607,27 @@ void rrc::write_pdu_pcch(byte_buffer_t *pdu)
 }
 
 /*******************************************************************************
-  RLC interface
+RLC interface
 *******************************************************************************/
 
-void rrc::max_retx_attempted()
-{
+void rrc::max_retx_attempted() {
   //TODO: Handle the radio link failure
   rrc_log->warning("Max RLC reTx attempted\n");
   //radio_link_failure();
 }
 
 /*******************************************************************************
-  Senders
+Senders
 *******************************************************************************/
 
-void rrc::send_con_request()
-{
+void rrc::send_con_request() {
   rrc_log->debug("Preparing RRC Connection Request\n");
   LIBLTE_RRC_UL_CCCH_MSG_STRUCT ul_ccch_msg;
-  LIBLTE_RRC_S_TMSI_STRUCT      s_tmsi;
+  LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
 
   // Prepare ConnectionRequest packet
   ul_ccch_msg.msg_type = LIBLTE_RRC_UL_CCCH_MSG_TYPE_RRC_CON_REQ;
-  if(nas->get_s_tmsi(&s_tmsi)) {
+  if (nas->get_s_tmsi(&s_tmsi)) {
     ul_ccch_msg.msg.rrc_con_req.ue_id_type = LIBLTE_RRC_CON_REQ_UE_ID_TYPE_S_TMSI;
     ul_ccch_msg.msg.rrc_con_req.ue_id.s_tmsi = s_tmsi;
   } else {
@@ -401,158 +635,148 @@ void rrc::send_con_request()
     ul_ccch_msg.msg.rrc_con_req.ue_id.random = 1000;
   }
   ul_ccch_msg.msg.rrc_con_req.cause = LIBLTE_RRC_CON_REQ_EST_CAUSE_MO_SIGNALLING;
-  liblte_rrc_pack_ul_ccch_msg(&ul_ccch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
+  liblte_rrc_pack_ul_ccch_msg(&ul_ccch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
 
   // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
       bit_buf.msg[bit_buf.N_bits + i] = 0;
     bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
-  byte_buffer_t *pdcp_buf = pool_allocate;
+  byte_buffer_t *pdcp_buf = pool_allocate;;
   srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
-  pdcp_buf->N_bytes = bit_buf.N_bits/8;
+  pdcp_buf->N_bytes = bit_buf.N_bits / 8;
   pdcp_buf->set_timestamp();
 
   // Set UE contention resolution ID in MAC
-  uint64_t uecri=0;
-  uint8_t *ue_cri_ptr = (uint8_t*) &uecri;
+  uint64_t uecri = 0;
+  uint8_t *ue_cri_ptr = (uint8_t *) &uecri;
   uint32_t nbytes = 6;
-  for (uint32_t i=0;i<nbytes;i++) {
-    ue_cri_ptr[nbytes-i-1] = pdcp_buf->msg[i];
+  for (uint32_t i = 0; i < nbytes; i++) {
+    ue_cri_ptr[nbytes - i - 1] = pdcp_buf->msg[i];
   }
   rrc_log->debug("Setting UE contention resolution ID: %d\n", uecri);
-  
+
   mac->set_contention_id(uecri);
 
   rrc_log->info("Sending RRC Connection Request on SRB0\n");
-  state = RRC_STATE_WAIT_FOR_CON_SETUP;
   pdcp->write_sdu(RB_ID_SRB0, pdcp_buf);
 }
 
-
 /* RRC connection re-establishment procedure (5.3.7) */
-void rrc::send_con_restablish_request()
-{
-  
-  srslte_cell_t cell; 
+void rrc::send_con_restablish_request() {
+
+  srslte_cell_t cell;
   phy->get_current_cell(&cell);
 
   LIBLTE_RRC_UL_CCCH_MSG_STRUCT ul_ccch_msg;
-  LIBLTE_RRC_S_TMSI_STRUCT      s_tmsi;
+  LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
 
   // Compute shortMAC-I
   uint8_t varShortMAC[128], varShortMAC_packed[16];
   bzero(varShortMAC, 128);
   bzero(varShortMAC_packed, 16);
-  uint8_t *msg_ptr = varShortMAC; 
+  uint8_t *msg_ptr = varShortMAC;
   liblte_rrc_pack_cell_identity_ie(0x1a2d0, &msg_ptr);
   liblte_rrc_pack_phys_cell_id_ie(cell.id, &msg_ptr);
-  mac_interface_rrc::ue_rnti_t ue_rnti; 
+  mac_interface_rrc::ue_rnti_t ue_rnti;
   mac->get_rntis(&ue_rnti);
   liblte_rrc_pack_c_rnti_ie(ue_rnti.crnti, &msg_ptr);
   srslte_bit_pack_vector(varShortMAC, varShortMAC_packed, msg_ptr - varShortMAC);
-  
+
   uint8_t mac_key[4];
   security_128_eia2(&k_rrc_int[16],
-                     1,
-                     1,
-                     1,
-                     varShortMAC_packed,
-                     7,
-                     mac_key);
-  
-  mac_interface_rrc::ue_rnti_t uernti; 
+                    1,
+                    1,
+                    1,
+                    varShortMAC_packed,
+                    7,
+                    mac_key);
+
+  mac_interface_rrc::ue_rnti_t uernti;
   mac->get_rntis(&uernti);
-  
+
   // Prepare ConnectionRestalishmentRequest packet
-  ul_ccch_msg.msg_type = LIBLTE_RRC_UL_CCCH_MSG_TYPE_RRC_CON_REEST_REQ;  
+  ul_ccch_msg.msg_type = LIBLTE_RRC_UL_CCCH_MSG_TYPE_RRC_CON_REEST_REQ;
   ul_ccch_msg.msg.rrc_con_reest_req.ue_id.c_rnti = uernti.crnti;
   ul_ccch_msg.msg.rrc_con_reest_req.ue_id.phys_cell_id = cell.id;
-  ul_ccch_msg.msg.rrc_con_reest_req.ue_id.short_mac_i = mac_key[2]<<8 | mac_key[3];
+  ul_ccch_msg.msg.rrc_con_reest_req.ue_id.short_mac_i = mac_key[2] << 8 | mac_key[3];
   ul_ccch_msg.msg.rrc_con_reest_req.cause = LIBLTE_RRC_CON_REEST_REQ_CAUSE_OTHER_FAILURE;
-  liblte_rrc_pack_ul_ccch_msg(&ul_ccch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
+  liblte_rrc_pack_ul_ccch_msg(&ul_ccch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
 
   rrc_log->info("Initiating RRC Connection Reestablishment Procedure\n");
   rrc_log->console("RRC Connection Reestablishment\n");
   mac_timers->get(t310)->stop();
   mac_timers->get(t311)->reset();
   mac_timers->get(t311)->run();
-  
+
   set_phy_default();
   mac->reset();
-  
+
   // FIXME: Cell selection should be different??
   phy->resync_sfn();
-  
-  // Wait for cell re-synchronization  
-  uint32_t timeout_cnt = 0; 
-  while(!phy->status_is_sync() && timeout_cnt < TIMEOUT_RESYNC_REESTABLISH){
+
+  // Wait for cell re-synchronization
+  uint32_t timeout_cnt = 0;
+  while (!phy->sync_status() && timeout_cnt < TIMEOUT_RESYNC_REESTABLISH) {
     usleep(10000);
-    timeout_cnt++; 
+    timeout_cnt++;
   }
   mac_timers->get(t301)->reset();
   mac_timers->get(t301)->run();
   mac_timers->get(t311)->stop();
   rrc_log->info("Cell Selection finished. Initiating transmission of RRC Connection Reestablishment Request\n");
-  
+
   // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
       bit_buf.msg[bit_buf.N_bits + i] = 0;
     bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
-  byte_buffer_t *pdcp_buf = pool_allocate;
+  byte_buffer_t *pdcp_buf = pool_allocate;;
   srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
-  pdcp_buf->N_bytes = bit_buf.N_bits/8;
+  pdcp_buf->N_bytes = bit_buf.N_bits / 8;
 
   // Set UE contention resolution ID in MAC
-  uint64_t uecri=0;
-  uint8_t *ue_cri_ptr = (uint8_t*) &uecri;
+  uint64_t uecri = 0;
+  uint8_t *ue_cri_ptr = (uint8_t *) &uecri;
   uint32_t nbytes = 6;
-  for (uint32_t i=0;i<nbytes;i++) {
-    ue_cri_ptr[nbytes-i-1] = pdcp_buf->msg[i];
+  for (uint32_t i = 0; i < nbytes; i++) {
+    ue_cri_ptr[nbytes - i - 1] = pdcp_buf->msg[i];
   }
   rrc_log->debug("Setting UE contention resolution ID: %d\n", uecri);
   mac->set_contention_id(uecri);
 
   rrc_log->info("Sending RRC Connection Resetablishment Request on SRB0\n");
-  state = RRC_STATE_WAIT_FOR_CON_SETUP;
   pdcp->write_sdu(RB_ID_SRB0, pdcp_buf);
 }
 
-
-void rrc::send_con_restablish_complete()
-{
+void rrc::send_con_restablish_complete() {
   rrc_log->debug("Preparing RRC Connection Reestablishment Complete\n");
   LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
 
   // Prepare ConnectionSetupComplete packet
   ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_RRC_CON_REEST_COMPLETE;
   ul_dcch_msg.msg.rrc_con_reest_complete.rrc_transaction_id = transaction_id;
-  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
+  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
 
   // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
       bit_buf.msg[bit_buf.N_bits + i] = 0;
     bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
-  byte_buffer_t *pdcp_buf = pool_allocate;
+  byte_buffer_t *pdcp_buf = pool_allocate;;
   srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
-  pdcp_buf->N_bytes = bit_buf.N_bits/8;
+  pdcp_buf->N_bytes = bit_buf.N_bits / 8;
 
-  state = RRC_STATE_RRC_CONNECTED;
+  state = RRC_STATE_CONNECTED;
   rrc_log->console("RRC Connected\n");
   rrc_log->info("Sending RRC Connection Reestablishment Complete\n");
   pdcp->write_sdu(RB_ID_SRB1, pdcp_buf);
 }
 
-void rrc::send_con_setup_complete(byte_buffer_t *nas_msg)
-{
+void rrc::send_con_setup_complete(byte_buffer_t *nas_msg) {
   rrc_log->debug("Preparing RRC Connection Setup Complete\n");
   LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
 
@@ -563,51 +787,48 @@ void rrc::send_con_setup_complete(byte_buffer_t *nas_msg)
   ul_dcch_msg.msg.rrc_con_setup_complete.selected_plmn_id = 1;
   memcpy(ul_dcch_msg.msg.rrc_con_setup_complete.dedicated_info_nas.msg, nas_msg->msg, nas_msg->N_bytes);
   ul_dcch_msg.msg.rrc_con_setup_complete.dedicated_info_nas.N_bytes = nas_msg->N_bytes;
-  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
+  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
 
   // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
       bit_buf.msg[bit_buf.N_bits + i] = 0;
     bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
-  byte_buffer_t *pdcp_buf = pool_allocate;
+  byte_buffer_t *pdcp_buf = pool_allocate;;
   srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
-  pdcp_buf->N_bytes = bit_buf.N_bits/8;
+  pdcp_buf->N_bytes = bit_buf.N_bits / 8;
   pdcp_buf->set_timestamp();
 
-  state = RRC_STATE_RRC_CONNECTED;
+  state = RRC_STATE_CONNECTED;
   rrc_log->console("RRC Connected\n");
   rrc_log->info("Sending RRC Connection Setup Complete\n");
   pdcp->write_sdu(RB_ID_SRB1, pdcp_buf);
 }
 
-void rrc::send_ul_info_transfer(uint32_t lcid, byte_buffer_t *sdu)
-{
+void rrc::send_ul_info_transfer(uint32_t lcid, byte_buffer_t *sdu) {
   rrc_log->debug("Preparing RX Info Transfer\n");
   LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
 
   // Prepare RX INFO packet
-  ul_dcch_msg.msg_type                                 = LIBLTE_RRC_UL_DCCH_MSG_TYPE_UL_INFO_TRANSFER;
+  ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_UL_INFO_TRANSFER;
   ul_dcch_msg.msg.ul_info_transfer.dedicated_info_type = LIBLTE_RRC_UL_INFORMATION_TRANSFER_TYPE_NAS;
   memcpy(ul_dcch_msg.msg.ul_info_transfer.dedicated_info.msg, sdu->msg, sdu->N_bytes);
   ul_dcch_msg.msg.ul_info_transfer.dedicated_info.N_bytes = sdu->N_bytes;
-  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
+  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
 
   // Reset and reuse sdu buffer
   byte_buffer_t *pdu = sdu;
   pdu->reset();
 
   // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
       bit_buf.msg[bit_buf.N_bits + i] = 0;
     bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
   srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
-  pdu->N_bytes = bit_buf.N_bits/8;
+  pdu->N_bytes = bit_buf.N_bits / 8;
   pdu->set_timestamp();
   pdu->set_timestamp();
 
@@ -615,62 +836,56 @@ void rrc::send_ul_info_transfer(uint32_t lcid, byte_buffer_t *sdu)
   pdcp->write_sdu(lcid, pdu);
 }
 
-void rrc::send_security_mode_complete(uint32_t lcid, byte_buffer_t *pdu)
-{
+void rrc::send_security_mode_complete(uint32_t lcid, byte_buffer_t *pdu) {
   rrc_log->debug("Preparing Security Mode Complete\n");
   LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
   ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_SECURITY_MODE_COMPLETE;
   ul_dcch_msg.msg.security_mode_complete.rrc_transaction_id = transaction_id;
-  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
+  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
 
   // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
       bit_buf.msg[bit_buf.N_bits + i] = 0;
     bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
   srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
-  pdu->N_bytes = bit_buf.N_bits/8;
+  pdu->N_bytes = bit_buf.N_bits / 8;
   pdu->set_timestamp();
 
   rrc_log->info("Sending Security Mode Complete\n");
   pdcp->write_sdu(lcid, pdu);
 }
 
-void rrc::send_rrc_con_reconfig_complete(uint32_t lcid, byte_buffer_t *pdu)
-{
+void rrc::send_rrc_con_reconfig_complete(uint32_t lcid, byte_buffer_t *pdu) {
   rrc_log->debug("Preparing RRC Connection Reconfig Complete\n");
   LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
 
   ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_RRC_CON_RECONFIG_COMPLETE;
   ul_dcch_msg.msg.rrc_con_reconfig_complete.rrc_transaction_id = transaction_id;
-  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
+  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
 
   // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
       bit_buf.msg[bit_buf.N_bits + i] = 0;
     bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
   srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
-  pdu->N_bytes = bit_buf.N_bits/8;
+  pdu->N_bytes = bit_buf.N_bits / 8;
   pdu->set_timestamp();
 
   rrc_log->info("Sending RRC Connection Reconfig Complete\n");
   pdcp->write_sdu(lcid, pdu);
 }
 
-void rrc::enable_capabilities()
-{
-  bool enable_ul_64 = ue_category>=5 && sib2.rr_config_common_sib.pusch_cnfg.enable_64_qam;
-  rrc_log->info("%s 64QAM PUSCH\n", enable_ul_64?"Enabling":"Disabling");
-  phy->set_config_64qam_en(enable_ul_64);  
+void rrc::enable_capabilities() {
+  bool enable_ul_64 = ue_category >= 5 && current_cell->sib2.rr_config_common_sib.pusch_cnfg.enable_64_qam;
+  rrc_log->info("%s 64QAM PUSCH\n", enable_ul_64 ? "Enabling" : "Disabling");
+  phy->set_config_64qam_en(enable_ul_64);
 }
 
-void rrc::send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu)
-{
+void rrc::send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu) {
   rrc_log->debug("Preparing UE Capability Info\n");
   LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
 
@@ -722,27 +937,26 @@ void rrc::send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu)
   cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[1] = true;
   cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[2] = true;
 
-  cap->feature_group_indicator_present         = true;
-  cap->feature_group_indicator                 = 0x62001000;
-  cap->inter_rat_params.utra_fdd_present       = false;
-  cap->inter_rat_params.utra_tdd128_present    = false;
-  cap->inter_rat_params.utra_tdd384_present    = false;
-  cap->inter_rat_params.utra_tdd768_present    = false;
-  cap->inter_rat_params.geran_present          = false;
-  cap->inter_rat_params.cdma2000_hrpd_present  = false;
+  cap->feature_group_indicator_present = true;
+  cap->feature_group_indicator = 0x62001000;
+  cap->inter_rat_params.utra_fdd_present = false;
+  cap->inter_rat_params.utra_tdd128_present = false;
+  cap->inter_rat_params.utra_tdd384_present = false;
+  cap->inter_rat_params.utra_tdd768_present = false;
+  cap->inter_rat_params.geran_present = false;
+  cap->inter_rat_params.cdma2000_hrpd_present = false;
   cap->inter_rat_params.cdma2000_1xrtt_present = false;
 
-  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
+  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
 
   // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
       bit_buf.msg[bit_buf.N_bits + i] = 0;
     bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
   srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
-  pdu->N_bytes = bit_buf.N_bits/8;
+  pdu->N_bytes = bit_buf.N_bits / 8;
   pdu->set_timestamp();
 
   rrc_log->info("Sending UE Capability Info\n");
@@ -750,112 +964,104 @@ void rrc::send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu)
 }
 
 /*******************************************************************************
-  Parsers
+Parsers
 *******************************************************************************/
 
-void rrc::parse_dl_ccch(byte_buffer_t *pdu)
-{
-  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes*8);
-  bit_buf.N_bits = pdu->N_bytes*8;
+void rrc::parse_dl_ccch(byte_buffer_t *pdu) {
+  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
+  bit_buf.N_bits = pdu->N_bytes * 8;
   pool->deallocate(pdu);
   bzero(&dl_ccch_msg, sizeof(LIBLTE_RRC_DL_CCCH_MSG_STRUCT));
-  liblte_rrc_unpack_dl_ccch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &dl_ccch_msg);
+  liblte_rrc_unpack_dl_ccch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &dl_ccch_msg);
 
   rrc_log->info("SRB0 - Received %s\n",
                 liblte_rrc_dl_ccch_msg_type_text[dl_ccch_msg.msg_type]);
 
-  switch(dl_ccch_msg.msg_type)
-  {
-  case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REJ:
-    rrc_log->info("Connection Reject received. Wait time: %d\n",
-                  dl_ccch_msg.msg.rrc_con_rej.wait_time);
-    state = RRC_STATE_IDLE;
-    break;
-  case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_SETUP:
-    rrc_log->info("Connection Setup received\n");
-    transaction_id = dl_ccch_msg.msg.rrc_con_setup.rrc_transaction_id;
-    handle_con_setup(&dl_ccch_msg.msg.rrc_con_setup);
-    rrc_log->info("Notifying NAS of connection setup\n");
-    state = RRC_STATE_COMPLETING_SETUP;
-    nas->notify_connection_setup();
-    break;
-  case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST:
-    rrc_log->info("Connection Reestablishment received\n");
-    rrc_log->console("Reestablishment OK\n");
-    transaction_id = dl_ccch_msg.msg.rrc_con_reest.rrc_transaction_id;
-    handle_con_reest(&dl_ccch_msg.msg.rrc_con_reest);
-    break;
-  case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST_REJ:
-    rrc_log->info("Connection Reestablishment Reject received\n");
-    rrc_log->console("Reestablishment Reject\n");
-    usleep(50000);
-    rrc_connection_release();
-    break;
-  default:
-    break;
+  switch (dl_ccch_msg.msg_type) {
+    case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REJ:
+      rrc_log->info("Connection Reject received. Wait time: %d\n",
+                    dl_ccch_msg.msg.rrc_con_rej.wait_time);
+      state = RRC_STATE_IDLE;
+      break;
+    case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_SETUP:
+      rrc_log->info("Connection Setup received\n");
+      transaction_id = dl_ccch_msg.msg.rrc_con_setup.rrc_transaction_id;
+      handle_con_setup(&dl_ccch_msg.msg.rrc_con_setup);
+      rrc_log->info("Notifying NAS of connection setup\n");
+      nas->notify_connection_setup();
+      break;
+    case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST:
+      rrc_log->info("Connection Reestablishment received\n");
+      rrc_log->console("Reestablishment OK\n");
+      transaction_id = dl_ccch_msg.msg.rrc_con_reest.rrc_transaction_id;
+      handle_con_reest(&dl_ccch_msg.msg.rrc_con_reest);
+      break;
+    case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST_REJ:
+      rrc_log->info("Connection Reestablishment Reject received\n");
+      rrc_log->console("Reestablishment Reject\n");
+      usleep(50000);
+      rrc_connection_release();
+      break;
+    default:
+      break;
   }
 }
 
-void rrc::parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu)
-{
-  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes*8);
-  bit_buf.N_bits = pdu->N_bytes*8;
-  liblte_rrc_unpack_dl_dcch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &dl_dcch_msg);
+void rrc::parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu) {
+  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
+  bit_buf.N_bits = pdu->N_bytes * 8;
+  liblte_rrc_unpack_dl_dcch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &dl_dcch_msg);
 
   rrc_log->info("%s - Received %s\n",
-                rb_id_str[lcid].c_str(),
+                get_rb_name(lcid).c_str(),
                 liblte_rrc_dl_dcch_msg_type_text[dl_dcch_msg.msg_type]);
 
   // Reset and reuse pdu buffer if possible
   pdu->reset();
 
-  switch(dl_dcch_msg.msg_type)
-  {
-  case LIBLTE_RRC_DL_DCCH_MSG_TYPE_DL_INFO_TRANSFER:
-    memcpy(pdu->msg, dl_dcch_msg.msg.dl_info_transfer.dedicated_info.msg, dl_dcch_msg.msg.dl_info_transfer.dedicated_info.N_bytes);
-    pdu->N_bytes = dl_dcch_msg.msg.dl_info_transfer.dedicated_info.N_bytes;
-    nas->write_pdu(lcid, pdu);
-    break;
-  case LIBLTE_RRC_DL_DCCH_MSG_TYPE_SECURITY_MODE_COMMAND:
-    transaction_id =  dl_dcch_msg.msg.security_mode_cmd.rrc_transaction_id;
+  switch (dl_dcch_msg.msg_type) {
+    case LIBLTE_RRC_DL_DCCH_MSG_TYPE_DL_INFO_TRANSFER:
+      memcpy(pdu->msg, dl_dcch_msg.msg.dl_info_transfer.dedicated_info.msg,
+             dl_dcch_msg.msg.dl_info_transfer.dedicated_info.N_bytes);
+      pdu->N_bytes = dl_dcch_msg.msg.dl_info_transfer.dedicated_info.N_bytes;
+      nas->write_pdu(lcid, pdu);
+      break;
+    case LIBLTE_RRC_DL_DCCH_MSG_TYPE_SECURITY_MODE_COMMAND:
+      transaction_id = dl_dcch_msg.msg.security_mode_cmd.rrc_transaction_id;
 
-    cipher_algo = (CIPHERING_ALGORITHM_ID_ENUM)dl_dcch_msg.msg.security_mode_cmd.sec_algs.cipher_alg;
-    integ_algo  = (INTEGRITY_ALGORITHM_ID_ENUM)dl_dcch_msg.msg.security_mode_cmd.sec_algs.int_alg;
+      cipher_algo = (CIPHERING_ALGORITHM_ID_ENUM) dl_dcch_msg.msg.security_mode_cmd.sec_algs.cipher_alg;
+      integ_algo = (INTEGRITY_ALGORITHM_ID_ENUM) dl_dcch_msg.msg.security_mode_cmd.sec_algs.int_alg;
 
-    // Configure PDCP for security
-    usim->generate_as_keys(nas->get_ul_count(), k_rrc_enc, k_rrc_int, k_up_enc, k_up_int, cipher_algo, integ_algo);
-    pdcp->config_security(lcid, k_rrc_enc, k_rrc_int, cipher_algo, integ_algo);
-    send_security_mode_complete(lcid, pdu);
-    break;
-  case LIBLTE_RRC_DL_DCCH_MSG_TYPE_RRC_CON_RECONFIG:
-    transaction_id = dl_dcch_msg.msg.rrc_con_reconfig.rrc_transaction_id;
-    handle_rrc_con_reconfig(lcid, &dl_dcch_msg.msg.rrc_con_reconfig, pdu);
-    break;
-  case LIBLTE_RRC_DL_DCCH_MSG_TYPE_UE_CAPABILITY_ENQUIRY:
-    transaction_id = dl_dcch_msg.msg.ue_cap_enquiry.rrc_transaction_id;
-    for(uint32_t i=0; i<dl_dcch_msg.msg.ue_cap_enquiry.N_ue_cap_reqs; i++)
-    {
-      if(LIBLTE_RRC_RAT_TYPE_EUTRA == dl_dcch_msg.msg.ue_cap_enquiry.ue_capability_request[i])
-      {
-        send_rrc_ue_cap_info(lcid, pdu);
-        break;
+      // Configure PDCP for security
+      usim->generate_as_keys(nas->get_ul_count(), k_rrc_enc, k_rrc_int, k_up_enc, k_up_int, cipher_algo, integ_algo);
+      pdcp->config_security(lcid, k_rrc_enc, k_rrc_int, cipher_algo, integ_algo);
+      send_security_mode_complete(lcid, pdu);
+      break;
+    case LIBLTE_RRC_DL_DCCH_MSG_TYPE_RRC_CON_RECONFIG:
+      transaction_id = dl_dcch_msg.msg.rrc_con_reconfig.rrc_transaction_id;
+      handle_rrc_con_reconfig(lcid, &dl_dcch_msg.msg.rrc_con_reconfig, pdu);
+      break;
+    case LIBLTE_RRC_DL_DCCH_MSG_TYPE_UE_CAPABILITY_ENQUIRY:
+      transaction_id = dl_dcch_msg.msg.ue_cap_enquiry.rrc_transaction_id;
+      for (uint32_t i = 0; i < dl_dcch_msg.msg.ue_cap_enquiry.N_ue_cap_reqs; i++) {
+        if (LIBLTE_RRC_RAT_TYPE_EUTRA == dl_dcch_msg.msg.ue_cap_enquiry.ue_capability_request[i]) {
+          send_rrc_ue_cap_info(lcid, pdu);
+          break;
+        }
       }
-    }
-    break;
-  case LIBLTE_RRC_DL_DCCH_MSG_TYPE_RRC_CON_RELEASE:
-    rrc_connection_release();
-    break;
-  default:
-    break;
+      break;
+    case LIBLTE_RRC_DL_DCCH_MSG_TYPE_RRC_CON_RELEASE:
+      rrc_connection_release();
+      break;
+    default:
+      break;
   }
 }
 
-
 /*******************************************************************************
-  Timer expiration callback 
+Timer expiration callback
 *******************************************************************************/
-void rrc::timer_expired(uint32_t timeout_id)
-{
+void rrc::timer_expired(uint32_t timeout_id) {
   if (timeout_id == t310) {
     rrc_log->info("Timer T310 expired: Radio Link Failure\n");
     radio_link_failure();
@@ -873,24 +1079,25 @@ void rrc::timer_expired(uint32_t timeout_id)
 }
 
 /*******************************************************************************
-  Helpers
+Helpers
 *******************************************************************************/
 
 void rrc::reset_ue() {
   phy->reset();
   mac->reset();
-  pdcp->reset();  
+  pdcp->reset();
   rlc->reset();
   mac->pcch_start_rx();
   mac_timers->get(safe_reset_timer)->stop();
   mac_timers->get(safe_reset_timer)->reset();
-  rrc_log->console("RRC Connection released.\n");
+  rrc_log->console("RRC Connection Released.\n");
 }
 
 void rrc::rrc_connection_release() {
   pthread_mutex_lock(&mutex);
   drb_up = false;
-  state  = RRC_STATE_IDLE;
+  state = RRC_STATE_IDLE;
+  phy->sync_stop();
   set_phy_default();
   set_mac_default();
   mac_timers->get(t311)->run();
@@ -902,191 +1109,129 @@ void rrc::rrc_connection_release() {
   pthread_mutex_unlock(&mutex);
 }
 
-void rrc::test_con_restablishment()
-{
-  printf("Testing connection Reestablishment\n"); 
+void rrc::test_con_restablishment() {
+  printf("Testing connection Reestablishment\n");
   send_con_restablish_request();
 }
 
 /* Detection of radio link failure (5.3.11.3) */
 void rrc::radio_link_failure() {
-  // TODO: Generate and store failure report 
-  
+  // TODO: Generate and store failure report
+
   rrc_log->warning("Detected Radio-Link Failure\n");
   rrc_log->console("Warning: Detected Radio-Link Failure\n");
-  if (state != RRC_STATE_RRC_CONNECTED) {
+  if (state != RRC_STATE_CONNECTED) {
     rrc_connection_release();
-  } else {    
+  } else {
     send_con_restablish_request();
   }
 }
 
-void* rrc::start_sib_thread(void *rrc_)
-{
-  rrc *r = (rrc*)rrc_;
-  r->sib_search();
-  return NULL;
-}
-
-void rrc::sib_search()
-{
-  bool      searching = true;
-  uint32_t  tti ;
-  uint32_t  si_win_start, si_win_len;
-  uint16_t  period;
-  uint32_t  nof_sib1_trials = 0; 
-  const int SIB1_SEARCH_TIMEOUT = 30; 
-
-  while(searching)
-  {
-    switch(state)
-    {
-    case RRC_STATE_SIB1_SEARCH:
-      // Instruct MAC to look for SIB1
-      while(!phy->status_is_sync()){
-        usleep(50000);
-      }
-      usleep(10000); 
-      tti          = mac->get_current_tti();
-      si_win_start = sib_start_tti(tti, 2, 5);
-      mac->bcch_start_rx(si_win_start, 1);
-      rrc_log->debug("Instructed MAC to search for SIB1, win_start=%d, win_len=%d\n",
-                     si_win_start, 1);
-      nof_sib1_trials++;
-      if (nof_sib1_trials >= SIB1_SEARCH_TIMEOUT) {
-        rrc_log->info("Timeout while searching for SIB1. Resynchronizing SFN...\n");
-        rrc_log->console("Timeout while searching for SIB1. Resynchronizing SFN...\n");
-        phy->resync_sfn();
-        nof_sib1_trials = 0; 
-      }
-      break;
-    case RRC_STATE_SIB2_SEARCH:
-      // Instruct MAC to look for SIB2
-      usleep(10000);
-      tti          = mac->get_current_tti();
-      period       = liblte_rrc_si_periodicity_num[sib1.sched_info[0].si_periodicity];
-      si_win_start = sib_start_tti(tti, period, 0);
-      si_win_len   = liblte_rrc_si_window_length_num[sib1.si_window_length];
-
-      mac->bcch_start_rx(si_win_start, si_win_len);
-      rrc_log->debug("Instructed MAC to search for SIB2, win_start=%d, win_len=%d\n",
-                     si_win_start, si_win_len);
-
-      break;
-    default:
-      searching = false;
-      break;
-    }
-    usleep(100000);
-  }
-}
 
 // Determine SI messages scheduling as in 36.331 5.2.3 Acquisition of an SI message
 uint32_t rrc::sib_start_tti(uint32_t tti, uint32_t period, uint32_t x) {
-  return (period*10*(1+tti/(period*10))+x)%10240; // the 1 means next opportunity
+  return (period * 10 * (1 + tti / (period * 10)) + x) % 10240; // the 1 means next opportunity
 }
 
-void rrc::apply_sib2_configs()
-{
-  if(RRC_STATE_WAIT_FOR_CON_SETUP != state){
-    rrc_log->error("State must be RRC_STATE_WAIT_FOR_CON_SETUP to handle SIB2. Actual state: %s\n",
-                   rrc_state_text[state]);
-    return;
-  }
-  
-  // Apply RACH timeAlginmentTimer configuration 
-  mac_interface_rrc::mac_cfg_t cfg; 
+void rrc::apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2) {
+
+  // Apply RACH timeAlginmentTimer configuration
+  mac_interface_rrc::mac_cfg_t cfg;
   mac->get_config(&cfg);
-  cfg.main.time_alignment_timer = sib2.time_alignment_timer; 
-  memcpy(&cfg.rach, &sib2.rr_config_common_sib.rach_cnfg, sizeof(LIBLTE_RRC_RACH_CONFIG_COMMON_STRUCT)); 
-  cfg.prach_config_index = sib2.rr_config_common_sib.prach_cnfg.root_sequence_index;
+
+  cfg.main.time_alignment_timer = sib2->time_alignment_timer;
+  memcpy(&cfg.rach, &sib2->rr_config_common_sib.rach_cnfg, sizeof(LIBLTE_RRC_RACH_CONFIG_COMMON_STRUCT));
+  cfg.prach_config_index = sib2->rr_config_common_sib.prach_cnfg.root_sequence_index;
   cfg.ul_harq_params.max_harq_msg3_tx = cfg.rach.max_harq_msg3_tx;
+
   mac->set_config(&cfg);
-  
+
   rrc_log->info("Set RACH ConfigCommon: NofPreambles=%d, ResponseWindow=%d, ContentionResolutionTimer=%d ms\n",
-         liblte_rrc_number_of_ra_preambles_num[sib2.rr_config_common_sib.rach_cnfg.num_ra_preambles],
-         liblte_rrc_ra_response_window_size_num[sib2.rr_config_common_sib.rach_cnfg.ra_resp_win_size],
-         liblte_rrc_mac_contention_resolution_timer_num[sib2.rr_config_common_sib.rach_cnfg.mac_con_res_timer]);
+                liblte_rrc_number_of_ra_preambles_num[sib2->rr_config_common_sib.rach_cnfg.num_ra_preambles],
+                liblte_rrc_ra_response_window_size_num[sib2->rr_config_common_sib.rach_cnfg.ra_resp_win_size],
+                liblte_rrc_mac_contention_resolution_timer_num[sib2->rr_config_common_sib.rach_cnfg.mac_con_res_timer]);
 
   // Apply PHY RR Config Common
-  phy_interface_rrc::phy_cfg_common_t common; 
-  memcpy(&common.pdsch_cnfg,  &sib2.rr_config_common_sib.pdsch_cnfg,  sizeof(LIBLTE_RRC_PDSCH_CONFIG_COMMON_STRUCT));
-  memcpy(&common.pusch_cnfg,  &sib2.rr_config_common_sib.pusch_cnfg,  sizeof(LIBLTE_RRC_PUSCH_CONFIG_COMMON_STRUCT));
-  memcpy(&common.pucch_cnfg,  &sib2.rr_config_common_sib.pucch_cnfg,  sizeof(LIBLTE_RRC_PUCCH_CONFIG_COMMON_STRUCT));
-  memcpy(&common.ul_pwr_ctrl, &sib2.rr_config_common_sib.ul_pwr_ctrl, sizeof(LIBLTE_RRC_UL_POWER_CONTROL_COMMON_STRUCT));
-  memcpy(&common.prach_cnfg,  &sib2.rr_config_common_sib.prach_cnfg,  sizeof(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT));
-  if (sib2.rr_config_common_sib.srs_ul_cnfg.present) {
-    memcpy(&common.srs_ul_cnfg,  &sib2.rr_config_common_sib.srs_ul_cnfg, sizeof(LIBLTE_RRC_SRS_UL_CONFIG_COMMON_STRUCT));
+  phy_interface_rrc::phy_cfg_common_t common;
+  memcpy(&common.pdsch_cnfg, &sib2->rr_config_common_sib.pdsch_cnfg, sizeof(LIBLTE_RRC_PDSCH_CONFIG_COMMON_STRUCT));
+  memcpy(&common.pusch_cnfg, &sib2->rr_config_common_sib.pusch_cnfg, sizeof(LIBLTE_RRC_PUSCH_CONFIG_COMMON_STRUCT));
+  memcpy(&common.pucch_cnfg, &sib2->rr_config_common_sib.pucch_cnfg, sizeof(LIBLTE_RRC_PUCCH_CONFIG_COMMON_STRUCT));
+  memcpy(&common.ul_pwr_ctrl, &sib2->rr_config_common_sib.ul_pwr_ctrl,
+         sizeof(LIBLTE_RRC_UL_POWER_CONTROL_COMMON_STRUCT));
+  memcpy(&common.prach_cnfg, &sib2->rr_config_common_sib.prach_cnfg, sizeof(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT));
+  if (sib2->rr_config_common_sib.srs_ul_cnfg.present) {
+    memcpy(&common.srs_ul_cnfg, &sib2->rr_config_common_sib.srs_ul_cnfg,
+           sizeof(LIBLTE_RRC_SRS_UL_CONFIG_COMMON_STRUCT));
   } else {
     // default is release
-    common.srs_ul_cnfg.present = false; 
+    common.srs_ul_cnfg.present = false;
   }
   phy->set_config_common(&common);
 
   phy->configure_ul_params();
 
   rrc_log->info("Set PUSCH ConfigCommon: HopOffset=%d, RSGroup=%d, RSNcs=%d, N_sb=%d\n",
-                sib2.rr_config_common_sib.pusch_cnfg.pusch_hopping_offset,
-                sib2.rr_config_common_sib.pusch_cnfg.ul_rs.group_assignment_pusch,
-                sib2.rr_config_common_sib.pusch_cnfg.ul_rs.cyclic_shift,
-                sib2.rr_config_common_sib.pusch_cnfg.n_sb);
+                sib2->rr_config_common_sib.pusch_cnfg.pusch_hopping_offset,
+                sib2->rr_config_common_sib.pusch_cnfg.ul_rs.group_assignment_pusch,
+                sib2->rr_config_common_sib.pusch_cnfg.ul_rs.cyclic_shift,
+                sib2->rr_config_common_sib.pusch_cnfg.n_sb);
 
   rrc_log->info("Set PUCCH ConfigCommon: DeltaShift=%d, CyclicShift=%d, N1=%d, NRB=%d\n",
-                liblte_rrc_delta_pucch_shift_num[sib2.rr_config_common_sib.pucch_cnfg.delta_pucch_shift],
-                sib2.rr_config_common_sib.pucch_cnfg.n_cs_an,
-                sib2.rr_config_common_sib.pucch_cnfg.n1_pucch_an,
-                sib2.rr_config_common_sib.pucch_cnfg.n_rb_cqi);
-  
+                liblte_rrc_delta_pucch_shift_num[sib2->rr_config_common_sib.pucch_cnfg.delta_pucch_shift],
+                sib2->rr_config_common_sib.pucch_cnfg.n_cs_an,
+                sib2->rr_config_common_sib.pucch_cnfg.n1_pucch_an,
+                sib2->rr_config_common_sib.pucch_cnfg.n_rb_cqi);
+
   rrc_log->info("Set PRACH ConfigCommon: SeqIdx=%d, HS=%s, FreqOffset=%d, ZC=%d, ConfigIndex=%d\n",
-                 sib2.rr_config_common_sib.prach_cnfg.root_sequence_index,
-                 sib2.rr_config_common_sib.prach_cnfg.prach_cnfg_info.high_speed_flag?"yes":"no",
-                 sib2.rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_freq_offset,
-                 sib2.rr_config_common_sib.prach_cnfg.prach_cnfg_info.zero_correlation_zone_config,
-                 sib2.rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_config_index);
+                sib2->rr_config_common_sib.prach_cnfg.root_sequence_index,
+                sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.high_speed_flag ? "yes" : "no",
+                sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_freq_offset,
+                sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.zero_correlation_zone_config,
+                sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_config_index);
 
   rrc_log->info("Set SRS ConfigCommon: BW-Configuration=%d, SF-Configuration=%d, ACKNACK=%s\n",
-                 liblte_rrc_srs_bw_config_num[sib2.rr_config_common_sib.srs_ul_cnfg.bw_cnfg],
-                 liblte_rrc_srs_subfr_config_num[sib2.rr_config_common_sib.srs_ul_cnfg.subfr_cnfg],
-                 sib2.rr_config_common_sib.srs_ul_cnfg.ack_nack_simul_tx?"yes":"no");
+                liblte_rrc_srs_bw_config_num[sib2->rr_config_common_sib.srs_ul_cnfg.bw_cnfg],
+                liblte_rrc_srs_subfr_config_num[sib2->rr_config_common_sib.srs_ul_cnfg.subfr_cnfg],
+                sib2->rr_config_common_sib.srs_ul_cnfg.ack_nack_simul_tx ? "yes" : "no");
+
+  mac_timers->get(t301)->set(this, liblte_rrc_t301_num[sib2->ue_timers_and_constants.t301]);
+  mac_timers->get(t310)->set(this, liblte_rrc_t310_num[sib2->ue_timers_and_constants.t310]);
+  mac_timers->get(t311)->set(this, liblte_rrc_t311_num[sib2->ue_timers_and_constants.t311]);
+  N310 = liblte_rrc_n310_num[sib2->ue_timers_and_constants.n310];
+  N311 = liblte_rrc_n311_num[sib2->ue_timers_and_constants.n311];
+
+  rrc_log->info("Set Constants and Timers: N310=%d, N311=%d, t301=%d, t310=%d, t311=%d\n",
+                N310, N311, mac_timers->get(t301)->get_timeout(),
+                mac_timers->get(t310)->get_timeout(), mac_timers->get(t311)->get_timeout());
 
-  mac_timers->get(t301)->set(this, liblte_rrc_t301_num[sib2.ue_timers_and_constants.t301]);
-  mac_timers->get(t310)->set(this, liblte_rrc_t310_num[sib2.ue_timers_and_constants.t310]);
-  mac_timers->get(t311)->set(this, liblte_rrc_t311_num[sib2.ue_timers_and_constants.t311]);
-  N310 = liblte_rrc_n310_num[sib2.ue_timers_and_constants.n310];
-  N311 = liblte_rrc_n311_num[sib2.ue_timers_and_constants.n311];
-  
-  rrc_log->info("Set Constants and Timers: N310=%d, N311=%d, t301=%d, t310=%d, t311=%d\n", 
-    N310, N311, mac_timers->get(t301)->get_timeout(), 
-    mac_timers->get(t310)->get_timeout(), mac_timers->get(t311)->get_timeout());
-  
 }
 
-  // Go through all information elements and apply defaults (9.2.4) if not defined
-void rrc::apply_phy_config_dedicated(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *phy_cnfg, bool apply_defaults) 
-{
-  // Get current configuration 
-  LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *current_cfg; 
-  phy_interface_rrc::phy_cfg_t c; 
+// Go through all information elements and apply defaults (9.2.4) if not defined
+void rrc::apply_phy_config_dedicated(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *phy_cnfg, bool apply_defaults) {
+  // Get current configuration
+  LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *current_cfg;
+  phy_interface_rrc::phy_cfg_t c;
   phy->get_config(&c);
   current_cfg = &c.dedicated;
-    
-  if(phy_cnfg->pucch_cnfg_ded_present) {
+
+  if (phy_cnfg->pucch_cnfg_ded_present) {
     memcpy(&current_cfg->pucch_cnfg_ded, &phy_cnfg->pucch_cnfg_ded, sizeof(LIBLTE_RRC_PUCCH_CONFIG_DEDICATED_STRUCT));
   } else if (apply_defaults) {
-    current_cfg->pucch_cnfg_ded.tdd_ack_nack_feedback_mode = LIBLTE_RRC_TDD_ACK_NACK_FEEDBACK_MODE_BUNDLING; 
-    current_cfg->pucch_cnfg_ded.ack_nack_repetition_setup_present = false; 
+    current_cfg->pucch_cnfg_ded.tdd_ack_nack_feedback_mode = LIBLTE_RRC_TDD_ACK_NACK_FEEDBACK_MODE_BUNDLING;
+    current_cfg->pucch_cnfg_ded.ack_nack_repetition_setup_present = false;
   }
-  if(phy_cnfg->pusch_cnfg_ded_present) {
-    memcpy(&current_cfg->pusch_cnfg_ded, &phy_cnfg->pusch_cnfg_ded, sizeof(LIBLTE_RRC_PUSCH_CONFIG_DEDICATED_STRUCT)); 
+  if (phy_cnfg->pusch_cnfg_ded_present) {
+    memcpy(&current_cfg->pusch_cnfg_ded, &phy_cnfg->pusch_cnfg_ded, sizeof(LIBLTE_RRC_PUSCH_CONFIG_DEDICATED_STRUCT));
   } else if (apply_defaults) {
-    current_cfg->pusch_cnfg_ded.beta_offset_ack_idx = 10; 
-    current_cfg->pusch_cnfg_ded.beta_offset_ri_idx  = 12;
+    current_cfg->pusch_cnfg_ded.beta_offset_ack_idx = 10;
+    current_cfg->pusch_cnfg_ded.beta_offset_ri_idx = 12;
     current_cfg->pusch_cnfg_ded.beta_offset_cqi_idx = 15;
   }
-  if(phy_cnfg->ul_pwr_ctrl_ded_present) {
-    memcpy(&current_cfg->ul_pwr_ctrl_ded, &phy_cnfg->ul_pwr_ctrl_ded, sizeof(LIBLTE_RRC_UL_POWER_CONTROL_DEDICATED_STRUCT)); 
+  if (phy_cnfg->ul_pwr_ctrl_ded_present) {
+    memcpy(&current_cfg->ul_pwr_ctrl_ded, &phy_cnfg->ul_pwr_ctrl_ded,
+           sizeof(LIBLTE_RRC_UL_POWER_CONTROL_DEDICATED_STRUCT));
   } else if (apply_defaults) {
-    current_cfg->ul_pwr_ctrl_ded.p0_ue_pusch     = 0; 
+    current_cfg->ul_pwr_ctrl_ded.p0_ue_pusch     = 0;
     current_cfg->ul_pwr_ctrl_ded.delta_mcs_en    = LIBLTE_RRC_DELTA_MCS_ENABLED_EN0; 
     current_cfg->ul_pwr_ctrl_ded.accumulation_en = true; 
     current_cfg->ul_pwr_ctrl_ded.p0_ue_pucch     = 0;
@@ -1097,140 +1242,144 @@ void rrc::apply_phy_config_dedicated(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT
   } else {
     current_cfg->ul_pwr_ctrl_ded.filter_coeff = LIBLTE_RRC_FILTER_COEFFICIENT_FC4;
   }
-  if(phy_cnfg->tpc_pdcch_cnfg_pucch_present) {
-    memcpy(&current_cfg->tpc_pdcch_cnfg_pucch, &phy_cnfg->tpc_pdcch_cnfg_pucch, sizeof(LIBLTE_RRC_TPC_PDCCH_CONFIG_STRUCT)); 
+  if (phy_cnfg->tpc_pdcch_cnfg_pucch_present) {
+    memcpy(&current_cfg->tpc_pdcch_cnfg_pucch, &phy_cnfg->tpc_pdcch_cnfg_pucch,
+           sizeof(LIBLTE_RRC_TPC_PDCCH_CONFIG_STRUCT));
   } else if (apply_defaults) {
-    current_cfg->tpc_pdcch_cnfg_pucch.setup_present = false; 
+    current_cfg->tpc_pdcch_cnfg_pucch.setup_present = false;
   }
-  if(phy_cnfg->tpc_pdcch_cnfg_pusch_present) {
-    memcpy(&current_cfg->tpc_pdcch_cnfg_pusch, &phy_cnfg->tpc_pdcch_cnfg_pusch, sizeof(LIBLTE_RRC_TPC_PDCCH_CONFIG_STRUCT)); 
+  if (phy_cnfg->tpc_pdcch_cnfg_pusch_present) {
+    memcpy(&current_cfg->tpc_pdcch_cnfg_pusch, &phy_cnfg->tpc_pdcch_cnfg_pusch,
+           sizeof(LIBLTE_RRC_TPC_PDCCH_CONFIG_STRUCT));
   } else {
-    current_cfg->tpc_pdcch_cnfg_pusch.setup_present = false; 
+    current_cfg->tpc_pdcch_cnfg_pusch.setup_present = false;
   }
-  if(phy_cnfg->cqi_report_cnfg_present) {
+  if (phy_cnfg->cqi_report_cnfg_present) {
     if (phy_cnfg->cqi_report_cnfg.report_periodic_present) {
-      memcpy(&current_cfg->cqi_report_cnfg.report_periodic, &phy_cnfg->cqi_report_cnfg.report_periodic, sizeof(LIBLTE_RRC_CQI_REPORT_PERIODIC_STRUCT));
-      current_cfg->cqi_report_cnfg.report_periodic_setup_present = phy_cnfg->cqi_report_cnfg.report_periodic_setup_present; 
+      memcpy(&current_cfg->cqi_report_cnfg.report_periodic, &phy_cnfg->cqi_report_cnfg.report_periodic,
+             sizeof(LIBLTE_RRC_CQI_REPORT_PERIODIC_STRUCT));
+      current_cfg->cqi_report_cnfg.report_periodic_setup_present = phy_cnfg->cqi_report_cnfg.report_periodic_setup_present;
     } else if (apply_defaults) {
-      current_cfg->cqi_report_cnfg.report_periodic_setup_present = false; 
+      current_cfg->cqi_report_cnfg.report_periodic_setup_present = false;
     }
     if (phy_cnfg->cqi_report_cnfg.report_mode_aperiodic_present) {
-      current_cfg->cqi_report_cnfg.report_mode_aperiodic         = phy_cnfg->cqi_report_cnfg.report_mode_aperiodic;
+      current_cfg->cqi_report_cnfg.report_mode_aperiodic = phy_cnfg->cqi_report_cnfg.report_mode_aperiodic;
       current_cfg->cqi_report_cnfg.report_mode_aperiodic_present = phy_cnfg->cqi_report_cnfg.report_mode_aperiodic_present;
     } else if (apply_defaults) {
-      current_cfg->cqi_report_cnfg.report_mode_aperiodic_present = false; 
+      current_cfg->cqi_report_cnfg.report_mode_aperiodic_present = false;
     }
     current_cfg->cqi_report_cnfg.nom_pdsch_rs_epre_offset = phy_cnfg->cqi_report_cnfg.nom_pdsch_rs_epre_offset;
   }
-  if(phy_cnfg->srs_ul_cnfg_ded_present && phy_cnfg->srs_ul_cnfg_ded.setup_present) {
-    memcpy(&current_cfg->srs_ul_cnfg_ded, &phy_cnfg->srs_ul_cnfg_ded, sizeof(LIBLTE_RRC_SRS_UL_CONFIG_DEDICATED_STRUCT)); 
+  if (phy_cnfg->srs_ul_cnfg_ded_present && phy_cnfg->srs_ul_cnfg_ded.setup_present) {
+    memcpy(&current_cfg->srs_ul_cnfg_ded, &phy_cnfg->srs_ul_cnfg_ded,
+           sizeof(LIBLTE_RRC_SRS_UL_CONFIG_DEDICATED_STRUCT));
   } else if (apply_defaults) {
-    current_cfg->srs_ul_cnfg_ded.setup_present = false; 
+    current_cfg->srs_ul_cnfg_ded.setup_present = false;
   }
-  if(phy_cnfg->antenna_info_present) {
+  if (phy_cnfg->antenna_info_present) {
     if (!phy_cnfg->antenna_info_default_value) {
-      if(phy_cnfg->antenna_info_explicit_value.tx_mode != LIBLTE_RRC_TRANSMISSION_MODE_1 &&
-         phy_cnfg->antenna_info_explicit_value.tx_mode != LIBLTE_RRC_TRANSMISSION_MODE_2) {
-        rrc_log->error("Transmission mode TM%s not currently supported by srsUE\n", liblte_rrc_transmission_mode_text[phy_cnfg->antenna_info_explicit_value.tx_mode]);
+      if (phy_cnfg->antenna_info_explicit_value.tx_mode != LIBLTE_RRC_TRANSMISSION_MODE_1 &&
+          phy_cnfg->antenna_info_explicit_value.tx_mode != LIBLTE_RRC_TRANSMISSION_MODE_2) {
+        rrc_log->error("Transmission mode TM%s not currently supported by srsUE\n",
+                       liblte_rrc_transmission_mode_text[phy_cnfg->antenna_info_explicit_value.tx_mode]);
       }
-      memcpy(&current_cfg->antenna_info_explicit_value, &phy_cnfg->antenna_info_explicit_value, sizeof(LIBLTE_RRC_ANTENNA_INFO_DEDICATED_STRUCT)); 
+      memcpy(&current_cfg->antenna_info_explicit_value, &phy_cnfg->antenna_info_explicit_value,
+             sizeof(LIBLTE_RRC_ANTENNA_INFO_DEDICATED_STRUCT));
     } else if (apply_defaults) {
       current_cfg->antenna_info_explicit_value.tx_mode = LIBLTE_RRC_TRANSMISSION_MODE_2;
-      current_cfg->antenna_info_explicit_value.codebook_subset_restriction_present = false; 
-      current_cfg->antenna_info_explicit_value.ue_tx_antenna_selection_setup_present = false; 
+      current_cfg->antenna_info_explicit_value.codebook_subset_restriction_present = false;
+      current_cfg->antenna_info_explicit_value.ue_tx_antenna_selection_setup_present = false;
     }
   } else if (apply_defaults) {
     current_cfg->antenna_info_explicit_value.tx_mode = LIBLTE_RRC_TRANSMISSION_MODE_2;
-    current_cfg->antenna_info_explicit_value.codebook_subset_restriction_present = false; 
-    current_cfg->antenna_info_explicit_value.ue_tx_antenna_selection_setup_present = false; 
+    current_cfg->antenna_info_explicit_value.codebook_subset_restriction_present = false;
+    current_cfg->antenna_info_explicit_value.ue_tx_antenna_selection_setup_present = false;
   }
-  if(phy_cnfg->sched_request_cnfg_present) {
-    memcpy(&current_cfg->sched_request_cnfg, &phy_cnfg->sched_request_cnfg, sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
+  if (phy_cnfg->sched_request_cnfg_present) {
+    memcpy(&current_cfg->sched_request_cnfg, &phy_cnfg->sched_request_cnfg,
+           sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
   } else if (apply_defaults) {
-    current_cfg->sched_request_cnfg.setup_present = false; 
+    current_cfg->sched_request_cnfg.setup_present = false;
   }
-  if(phy_cnfg->pdsch_cnfg_ded_present) {
-    current_cfg->pdsch_cnfg_ded = phy_cnfg->pdsch_cnfg_ded; 
+  if (phy_cnfg->pdsch_cnfg_ded_present) {
+    current_cfg->pdsch_cnfg_ded = phy_cnfg->pdsch_cnfg_ded;
   } else if (apply_defaults) {
-    current_cfg->pdsch_cnfg_ded = LIBLTE_RRC_PDSCH_CONFIG_P_A_DB_0; 
+    current_cfg->pdsch_cnfg_ded = LIBLTE_RRC_PDSCH_CONFIG_P_A_DB_0;
   }
 
   if (phy_cnfg->cqi_report_cnfg_present) {
     if (phy_cnfg->cqi_report_cnfg.report_periodic_present) {
-      rrc_log->info("Set cqi-PUCCH-ResourceIndex=%d, cqi-pmi-ConfigIndex=%d, cqi-FormatIndicatorPeriodic=%s\n", 
-        current_cfg->cqi_report_cnfg.report_periodic.pucch_resource_idx, 
-        current_cfg->cqi_report_cnfg.report_periodic.pmi_cnfg_idx, 
-        liblte_rrc_cqi_format_indicator_periodic_text[current_cfg->cqi_report_cnfg.report_periodic.format_ind_periodic]); 
-    } 
+      rrc_log->info("Set cqi-PUCCH-ResourceIndex=%d, cqi-pmi-ConfigIndex=%d, cqi-FormatIndicatorPeriodic=%s\n",
+                    current_cfg->cqi_report_cnfg.report_periodic.pucch_resource_idx,
+                    current_cfg->cqi_report_cnfg.report_periodic.pmi_cnfg_idx,
+                    liblte_rrc_cqi_format_indicator_periodic_text[current_cfg->cqi_report_cnfg.report_periodic.format_ind_periodic]);
+    }
     if (phy_cnfg->cqi_report_cnfg.report_mode_aperiodic_present) {
-      rrc_log->info("Set cqi-ReportModeAperiodic=%s\n", 
-                    liblte_rrc_cqi_report_mode_aperiodic_text[current_cfg->cqi_report_cnfg.report_mode_aperiodic]); 
-    } 
-    
+      rrc_log->info("Set cqi-ReportModeAperiodic=%s\n",
+                    liblte_rrc_cqi_report_mode_aperiodic_text[current_cfg->cqi_report_cnfg.report_mode_aperiodic]);
+    }
+
   }
-  
+
   if (phy_cnfg->sched_request_cnfg_present) {
     rrc_log->info("Set PHY config ded: SR-n_pucch=%d, SR-ConfigIndex=%d, SR-TransMax=%d\n",
-                current_cfg->sched_request_cnfg.sr_pucch_resource_idx,
-                current_cfg->sched_request_cnfg.sr_cnfg_idx,
-                liblte_rrc_dsr_trans_max_num[current_cfg->sched_request_cnfg.dsr_trans_max]);
+                  current_cfg->sched_request_cnfg.sr_pucch_resource_idx,
+                  current_cfg->sched_request_cnfg.sr_cnfg_idx,
+                  liblte_rrc_dsr_trans_max_num[current_cfg->sched_request_cnfg.dsr_trans_max]);
   }
-  
+
   if (current_cfg->srs_ul_cnfg_ded_present) {
     rrc_log->info("Set PHY config ded: SRS-ConfigIndex=%d, SRS-bw=%s, SRS-Nrcc=%d, SRS-hop=%s, SRS-Ncs=%s\n",
-                current_cfg->srs_ul_cnfg_ded.srs_cnfg_idx,
-                liblte_rrc_srs_bandwidth_text[current_cfg->srs_ul_cnfg_ded.srs_bandwidth],
-                current_cfg->srs_ul_cnfg_ded.freq_domain_pos,
-                liblte_rrc_srs_hopping_bandwidth_text[current_cfg->srs_ul_cnfg_ded.srs_hopping_bandwidth],
-                liblte_rrc_cyclic_shift_text[current_cfg->srs_ul_cnfg_ded.cyclic_shift]);
+                  current_cfg->srs_ul_cnfg_ded.srs_cnfg_idx,
+                  liblte_rrc_srs_bandwidth_text[current_cfg->srs_ul_cnfg_ded.srs_bandwidth],
+                  current_cfg->srs_ul_cnfg_ded.freq_domain_pos,
+                  liblte_rrc_srs_hopping_bandwidth_text[current_cfg->srs_ul_cnfg_ded.srs_hopping_bandwidth],
+                  liblte_rrc_cyclic_shift_text[current_cfg->srs_ul_cnfg_ded.cyclic_shift]);
   }
-  
+
   phy->set_config_dedicated(current_cfg);
 
   // Apply changes to PHY
   phy->configure_ul_params();
-
 }
 
-void rrc::apply_mac_config_dedicated(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT *mac_cnfg, bool apply_defaults) 
-{
+void rrc::apply_mac_config_dedicated(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT *mac_cnfg, bool apply_defaults) {
   // Set Default MAC main configuration (9.2.2)
   LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT default_cfg;
   bzero(&default_cfg, sizeof(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT));
-  default_cfg.ulsch_cnfg.max_harq_tx        = LIBLTE_RRC_MAX_HARQ_TX_N5;
+  default_cfg.ulsch_cnfg.max_harq_tx = LIBLTE_RRC_MAX_HARQ_TX_N5;
   default_cfg.ulsch_cnfg.periodic_bsr_timer = LIBLTE_RRC_PERIODIC_BSR_TIMER_INFINITY;
-  default_cfg.ulsch_cnfg.retx_bsr_timer     = LIBLTE_RRC_RETRANSMISSION_BSR_TIMER_SF2560;  
-  default_cfg.ulsch_cnfg.tti_bundling       = false;
-  default_cfg.drx_cnfg.setup_present        = false;
-  default_cfg.phr_cnfg.setup_present        = false;
-  default_cfg.time_alignment_timer          = LIBLTE_RRC_TIME_ALIGNMENT_TIMER_INFINITY; 
+  default_cfg.ulsch_cnfg.retx_bsr_timer = LIBLTE_RRC_RETRANSMISSION_BSR_TIMER_SF2560;
+  default_cfg.ulsch_cnfg.tti_bundling = false;
+  default_cfg.drx_cnfg.setup_present = false;
+  default_cfg.phr_cnfg.setup_present = false;
+  default_cfg.time_alignment_timer = LIBLTE_RRC_TIME_ALIGNMENT_TIMER_INFINITY;
 
 
   if (!apply_defaults) {
-    if(mac_cnfg->ulsch_cnfg_present)
-    {
-      if(mac_cnfg->ulsch_cnfg.max_harq_tx_present) {
+    if (mac_cnfg->ulsch_cnfg_present) {
+      if (mac_cnfg->ulsch_cnfg.max_harq_tx_present) {
         default_cfg.ulsch_cnfg.max_harq_tx = mac_cnfg->ulsch_cnfg.max_harq_tx;
-        default_cfg.ulsch_cnfg.max_harq_tx_present = true; 
+        default_cfg.ulsch_cnfg.max_harq_tx_present = true;
       }
-      if(mac_cnfg->ulsch_cnfg.periodic_bsr_timer_present) {
+      if (mac_cnfg->ulsch_cnfg.periodic_bsr_timer_present) {
         default_cfg.ulsch_cnfg.periodic_bsr_timer = mac_cnfg->ulsch_cnfg.periodic_bsr_timer;
-        default_cfg.ulsch_cnfg.periodic_bsr_timer_present = true; 
+        default_cfg.ulsch_cnfg.periodic_bsr_timer_present = true;
       }
       default_cfg.ulsch_cnfg.retx_bsr_timer = mac_cnfg->ulsch_cnfg.retx_bsr_timer;
-      default_cfg.ulsch_cnfg.tti_bundling   = mac_cnfg->ulsch_cnfg.tti_bundling;
+      default_cfg.ulsch_cnfg.tti_bundling = mac_cnfg->ulsch_cnfg.tti_bundling;
     }
-    if(mac_cnfg->drx_cnfg_present) {
+    if (mac_cnfg->drx_cnfg_present) {
       memcpy(&default_cfg.drx_cnfg, &mac_cnfg->drx_cnfg, sizeof(LIBLTE_RRC_DRX_CONFIG_STRUCT));
-      default_cfg.drx_cnfg_present = true; 
+      default_cfg.drx_cnfg_present = true;
     }
-    if(mac_cnfg->phr_cnfg_present) {
-      memcpy(&default_cfg.phr_cnfg, &mac_cnfg->phr_cnfg, sizeof(LIBLTE_RRC_PHR_CONFIG_STRUCT)); 
-      default_cfg.phr_cnfg_present = true; 
+    if (mac_cnfg->phr_cnfg_present) {
+      memcpy(&default_cfg.phr_cnfg, &mac_cnfg->phr_cnfg, sizeof(LIBLTE_RRC_PHR_CONFIG_STRUCT));
+      default_cfg.phr_cnfg_present = true;
     }
     default_cfg.time_alignment_timer = mac_cnfg->time_alignment_timer;
   }
-  
+
   // Setup MAC configuration 
   mac->set_config_main(&default_cfg);
 
@@ -1246,98 +1395,92 @@ void rrc::apply_mac_config_dedicated(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT *mac_cnfg
                 liblte_rrc_periodic_bsr_timer_num[default_cfg.ulsch_cnfg.periodic_bsr_timer]);
   if (default_cfg.phr_cnfg_present) {
     rrc_log->info("Set MAC PHR config: periodicPHR-Timer=%d, prohibitPHR-Timer=%d, dl-PathlossChange=%d\n",
-      liblte_rrc_periodic_phr_timer_num[default_cfg.phr_cnfg.periodic_phr_timer],
-      liblte_rrc_prohibit_phr_timer_num[default_cfg.phr_cnfg.prohibit_phr_timer],
-      liblte_rrc_dl_pathloss_change_num[default_cfg.phr_cnfg.dl_pathloss_change]);
+                  liblte_rrc_periodic_phr_timer_num[default_cfg.phr_cnfg.periodic_phr_timer],
+                  liblte_rrc_prohibit_phr_timer_num[default_cfg.phr_cnfg.prohibit_phr_timer],
+                  liblte_rrc_dl_pathloss_change_num[default_cfg.phr_cnfg.dl_pathloss_change]);
   }
 }
 
-void rrc::apply_rr_config_dedicated(LIBLTE_RRC_RR_CONFIG_DEDICATED_STRUCT *cnfg) {   
-  if(cnfg->phy_cnfg_ded_present) {
-   apply_phy_config_dedicated(&cnfg->phy_cnfg_ded, false);
-   // Apply SR configuration to MAC 
-   if (cnfg->phy_cnfg_ded.sched_request_cnfg_present) {
-    mac->set_config_sr(&cnfg->phy_cnfg_ded.sched_request_cnfg);
-   }
+void rrc::apply_rr_config_dedicated(LIBLTE_RRC_RR_CONFIG_DEDICATED_STRUCT *cnfg) {
+  if (cnfg->phy_cnfg_ded_present) {
+    apply_phy_config_dedicated(&cnfg->phy_cnfg_ded, false);
+    // Apply SR configuration to MAC
+    if (cnfg->phy_cnfg_ded.sched_request_cnfg_present) {
+      mac->set_config_sr(&cnfg->phy_cnfg_ded.sched_request_cnfg);
+    }
   }
-  
-  if(cnfg->mac_main_cnfg_present) {
+
+  if (cnfg->mac_main_cnfg_present) {
     apply_mac_config_dedicated(&cnfg->mac_main_cnfg.explicit_value, cnfg->mac_main_cnfg.default_value);
   }
-  
-  if(cnfg->sps_cnfg_present) {
+
+  if (cnfg->sps_cnfg_present) {
     //TODO
   }
-  if(cnfg->rlf_timers_and_constants_present) {
+  if (cnfg->rlf_timers_and_constants_present) {
     //TODO
   }
-  for(uint32_t i=0; i<cnfg->srb_to_add_mod_list_size; i++) {
+  for (uint32_t i = 0; i < cnfg->srb_to_add_mod_list_size; i++) {
     // TODO: handle SRB modification
     add_srb(&cnfg->srb_to_add_mod_list[i]);
   }
-  for(uint32_t i=0; i<cnfg->drb_to_release_list_size; i++) {
+  for (uint32_t i = 0; i < cnfg->drb_to_release_list_size; i++) {
     release_drb(cnfg->drb_to_release_list[i]);
   }
-  for(uint32_t i=0; i<cnfg->drb_to_add_mod_list_size; i++) {
+  for (uint32_t i = 0; i < cnfg->drb_to_add_mod_list_size; i++) {
     // TODO: handle DRB modification
     add_drb(&cnfg->drb_to_add_mod_list[i]);
   }
 }
 
-void rrc::handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup)
-{
+void rrc::handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup) {
   // Apply the Radio Resource configuration
-  apply_rr_config_dedicated(&setup->rr_cnfg);  
+  apply_rr_config_dedicated(&setup->rr_cnfg);
 }
 
 /* Reception of RRCConnectionReestablishment by the UE 5.3.7.5 */
-void rrc::handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup) 
-{
+void rrc::handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup) {
   mac_timers->get(t301)->stop();
-  
+
   // TODO: Restablish DRB1. Not done because never was suspended
 
   // Apply the Radio Resource configuration
   apply_rr_config_dedicated(&setup->rr_cnfg);
 
   // TODO: Some security stuff here... is it necessary?
-  
+
   send_con_restablish_complete();
 }
 
 
-void rrc::handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig, byte_buffer_t *pdu)
-{
+void rrc::handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig,
+                                  byte_buffer_t *pdu) {
   uint32_t i;
-  
+
   if (reconfig->rr_cnfg_ded_present) {
     apply_rr_config_dedicated(&reconfig->rr_cnfg_ded);
   } else {
     printf("received con reconfig no rr confg present\n");
   }
-  if(reconfig->meas_cnfg_present)
-  {
+  if (reconfig->meas_cnfg_present) {
     //TODO: handle meas_cnfg
   }
-  if(reconfig->mob_ctrl_info_present)
-  {
+  if (reconfig->mob_ctrl_info_present) {
     //TODO: handle mob_ctrl_info
   }
 
   send_rrc_con_reconfig_complete(lcid, pdu);
 
   byte_buffer_t *nas_sdu;
-  for(i=0;i<reconfig->N_ded_info_nas;i++)
-  {
-    nas_sdu = pool_allocate;
+  for (i = 0; i < reconfig->N_ded_info_nas; i++) {
+    nas_sdu = pool_allocate;;
     memcpy(nas_sdu->msg, &reconfig->ded_info_nas_list[i].msg, reconfig->ded_info_nas_list[i].N_bytes);
     nas_sdu->N_bytes = reconfig->ded_info_nas_list[i].N_bytes;
     nas->write_pdu(lcid, nas_sdu);
   }
 }
 
-void rrc::add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg)
-{
+void rrc::add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg) {
   // Setup PDCP
   pdcp->add_bearer(srb_cnfg->srb_id, srslte_pdcp_config_t(true)); // Set PDCP config control flag
   if(RB_ID_SRB2 == srb_cnfg->srb_id) {
@@ -1345,10 +1488,8 @@ void rrc::add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg)
   }
 
   // Setup RLC
-  if(srb_cnfg->rlc_cnfg_present)
-  {
-    if(srb_cnfg->rlc_default_cnfg_present)
-    {
+  if (srb_cnfg->rlc_cnfg_present) {
+    if (srb_cnfg->rlc_default_cnfg_present) {
       rlc->add_bearer(srb_cnfg->srb_id);
     }else{
       rlc->add_bearer(srb_cnfg->srb_id, srslte_rlc_config_t(&srb_cnfg->rlc_explicit_cnfg));
@@ -1356,57 +1497,51 @@ void rrc::add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg)
   }
 
   // Setup MAC
-  uint8_t  log_chan_group       =  0;
-  uint8_t  priority             =  1;
-  int      prioritized_bit_rate = -1;
-  int      bucket_size_duration = -1;
+  uint8_t log_chan_group = 0;
+  uint8_t priority = 1;
+  int prioritized_bit_rate = -1;
+  int bucket_size_duration = -1;
 
-  if(srb_cnfg->lc_cnfg_present)
-  {
-    if(srb_cnfg->lc_default_cnfg_present)
-    {
-      if(RB_ID_SRB2 == srb_cnfg->srb_id)
+  if (srb_cnfg->lc_cnfg_present) {
+    if (srb_cnfg->lc_default_cnfg_present) {
+      if (RB_ID_SRB2 == srb_cnfg->srb_id)
         priority = 3;
-    }else{
-      if(srb_cnfg->lc_explicit_cnfg.log_chan_sr_mask_present)
-      {
+    } else {
+      if (srb_cnfg->lc_explicit_cnfg.log_chan_sr_mask_present) {
         //TODO
       }
-      if(srb_cnfg->lc_explicit_cnfg.ul_specific_params_present)
-      {
-        if(srb_cnfg->lc_explicit_cnfg.ul_specific_params.log_chan_group_present)
-          log_chan_group      = srb_cnfg->lc_explicit_cnfg.ul_specific_params.log_chan_group;
+      if (srb_cnfg->lc_explicit_cnfg.ul_specific_params_present) {
+        if (srb_cnfg->lc_explicit_cnfg.ul_specific_params.log_chan_group_present)
+          log_chan_group = srb_cnfg->lc_explicit_cnfg.ul_specific_params.log_chan_group;
 
-        priority              = srb_cnfg->lc_explicit_cnfg.ul_specific_params.priority;
-        prioritized_bit_rate  = liblte_rrc_prioritized_bit_rate_num[srb_cnfg->lc_explicit_cnfg.ul_specific_params.prioritized_bit_rate];
-        bucket_size_duration  = liblte_rrc_bucket_size_duration_num[srb_cnfg->lc_explicit_cnfg.ul_specific_params.bucket_size_duration];
+        priority = srb_cnfg->lc_explicit_cnfg.ul_specific_params.priority;
+        prioritized_bit_rate = liblte_rrc_prioritized_bit_rate_num[srb_cnfg->lc_explicit_cnfg.ul_specific_params.prioritized_bit_rate];
+        bucket_size_duration = liblte_rrc_bucket_size_duration_num[srb_cnfg->lc_explicit_cnfg.ul_specific_params.bucket_size_duration];
       }
     }
     mac->setup_lcid(srb_cnfg->srb_id, log_chan_group, priority, prioritized_bit_rate, bucket_size_duration);
   }
 
   srbs[srb_cnfg->srb_id] = *srb_cnfg;
-  rrc_log->info("Added radio bearer %s\n", rb_id_str[srb_cnfg->srb_id].c_str());
+  rrc_log->info("Added radio bearer %s\n", get_rb_name(srb_cnfg->srb_id).c_str());
 }
 
-void rrc::add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg)
-{
+void rrc::add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg) {
 
-  if(!drb_cnfg->pdcp_cnfg_present ||
-     !drb_cnfg->rlc_cnfg_present  ||
-     !drb_cnfg->lc_cnfg_present)
-  {
+  if (!drb_cnfg->pdcp_cnfg_present ||
+      !drb_cnfg->rlc_cnfg_present ||
+      !drb_cnfg->lc_cnfg_present) {
     rrc_log->error("Cannot add DRB - incomplete configuration\n");
     return;
   }
-  uint32_t lcid = 0; 
+  uint32_t lcid = 0;
   if (drb_cnfg->lc_id_present) {
     lcid = drb_cnfg->lc_id;
   } else {
     lcid = RB_ID_SRB2 + drb_cnfg->drb_id;
     rrc_log->warning("LCID not present, using %d\n", lcid);
   }
-  
+
   // Setup PDCP
   srslte_pdcp_config_t pdcp_cfg;
   pdcp_cfg.is_data = true;
@@ -1422,85 +1557,81 @@ void rrc::add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg)
   rlc->add_bearer(lcid, srslte_rlc_config_t(&drb_cnfg->rlc_cnfg));
 
   // Setup MAC
-  uint8_t  log_chan_group       =  0;
-  uint8_t  priority             =  1;
-  int      prioritized_bit_rate = -1;
-  int      bucket_size_duration = -1;
-  if(drb_cnfg->lc_cnfg.ul_specific_params_present)
-  {
-    if(drb_cnfg->lc_cnfg.ul_specific_params.log_chan_group_present) {
-      log_chan_group      = drb_cnfg->lc_cnfg.ul_specific_params.log_chan_group;
+  uint8_t log_chan_group = 0;
+  uint8_t priority = 1;
+  int prioritized_bit_rate = -1;
+  int bucket_size_duration = -1;
+  if (drb_cnfg->lc_cnfg.ul_specific_params_present) {
+    if (drb_cnfg->lc_cnfg.ul_specific_params.log_chan_group_present) {
+      log_chan_group = drb_cnfg->lc_cnfg.ul_specific_params.log_chan_group;
     } else {
       rrc_log->warning("LCG not present, setting to 0\n");
     }
-    priority              = drb_cnfg->lc_cnfg.ul_specific_params.priority;
-    prioritized_bit_rate  = liblte_rrc_prioritized_bit_rate_num[drb_cnfg->lc_cnfg.ul_specific_params.prioritized_bit_rate];
-    
+    priority = drb_cnfg->lc_cnfg.ul_specific_params.priority;
+    prioritized_bit_rate = liblte_rrc_prioritized_bit_rate_num[drb_cnfg->lc_cnfg.ul_specific_params.prioritized_bit_rate];
+
     if (prioritized_bit_rate > 0) {
       rrc_log->warning("PBR>0 currently not supported. Setting it to Inifinty\n");
-      prioritized_bit_rate = -1; 
+      prioritized_bit_rate = -1;
     }
-    
-    bucket_size_duration  = liblte_rrc_bucket_size_duration_num[drb_cnfg->lc_cnfg.ul_specific_params.bucket_size_duration];
-  } 
+
+    bucket_size_duration = liblte_rrc_bucket_size_duration_num[drb_cnfg->lc_cnfg.ul_specific_params.bucket_size_duration];
+  }
   mac->setup_lcid(lcid, log_chan_group, priority, prioritized_bit_rate, bucket_size_duration);
-  
+
   drbs[lcid] = *drb_cnfg;
   drb_up     = true;
-  rrc_log->info("Added radio bearer %s\n", rb_id_str[lcid].c_str());
+  rrc_log->info("Added radio bearer %s\n", get_rb_name(lcid).c_str());
 }
 
-void rrc::release_drb(uint8_t lcid)
-{
+void rrc::release_drb(uint8_t lcid) {
   // TODO
 }
 
 /************************** 
- *  DEFAULT VALUES Section 9
+*  DEFAULT VALUES Section 9
 ****************************/
 
 // PHY CONFIG DEDICATED Defaults (3GPP 36.331 v10 9.2.4)
-void rrc::set_phy_default_pucch_srs()
-{
-  
+void rrc::set_phy_default_pucch_srs() {
+
   phy_interface_rrc::phy_cfg_t current_cfg;
   phy->get_config(&current_cfg);
-  
+
   // Set defaults to CQI, SRS and SR
-  current_cfg.dedicated.cqi_report_cnfg_present    = false; 
-  current_cfg.dedicated.srs_ul_cnfg_ded_present    = false; 
-  current_cfg.dedicated.sched_request_cnfg_present = false; 
+  current_cfg.dedicated.cqi_report_cnfg_present = false;
+  current_cfg.dedicated.srs_ul_cnfg_ded_present = false;
+  current_cfg.dedicated.sched_request_cnfg_present = false;
 
   apply_phy_config_dedicated(&current_cfg.dedicated, true);
-  
-  // Release SR configuration from MAC 
-  LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT cfg; 
+
+  // Release SR configuration from MAC
+  LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT cfg;
   bzero(&cfg, sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
   mac->set_config_sr(&cfg);
 }
 
-void rrc::set_phy_default()
-{
-  LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT defaults; 
+void rrc::set_phy_default() {
+  LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT defaults;
   bzero(&defaults, sizeof(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT));
   apply_phy_config_dedicated(&defaults, true);
 }
 
-void rrc::set_mac_default()
-{
+void rrc::set_mac_default() {
   apply_mac_config_dedicated(NULL, true);
-  LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT sr_cfg; 
+  LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT sr_cfg;
   bzero(&sr_cfg, sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
-  sr_cfg.setup_present = false; 
+  sr_cfg.setup_present = false;
   mac->set_config_sr(&sr_cfg);
 }
 
 void rrc::set_rrc_default() {
   N310 = 1;
-  N311 = 1; 
+  N311 = 1;
   mac_timers->get(t310)->set(this, 1000);
   mac_timers->get(t311)->set(this, 1000);
   mac_timers->get(safe_reset_timer)->set(this, 10);
 }
 
+
 } // namespace srsue
diff --git a/srsue/test/phy/CMakeLists.txt b/srsue/test/phy/CMakeLists.txt
index bb463efb0..a0570f0e7 100644
--- a/srsue/test/phy/CMakeLists.txt
+++ b/srsue/test/phy/CMakeLists.txt
@@ -18,8 +18,8 @@
 # and at http://www.gnu.org/licenses/.
 #
 
-add_executable(ue_itf_test_sib1 ue_itf_test_sib1.cc)
-target_link_libraries(ue_itf_test_sib1 srsue_phy srslte_common srslte_phy srslte_radio ${CMAKE_THREAD_LIBS_INIT} ${Boost_LIBRARIES})
+#add_executable(ue_itf_test_sib1 ue_itf_test_sib1.cc)
+#target_link_libraries(ue_itf_test_sib1 srsue_phy srslte_common srslte_phy srslte_radio ${CMAKE_THREAD_LIBS_INIT} ${Boost_LIBRARIES})
 
-add_executable(ue_itf_test_prach ue_itf_test_prach.cc)
-target_link_libraries(ue_itf_test_prach srsue_phy srslte_common srslte_phy srslte_radio ${CMAKE_THREAD_LIBS_INIT} ${Boost_LIBRARIES})
+#add_executable(ue_itf_test_prach ue_itf_test_prach.cc)
+#target_link_libraries(ue_itf_test_prach srsue_phy srslte_common srslte_phy srslte_radio ${CMAKE_THREAD_LIBS_INIT} ${Boost_LIBRARIES})
diff --git a/srsue/test/phy/ue_itf_test_prach.cc b/srsue/test/phy/ue_itf_test_prach.cc
index c2d78470d..42e04c495 100644
--- a/srsue/test/phy/ue_itf_test_prach.cc
+++ b/srsue/test/phy/ue_itf_test_prach.cc
@@ -380,9 +380,7 @@ int main(int argc, char *argv[])
   radio.set_tx_freq(prog_args.rf_tx_freq);
   
   // Instruct the PHY to configure PRACH parameters and sync to current cell 
-  my_phy.sync_start();
-  
-  while(!my_phy.status_is_sync()) {
+  while(!my_phy.sync_status()) {
     usleep(20000);
   }
 
diff --git a/srsue/test/phy/ue_itf_test_sib1.cc b/srsue/test/phy/ue_itf_test_sib1.cc
index be3cb18e8..90f8b5b43 100644
--- a/srsue/test/phy/ue_itf_test_sib1.cc
+++ b/srsue/test/phy/ue_itf_test_sib1.cc
@@ -194,11 +194,9 @@ int main(int argc, char *argv[])
   // Set RX freq and gain
   radio.set_rx_freq(prog_args.rf_freq);
   
-  my_phy.sync_start();
-  
-  bool running = true; 
+  bool running = true;
   while(running) {
-    if (bch_decoded && my_phy.status_is_sync()) {
+    if (bch_decoded && my_phy.sync_status()) {
       uint32_t tti = my_phy.get_current_tti();
       
       // SIB1 is scheduled in subframe #5 of even frames, try to decode next frame SIB1
@@ -208,7 +206,7 @@ int main(int argc, char *argv[])
       total_pkts++;       
     }
     usleep(30000);    
-    if (bch_decoded && my_phy.status_is_sync() && total_pkts > 0) {
+    if (bch_decoded && my_phy.sync_status() && total_pkts > 0) {
       if (srslte_verbose == SRSLTE_VERBOSE_NONE && srsapps_verbose == 0) {
         float gain = prog_args.rf_gain; 
         if (gain < 0) {
diff --git a/srsue/test/upper/CMakeLists.txt b/srsue/test/upper/CMakeLists.txt
index 324ae4913..cbffe1cc8 100644
--- a/srsue/test/upper/CMakeLists.txt
+++ b/srsue/test/upper/CMakeLists.txt
@@ -18,17 +18,6 @@
 # and at http://www.gnu.org/licenses/.
 #
 
-# IP traffic over RLC test 
-add_executable(ip_test ip_test.cc)
-target_link_libraries(ip_test srsue_mac
-                              srsue_phy
-                              srslte_common
-                              srslte_phy
-                              srslte_radio
-                              srslte_upper
-                              ${CMAKE_THREAD_LIBS_INIT}
-                              ${Boost_LIBRARIES})
-
 add_executable(usim_test usim_test.cc)
 target_link_libraries(usim_test srsue_upper srslte_upper srslte_phy)
 add_test(usim_test usim_test)
diff --git a/srsue/ue.conf.example b/srsue/ue.conf.example
index ea162e337..76868008f 100644
--- a/srsue/ue.conf.example
+++ b/srsue/ue.conf.example
@@ -3,8 +3,8 @@
 #####################################################################
 # RF configuration
 #
-# dl_freq: Downlink centre frequency (Hz).
-# ul_freq: Uplink centre frequency (Hz).
+# dl_earfcn: Downlink EARFCN code.
+# freq_offset: Uplink and Downlink optional frequency offset (in Hz)
 # tx_gain: Transmit gain (dB). 
 # rx_gain: Optional receive gain (dB). If disabled, AGC if enabled
 #
@@ -21,8 +21,8 @@
 #                     Default "auto". B210 USRP: 400 us, bladeRF: 0 us. 
 #####################################################################
 [rf]
-dl_freq = 2685000000
-ul_freq = 2565000000
+dl_earfcn = 3400
+freq_offset = 0
 tx_gain = 80
 rx_gain = 50