Merging next into guti attach branch

CMakeLists.txt

@@ -74,6 +74,7 @@ option(ENABLE_BLADERF  "Enable BladeRF"                           ON)
 
 option(BUILD_STATIC    "Attempt to statically link external deps" OFF)
 option(RPATH           "Enable RPATH"                             OFF)
+option(ENABLE_ASAN     "Enable gcc address sanitizer"             OFF)
 
 option(USE_LTE_RATES   "Use standard LTE sampling rates"          OFF)
 
@@ -256,9 +257,14 @@ if(CMAKE_C_COMPILER_ID MATCHES "GNU" OR CMAKE_C_COMPILER_ID MATCHES "Clang")
   if(${CMAKE_BUILD_TYPE} STREQUAL "Debug")
     set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -g -O0 -DDEBUG_MODE")
   else(${CMAKE_BUILD_TYPE} STREQUAL "Debug")
+    if(${CMAKE_BUILD_TYPE} STREQUAL "RelWithDebInfo")
+      set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DDEBUG_MODE")
+    else(${CMAKE_BUILD_TYPE} STREQUAL "RelWithDebInfo")
       set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3")
+    endif(${CMAKE_BUILD_TYPE} STREQUAL "RelWithDebInfo")
   endif(${CMAKE_BUILD_TYPE} STREQUAL "Debug")
 
+
   if (USE_LTE_RATES)
     message(STATUS "Using standard LTE sampling rates")
     set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DFORCE_STANDARD_RATE")
@@ -303,6 +309,10 @@ if(CMAKE_C_COMPILER_ID MATCHES "GNU" OR CMAKE_C_COMPILER_ID MATCHES "Clang")
   if(NOT WIN32)
       ADD_CXX_COMPILER_FLAG_IF_AVAILABLE(-fvisibility=hidden HAVE_VISIBILITY_HIDDEN)
   endif(NOT WIN32)
+ if (ENABLE_ASAN)
+   set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fsanitize=address")
+   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address")
+ endif (ENABLE_ASAN)
 endif(CMAKE_C_COMPILER_ID MATCHES "GNU" OR CMAKE_C_COMPILER_ID MATCHES "Clang")
 
 if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")

lib/examples/pdsch_ue.c

@@ -451,9 +451,6 @@ int main(int argc, char **argv) {
       exit(0);
     }
 
-    srslte_rf_stop_rx_stream(&rf);
-    srslte_rf_flush_buffer(&rf);    
-
     /* set sampling frequency */
     int srate = srslte_sampling_freq_hz(cell.nof_prb);    
     if (srate != -1) {  

lib/include/srslte/common/buffer_pool.h

@@ -72,6 +72,10 @@ public:
       delete available.top();
       available.pop();
     }
+
+    for (uint32_t i = 0; i < used.size(); i++) {
+      delete used[i];
+    }
   }
   
   void print_all_buffers()

lib/include/srslte/common/log.h

@@ -84,6 +84,8 @@ public:
     level_text_short = true;
   }
 
+  virtual ~log() {};
+
   // This function shall be called at the start of every tti for printing tti
   void step(uint32_t tti_) {
     tti = tti_;

lib/include/srslte/common/msg_queue.h

@@ -56,6 +56,9 @@ public:
 
   ~msg_queue()
   {
+    pthread_mutex_destroy(&mutex);
+    pthread_cond_destroy(&not_empty);
+    pthread_cond_destroy(&not_full);
     delete [] buf;
   }
 

lib/include/srslte/common/timeout.h

@@ -84,7 +84,6 @@ public:
   }
   void thread_func()
   {
-   
     // substract time elapsed until now from timer duration
     gettimeofday(&start_time[2], NULL); 
     get_time_interval(start_time);
@@ -105,6 +104,14 @@ public:
       return false;
     }
   }
+  int32_t get_msec_to_expire() {
+    if (running) {
+      gettimeofday(&start_time[2], NULL);
+      get_time_interval(start_time);
+      return (duration_msec*1000 - start_time[0].tv_usec)/1000;
+    }
+    return 0;
+  }
   bool is_running()
   {
     return running;

lib/include/srslte/common/timers.h

@@ -151,15 +151,16 @@ public:
       fprintf(stderr, "Error getting unique timer id: no more timers available\n");
       return 0;
     } else {
-      while(used_timers[next_timer]) {
+      for (uint32_t i=0;i<nof_timers;i++) {
-        next_timer++;
+        if (!used_timers[i]) {
-        if (next_timer >= nof_timers) {
+          used_timers[i] = true;
-          next_timer=0;
+          nof_used_timers++;
+          return i;
         }
       }
-      used_timers[next_timer] = true;
+      fprintf(stderr, "Error getting unique timer id: no more timers available but nof_used_timers=%d, nof_timers=%d\n",
-      nof_used_timers++;
+              nof_used_timers, nof_timers);
-      return next_timer;
+      return 0;
     }
   }
 private:

lib/include/srslte/interfaces/ue_interfaces.h

@@ -119,7 +119,7 @@ public:
   virtual uint32_t  get_ul_count() = 0;
   virtual bool      get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi) = 0;
   virtual bool      get_k_asme(uint8_t *k_asme_, uint32_t n) = 0;
-  virtual void      plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) = 0;
+  virtual bool      plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) = 0;
   virtual void      plmn_search_end() = 0;
 };
 
@@ -173,7 +173,7 @@ public:
   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 void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, bool connect_request = false) = 0;
   virtual std::string get_rb_name(uint32_t lcid) = 0;
 };
 
@@ -475,6 +475,7 @@ typedef struct {
   int cqi_fixed; 
   float snr_ema_coeff; 
   std::string snr_estim_alg;
+  bool cfo_is_doppler;
   bool cfo_integer_enabled; 
   float cfo_correct_tol_hz;
   float cfo_pss_ema;
@@ -487,12 +488,14 @@ typedef struct {
   uint32_t cfo_ref_mask;
   bool average_subframe_enabled;
   int time_correct_period; 
-  bool sfo_correct_disable; 
   std::string sss_algorithm;
   float estimator_fil_w;   
   bool rssi_sensor_enabled;
   bool sic_pss_enabled;
   float rx_gain_offset;
+  bool pdsch_csi_enabled;
+  uint32_t intra_freq_meas_len_ms;
+  uint32_t intra_freq_meas_period_ms;
 } phy_args_t; 
 
 

lib/include/srslte/phy/ch_estimation/chest_dl.h

@@ -83,9 +83,12 @@ typedef struct {
   srslte_interp_lin_t srslte_interp_lin_mbsfn;
   float rssi[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS]; 
   float rsrp[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
+  float rsrp_corr[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
   float noise_estimate[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
   float cfo;
 
+  bool     rsrp_neighbour;
+
   bool     cfo_estimate_enable;
   uint32_t cfo_estimate_sf_mask;
 
@@ -158,6 +161,9 @@ SRSLTE_API void srslte_chest_dl_cfo_estimate_enable(srslte_chest_dl_t *q,
 SRSLTE_API void srslte_chest_dl_average_subframe(srslte_chest_dl_t *q,
                                                  bool enable);
 
+SRSLTE_API void srslte_chest_dl_set_rsrp_neighbour(srslte_chest_dl_t *q,
+                                                   bool rsrp_for_neighbour);
+
 SRSLTE_API float srslte_chest_dl_get_noise_estimate(srslte_chest_dl_t *q);
 
 SRSLTE_API float srslte_chest_dl_get_cfo(srslte_chest_dl_t *q);
@@ -185,4 +191,6 @@ SRSLTE_API float srslte_chest_dl_get_rsrp_port(srslte_chest_dl_t *q,
 
 SRSLTE_API float srslte_chest_dl_get_rsrp(srslte_chest_dl_t *q);
 
+SRSLTE_API float srslte_chest_dl_get_rsrp_neighbour(srslte_chest_dl_t *q);
+
 #endif

lib/include/srslte/phy/fec/softbuffer.h

@@ -42,6 +42,9 @@
 typedef struct SRSLTE_API {
   uint32_t max_cb;
   int16_t **buffer_f;  
+  uint8_t **data;
+  bool *cb_crc;
+  bool tb_crc;
 } srslte_softbuffer_rx_t;
 
 typedef struct SRSLTE_API {

lib/include/srslte/phy/mimo/precoding.h

@@ -79,6 +79,7 @@ SRSLTE_API int srslte_precoding_type(cf_t *x[SRSLTE_MAX_LAYERS],
 SRSLTE_API int srslte_predecoding_single(cf_t *y, 
                                          cf_t *h, 
                                          cf_t *x, 
+                                         float *csi,
                                          int nof_symbols, 
                                          float scaling,
                                          float noise_estimate);
@@ -86,6 +87,7 @@ SRSLTE_API int srslte_predecoding_single(cf_t *y,
 SRSLTE_API int srslte_predecoding_single_multi(cf_t *y[SRSLTE_MAX_PORTS], 
                                                cf_t *h[SRSLTE_MAX_PORTS], 
                                                cf_t *x, 
+                                               float *csi,
                                                int nof_rxant,
                                                int nof_symbols, 
                                                float scaling,
@@ -111,6 +113,7 @@ SRSLTE_API void srslte_predecoding_set_mimo_decoder (srslte_mimo_decoder_t _mimo
 SRSLTE_API int srslte_predecoding_type(cf_t *y[SRSLTE_MAX_PORTS],
                                        cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
                                        cf_t *x[SRSLTE_MAX_LAYERS],
+                                       float *csi,
                                        int nof_rxant,
                                        int nof_ports,
                                        int nof_layers,

lib/include/srslte/phy/phch/pdsch.h

@@ -76,6 +76,9 @@ typedef struct SRSLTE_API {
   cf_t *d[SRSLTE_MAX_CODEWORDS];                /* Modulated/Demodulated codewords */
   void *e[SRSLTE_MAX_CODEWORDS];
 
+  bool csi_enabled;
+  float *csi[SRSLTE_MAX_CODEWORDS];             /* Channel Strengh Indicator */
+
   /* tx & rx objects */
   srslte_modem_table_t mod[4];
   
@@ -107,6 +110,9 @@ SRSLTE_API int srslte_pdsch_set_rnti(srslte_pdsch_t *q,
 SRSLTE_API void srslte_pdsch_set_power_allocation(srslte_pdsch_t *q,
                                                   float rho_a);
 
+SRSLTE_API int srslte_pdsch_enable_csi(srslte_pdsch_t *q,
+                                       bool enable);
+
 SRSLTE_API void srslte_pdsch_free_rnti(srslte_pdsch_t *q, 
                                       uint16_t rnti);
 

lib/include/srslte/phy/ue/ue_dl.h

@@ -84,8 +84,6 @@ typedef struct SRSLTE_API {
   srslte_ofdm_t fft_mbsfn;
   srslte_chest_dl_t chest;
   
-  srslte_cfo_t sfo_correct; 
-  
   srslte_pdsch_cfg_t pdsch_cfg;
   srslte_pdsch_cfg_t pmch_cfg;
   srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS];
@@ -126,8 +124,6 @@ typedef struct SRSLTE_API {
   srslte_dci_msg_t pending_ul_dci_msg; 
   uint16_t pending_ul_dci_rnti; 
   
-  float sample_offset;
-
   float last_phich_corr;
 }srslte_ue_dl_t;
 
@@ -195,9 +191,6 @@ SRSLTE_API int srslte_ue_dl_find_dl_dci_type(srslte_ue_dl_t *q,
 
 SRSLTE_API uint32_t srslte_ue_dl_get_ncce(srslte_ue_dl_t *q);
 
-SRSLTE_API void srslte_ue_dl_set_sample_offset(srslte_ue_dl_t * q, 
-                                               float sample_offset); 
-
 SRSLTE_API int srslte_ue_dl_decode(srslte_ue_dl_t *q,
                                    uint8_t *data[SRSLTE_MAX_CODEWORDS],
                                    uint32_t tm,

lib/include/srslte/phy/utils/ringbuffer.h

@@ -28,6 +28,8 @@ SRSLTE_API void srslte_ringbuffer_reset(srslte_ringbuffer_t *q);
 
 SRSLTE_API int  srslte_ringbuffer_status(srslte_ringbuffer_t *q);
 
+SRSLTE_API int srslte_ringbuffer_space(srslte_ringbuffer_t *q);
+
 SRSLTE_API int  srslte_ringbuffer_write(srslte_ringbuffer_t *q,
                                        void *ptr,
                                        int nof_bytes);

lib/include/srslte/phy/utils/simd.h

@@ -530,7 +530,7 @@ static inline simd_cf_t srslte_simd_cfi_loadu(const cf_t *ptr) {
                                                          0x11, 0x13, 0x15, 0x17,
                                                          0x19, 0x1B, 0x1D, 0x1F), in2);
 #else /* LV_HAVE_AVX512 */
-  #ifdef LV_HAVE_AVX2
+#ifdef LV_HAVE_AVX2
   __m256 in1 = _mm256_permute_ps(_mm256_loadu_ps((float*)(ptr)), 0b11011000);
   __m256 in2 = _mm256_permute_ps(_mm256_loadu_ps((float*)(ptr + 4)), 0b11011000);
   ret.re = _mm256_unpacklo_ps(in1, in2);
@@ -705,6 +705,18 @@ static inline void srslte_simd_cf_storeu(float *re, float *im, simd_cf_t simdreg
 #endif /* LV_HAVE_AVX512 */
 }
 
+static inline simd_f_t srslte_simd_cf_re(simd_cf_t in) {
+  simd_f_t out = in.re;
+#ifndef LV_HAVE_AVX512
+#ifdef LV_HAVE_AVX2
+  /* Permute for AVX registers (mis SSE registers) */
+  const __m256i idx = _mm256_setr_epi32(0, 2, 4, 6, 1, 3, 5, 7);
+  out = _mm256_permutevar8x32_ps(out, idx);
+#endif /* LV_HAVE_AVX2 */
+#endif /* LV_HAVE_AVX512 */
+  return out;
+}
+
 static inline simd_cf_t srslte_simd_cf_set1 (cf_t x) {
   simd_cf_t ret;
 #ifdef LV_HAVE_AVX512

lib/include/srslte/phy/utils/vector.h

@@ -73,7 +73,7 @@ SRSLTE_API void srslte_vec_fprint_byte(FILE *stream, uint8_t *x, const uint32_t
 SRSLTE_API void srslte_vec_fprint_i(FILE *stream, int *x, const uint32_t len);
 SRSLTE_API void srslte_vec_fprint_s(FILE *stream, short *x, const uint32_t len); 
 SRSLTE_API void srslte_vec_fprint_hex(FILE *stream, uint8_t *x, const uint32_t len);
-SRSLTE_API void srslte_vec_sprint_hex(char *str, uint8_t *x, const uint32_t len); 
+SRSLTE_API void srslte_vec_sprint_hex(char *str, const uint32_t max_str_len, uint8_t *x, const uint32_t len);
 
 /* Saves/loads a vector to a file */
 SRSLTE_API void srslte_vec_save_file(char *filename, const void *buffer, const uint32_t len);

lib/src/common/buffer_pool.cc

@@ -55,11 +55,4 @@ void byte_buffer_pool::cleanup(void)
   pthread_mutex_unlock(&instance_mutex);
 }
   
-  
-
-
-
-
-
-
 } // namespace srsue

lib/src/common/pdu.cc

@@ -122,6 +122,12 @@ uint8_t* sch_pdu::write_packet(srslte::log *log_h)
   sch_subh padding; 
   padding.set_padding();
 
+  if (nof_subheaders <= 0) {
+    log_h->error("Trying to write packet with invalid number of subheaders (nof_subheaders=%d).\n", nof_subheaders);
+    log_h->console("Trying to write packet with invalid number of subheaders (nof_subheaders=%d).\n", nof_subheaders);
+    return NULL;
+  }
+
   if (init_rem_len < 0) {
     log_h->error("init_rem_len=%d\n", init_rem_len);
     return NULL; 

lib/src/phy/ch_estimation/chest_dl.c

@@ -88,7 +88,7 @@ int srslte_chest_dl_init(srslte_chest_dl_t *q, uint32_t max_prb)
       goto clean_exit;
     }
     
-    q->mbsfn_refs = calloc(SRSLTE_MAX_MBSFN_AREA_IDS, sizeof(srslte_refsignal_t*));
+    q->mbsfn_refs = calloc(SRSLTE_MAX_MBSFN_AREA_IDS, sizeof(srslte_refsignal_t));
     if (!q->mbsfn_refs) {
       fprintf(stderr, "Calloc error initializing mbsfn_refs (%d)\n", ret);
       goto clean_exit;
@@ -153,6 +153,8 @@ int srslte_chest_dl_init(srslte_chest_dl_t *q, uint32_t max_prb)
     
     q->noise_alg = SRSLTE_NOISE_ALG_REFS; 
 
+    q->rsrp_neighbour = false;
+
     q->smooth_filter_len = 3; 
     srslte_chest_dl_set_smooth_filter3_coeff(q, 0.1);
     
@@ -169,14 +171,14 @@ clean_exit:
 
 void srslte_chest_dl_free(srslte_chest_dl_t *q) 
 {
-  int i;
   if(&q->csr_refs)
     srslte_refsignal_free(&q->csr_refs);
 
   if (q->mbsfn_refs) {
-    for (i=0; i<SRSLTE_MAX_MBSFN_AREA_IDS; i++) {
+    for (int i=0; i<SRSLTE_MAX_MBSFN_AREA_IDS; i++) {
       if (q->mbsfn_refs[i]) {
         srslte_refsignal_free(q->mbsfn_refs[i]);
+        free(q->mbsfn_refs[i]);
       }
     }
     free(q->mbsfn_refs);
@@ -206,7 +208,8 @@ void srslte_chest_dl_free(srslte_chest_dl_t *q)
 
 
 int srslte_chest_dl_set_mbsfn_area_id(srslte_chest_dl_t *q, uint16_t mbsfn_area_id){
-  if(!q->mbsfn_refs[mbsfn_area_id]){
+  if (mbsfn_area_id < SRSLTE_MAX_MBSFN_AREA_IDS) {
+    if(!q->mbsfn_refs[mbsfn_area_id]) {
       q->mbsfn_refs[mbsfn_area_id] = calloc(1, sizeof(srslte_refsignal_t));
     }
     if(q->mbsfn_refs[mbsfn_area_id]) {
@@ -215,6 +218,8 @@ int srslte_chest_dl_set_mbsfn_area_id(srslte_chest_dl_t *q, uint16_t mbsfn_area_
       }
     }
     return SRSLTE_SUCCESS;
+  }
+  return SRSLTE_ERROR;
 }
 
 int srslte_chest_dl_set_cell(srslte_chest_dl_t *q, srslte_cell_t cell)
@@ -554,14 +559,18 @@ int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, u
               q->pilot_estimates, npilots);
 
   /* Compute RSRP for the channel estimates in this port */
+  if (q->rsrp_neighbour) {
     double energy = cabs(srslte_vec_acc_cc(q->pilot_estimates, npilots)/npilots);
-  q->rsrp[rxant_id][port_id] = energy*energy;
+    q->rsrp_corr[rxant_id][port_id] = energy*energy;
+  }
+  q->rsrp[rxant_id][port_id] = srslte_vec_avg_power_cf(q->pilot_recv_signal, npilots);
   q->rssi[rxant_id][port_id] = srslte_chest_dl_rssi(q, input, port_id);
 
   chest_interpolate_noise_est(q, input, ce, sf_idx, port_id, rxant_id, SRSLTE_SF_NORM);
 
   return 0;
 }
+
 int srslte_chest_dl_estimate_port_mbsfn(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, uint32_t sf_idx, uint32_t port_id, uint32_t rxant_id, uint16_t mbsfn_area_id)
 {
 
@@ -620,6 +629,10 @@ int srslte_chest_dl_estimate_multi_mbsfn(srslte_chest_dl_t *q, cf_t *input[SRSLT
   return SRSLTE_SUCCESS;
 }
 
+void srslte_chest_dl_set_rsrp_neighbour(srslte_chest_dl_t *q, bool rsrp_for_neighbour) {
+  q->rsrp_neighbour = rsrp_for_neighbour;
+}
+
 void srslte_chest_dl_average_subframe(srslte_chest_dl_t *q, bool enable)
 {
   q->average_subframe = enable;
@@ -707,8 +720,21 @@ float srslte_chest_dl_get_rsrp_port(srslte_chest_dl_t *q, uint32_t port) {
   return sum;
 }
 
+float srslte_chest_dl_get_rsrp_neighbour_port(srslte_chest_dl_t *q, uint32_t port) {
+  float sum = 0.0f;
+  for (int j = 0; j < q->cell.nof_ports; ++j) {
+    sum +=q->rsrp_corr[port][j];
+  }
+
+  if (q->cell.nof_ports) {
+    sum /= q->cell.nof_ports;
+  }
+
+  return sum;
+}
+
 float srslte_chest_dl_get_rsrp(srslte_chest_dl_t *q) {
-  float max = -0.0f;
+  float max = -1e9;
   for (int i = 0; i < q->last_nof_antennas; ++i) {
     float v = srslte_chest_dl_get_rsrp_port(q, i);
     if (v > max) {
@@ -717,3 +743,14 @@ float srslte_chest_dl_get_rsrp(srslte_chest_dl_t *q) {
   }
   return max;
 }
+
+float srslte_chest_dl_get_rsrp_neighbour(srslte_chest_dl_t *q) {
+  float max = -1e9;
+  for (int i = 0; i < q->last_nof_antennas; ++i) {
+    float v = srslte_chest_dl_get_rsrp_neighbour_port(q, i);
+    if (v > max) {
+      max = v;
+    }
+  }
+  return max;
+}

lib/src/phy/ch_estimation/test/chest_test_dl.c

@@ -173,7 +173,7 @@ int main(int argc, char **argv) {
         
         gettimeofday(&t[1], NULL);
         for (int j=0;j<100;j++) {
-          srslte_predecoding_single(input, ce, output, num_re, 1.0f, 0);
+          srslte_predecoding_single(input, ce, output, NULL, num_re, 1.0f, 0);
         }
         gettimeofday(&t[2], NULL);
         get_time_interval(t);
@@ -188,7 +188,7 @@ int main(int argc, char **argv) {
 
         gettimeofday(&t[1], NULL);
         for (int j=0;j<100;j++) {
-          srslte_predecoding_single(input, ce, output, num_re, 1.0f, srslte_chest_dl_get_noise_estimate(&est));
+          srslte_predecoding_single(input, ce, output, NULL, num_re, 1.0f, srslte_chest_dl_get_noise_estimate(&est));
         }
         gettimeofday(&t[2], NULL);
         get_time_interval(t);

lib/src/phy/fec/softbuffer.c

@@ -47,32 +47,56 @@ int srslte_softbuffer_rx_init(srslte_softbuffer_rx_t *q, uint32_t nof_prb) {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   
   if (q != NULL) {    
-    ret = SRSLTE_ERROR; 
-    
     bzero(q, sizeof(srslte_softbuffer_rx_t));
     
     ret = srslte_ra_tbs_from_idx(26, nof_prb);
     if (ret != SRSLTE_ERROR) {
       q->max_cb =  (uint32_t) ret / (SRSLTE_TCOD_MAX_LEN_CB - 24) + 1; 
+      ret = SRSLTE_ERROR;
       
       q->buffer_f = srslte_vec_malloc(sizeof(int16_t*) * q->max_cb);
       if (!q->buffer_f) {
         perror("malloc");
-        return SRSLTE_ERROR;
+        goto clean_exit;
+      }
+      
+      q->data = srslte_vec_malloc(sizeof(uint8_t*) * q->max_cb);
+      if (!q->data) {
+        perror("malloc");
+        goto clean_exit;
+      }
+
+      q->cb_crc = srslte_vec_malloc(sizeof(bool) * q->max_cb);
+      if (!q->cb_crc) {
+        perror("malloc");
+        goto clean_exit;
       }
+      bzero(q->cb_crc, sizeof(bool) * q->max_cb);
 
       // FIXME: Use HARQ buffer limitation based on UE category
       for (uint32_t i=0;i<q->max_cb;i++) {
         q->buffer_f[i] = srslte_vec_malloc(sizeof(int16_t) * SOFTBUFFER_SIZE);
         if (!q->buffer_f[i]) {
           perror("malloc");
-          return SRSLTE_ERROR;
+          goto clean_exit;
+        }
+
+        q->data[i] = srslte_vec_malloc(sizeof(uint8_t) * 6144/8);
+        if (!q->data[i]) {
+          perror("malloc");
+          goto clean_exit;
         }
       }      
       //srslte_softbuffer_rx_reset(q);
       ret = SRSLTE_SUCCESS;
     }
   }
+
+  clean_exit:
+  if (ret != SRSLTE_SUCCESS) {
+    srslte_softbuffer_rx_free(q);
+  }
+
   return ret;
 }
 
@@ -86,6 +110,17 @@ void srslte_softbuffer_rx_free(srslte_softbuffer_rx_t *q) {
       }
       free(q->buffer_f);
     }
+    if (q->data) {
+      for (uint32_t i=0;i<q->max_cb;i++) {
+        if (q->data[i]) {
+          free(q->data[i]);
+        }
+      }
+      free(q->data);
+    }
+    if (q->cb_crc) {
+      free(q->cb_crc);
+    }
     bzero(q, sizeof(srslte_softbuffer_rx_t));
   }
 }
@@ -110,6 +145,9 @@ void srslte_softbuffer_rx_reset_cb(srslte_softbuffer_rx_t *q, uint32_t nof_cb) {
       }
     }
   }
+  if (q->cb_crc) {
+    bzero(q->cb_crc, sizeof(bool) * q->max_cb);
+  }
 }
 
 

lib/src/phy/fec/test/turbocoder_test.c

@@ -71,8 +71,6 @@ int main(int argc, char **argv) {
 
   parse_args(argc, argv);
   
-  srslte_tcod_gentable();
-
   srslte_tcod_t tcod;
   srslte_tcod_init(&tcod, 6144);
 

lib/src/phy/fec/test/viterbi_test.c

@@ -279,6 +279,7 @@ int main(int argc, char **argv) {
   free(llr);
   free(llr_c);
   free(data_rx);
+  free(data_rx2);
   
   if (snr_points == 1) {
     int expected_errors = get_expected_errors(nof_frames, seed, frame_length, tail_biting, ebno_db);

lib/src/phy/mimo/precoding.c

@@ -34,6 +34,7 @@
 #include "srslte/phy/utils/vector.h"
 #include "srslte/phy/utils/debug.h"
 #include "srslte/phy/utils/mat.h"
+#include "srslte/phy/utils/simd.h"
 
 #ifdef LV_HAVE_SSE
 #include <immintrin.h>
@@ -252,8 +253,49 @@ int srslte_predecoding_single_gen(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_
   return nof_symbols;
 }
 
+int srslte_predecoding_single_csi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS], cf_t *x, float *csi, int nof_rxant, int nof_symbols, float scaling, float noise_estimate) {
+  int i = 0;
+
+#if SRSLTE_SIMD_CF_SIZE
+  const simd_f_t _noise = srslte_simd_f_set1(noise_estimate);
+  const simd_f_t _scaling = srslte_simd_f_set1(1.0f / scaling);
+
+  for (; i < nof_symbols - SRSLTE_SIMD_CF_SIZE + 1; i += SRSLTE_SIMD_CF_SIZE) {
+    simd_cf_t _r = srslte_simd_cf_zero();
+    simd_f_t _hh = srslte_simd_f_zero();
+
+    for (int p = 0; p < nof_rxant; p++) {
+      simd_cf_t _y = srslte_simd_cfi_load(&y[p][i]);
+      simd_cf_t _h = srslte_simd_cfi_load(&h[p][i]);
+
+      _r = srslte_simd_cf_add(_r, srslte_simd_cf_conjprod(_y, _h));
+      _hh = srslte_simd_f_add(_hh, srslte_simd_cf_re(srslte_simd_cf_conjprod(_h, _h)));
+    }
+
+    simd_f_t _csi = srslte_simd_f_add(_hh, _noise);
+    simd_cf_t _x = srslte_simd_cf_mul(srslte_simd_cf_mul(_r, _scaling), srslte_simd_f_rcp(_csi));
+
+    srslte_simd_f_store(&csi[i], _csi);
+    srslte_simd_cfi_store(&x[i], _x);
+  }
+#endif
+
+  for (; i < nof_symbols; i++) {
+    cf_t r = 0;
+    float hh = 0;
+    float _scaling = 1.0f / scaling;
+    for (int p = 0; p < nof_rxant; p++) {
+      r += y[p][i] * conj(h[p][i]);
+      hh += (__real__ h[p][i] * __real__ h[p][i]) + (__imag__ h[p][i] * __imag__ h[p][i]);
+    }
+    csi[i] = hh + noise_estimate;
+    x[i] = r * _scaling / csi[i];
+  }
+  return nof_symbols;
+}
+
 /* ZF/MMSE SISO equalizer x=y(h'h+no)^(-1)h' (ZF if n0=0.0)*/
-int srslte_predecoding_single(cf_t *y_, cf_t *h_, cf_t *x, int nof_symbols, float scaling, float noise_estimate) {
+int srslte_predecoding_single(cf_t *y_, cf_t *h_, cf_t *x, float *csi, int nof_symbols, float scaling, float noise_estimate) {
   
   cf_t *y[SRSLTE_MAX_PORTS]; 
   cf_t *h[SRSLTE_MAX_PORTS];
@@ -261,6 +303,10 @@ int srslte_predecoding_single(cf_t *y_, cf_t *h_, cf_t *x, int nof_symbols, floa
   h[0] = h_; 
   int nof_rxant = 1; 
   
+  if (csi) {
+    return srslte_predecoding_single_csi(y, h, x, csi, nof_rxant, nof_symbols, scaling, noise_estimate);
+  }
+
 #ifdef LV_HAVE_AVX
   if (nof_symbols > 32 && nof_rxant <= 2) {
     return srslte_predecoding_single_avx(y, h, x, nof_rxant, nof_symbols, scaling, noise_estimate);
@@ -281,8 +327,12 @@ int srslte_predecoding_single(cf_t *y_, cf_t *h_, cf_t *x, int nof_symbols, floa
 }
 
 /* ZF/MMSE SISO equalizer x=y(h'h+no)^(-1)h' (ZF if n0=0.0)*/
-int srslte_predecoding_single_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS], cf_t *x,
+int srslte_predecoding_single_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS], cf_t *x, float *csi,
                                     int nof_rxant, int nof_symbols, float scaling, float noise_estimate) {
+  if (csi) {
+    return srslte_predecoding_single_csi(y, h, x, csi, nof_rxant, nof_symbols, scaling, noise_estimate);
+  }
+
 #ifdef LV_HAVE_AVX
   if (nof_symbols > 32) {
     return srslte_predecoding_single_avx(y, h, x, nof_rxant, nof_symbols, scaling, noise_estimate);
@@ -1418,7 +1468,7 @@ void srslte_predecoding_set_mimo_decoder (srslte_mimo_decoder_t _mimo_decoder) {
 
 /* 36.211 v10.3.0 Section 6.3.4 */
 int srslte_predecoding_type(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
-                            cf_t *x[SRSLTE_MAX_LAYERS], int nof_rxant, int nof_ports, int nof_layers,
+                            cf_t *x[SRSLTE_MAX_LAYERS], float *csi, int nof_rxant, int nof_ports, int nof_layers,
                             int codebook_idx, int nof_symbols, srslte_mimo_type_t type, float scaling,
                             float noise_estimate) {
 
@@ -1451,7 +1501,7 @@ int srslte_predecoding_type(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS]
     return -1; 
   case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
     if (nof_ports == 1 && nof_layers == 1) {
-      return srslte_predecoding_single_multi(y, h[0], x[0], nof_rxant, nof_symbols, scaling, noise_estimate);
+      return srslte_predecoding_single_multi(y, h[0], x[0], csi, nof_rxant, nof_symbols, scaling, noise_estimate);
     } else {
       fprintf(stderr,
           "Number of ports and layers must be 1 for transmission on single antenna ports (%d, %d)\n", nof_ports, nof_layers);

lib/src/phy/mimo/test/precoder_test.c

@@ -291,7 +291,7 @@ int main(int argc, char **argv) {
   /* predecoding / equalization */
   struct timeval t[3];
   gettimeofday(&t[1], NULL);
-  srslte_predecoding_type(r, h, xr, nof_rx_ports, nof_tx_ports, nof_layers,
+  srslte_predecoding_type(r, h, xr, NULL, nof_rx_ports, nof_tx_ports, nof_layers,
                           codebook_idx, nof_re, type, scaling, powf(10, -snr_db / 10));
   gettimeofday(&t[2], NULL);
   get_time_interval(t);

lib/src/phy/modem/test/modem_test.c

@@ -197,6 +197,7 @@ int main(int argc, char **argv) {
     }
   }
 
+  free(llr2);
   free(llr);
   free(symbols);
   free(symbols_bytes);

lib/src/phy/phch/pbch.c

@@ -400,6 +400,7 @@ int decode_frame(srslte_pbch_t *q, uint32_t src, uint32_t dst, uint32_t n,
     uint32_t nof_bits, uint32_t nof_ports) {
   int j;
 
+  if (dst + n <= 4 && src + n <= 4) {
     memcpy(&q->temp[dst * nof_bits], &q->llr[src * nof_bits],
            n * nof_bits * sizeof(float));
 
@@ -428,6 +429,11 @@ int decode_frame(srslte_pbch_t *q, uint32_t src, uint32_t dst, uint32_t n,
     } else {
       return SRSLTE_SUCCESS;
     }
+  } else {
+    fprintf(stderr, "Error in PBCH decoder: Invalid frame pointers dst=%d, src=%d, n=%d\n", src, dst, n);
+    return -1;
+  }
+
 }
 
 /* Decodes the PBCH channel
@@ -483,6 +489,8 @@ int srslte_pbch_decode(srslte_pbch_t *q, cf_t *slot1_symbols, cf_t *ce_slot1[SRS
     q->frame_idx++;
     ret = 0;
 
+    uint32_t frame_idx = q->frame_idx;
+
     /* Try decoding for 1 to cell.nof_ports antennas */
     if (q->search_all_ports) {
       nant = 1; 
@@ -492,12 +500,12 @@ int srslte_pbch_decode(srslte_pbch_t *q, cf_t *slot1_symbols, cf_t *ce_slot1[SRS
 
     do {
       if (nant != 3) {
-        DEBUG("Trying %d TX antennas with %d frames\n", nant, q->frame_idx);
+        DEBUG("Trying %d TX antennas with %d frames\n", nant, frame_idx);
 
         /* in control channels, only diversity is supported */
         if (nant == 1) {
           /* no need for layer demapping */
-          srslte_predecoding_single(q->symbols[0], q->ce[0], q->d, q->nof_symbols, 1.0f, noise_estimate);
+          srslte_predecoding_single(q->symbols[0], q->ce[0], q->d, NULL, q->nof_symbols, 1.0f, noise_estimate);
         } else {
           srslte_predecoding_diversity(q->symbols[0], q->ce, x, nant,
                                        q->nof_symbols, 1.0f);
@@ -505,19 +513,19 @@ int srslte_pbch_decode(srslte_pbch_t *q, cf_t *slot1_symbols, cf_t *ce_slot1[SRS
         }
 
         /* demodulate symbols */
-        srslte_demod_soft_demodulate(SRSLTE_MOD_QPSK, q->d, &q->llr[nof_bits * (q->frame_idx - 1)], q->nof_symbols);
+        srslte_demod_soft_demodulate(SRSLTE_MOD_QPSK, q->d, &q->llr[nof_bits * (frame_idx - 1)], q->nof_symbols);
         
         /* We don't know where the 40 ms begin, so we try all combinations. E.g. if we received
         * 4 frames, try 1,2,3,4 individually, 12, 23, 34 in pairs, 123, 234 and finally 1234.
         * We know they are ordered.
         */
-        for (nb = 0; nb < q->frame_idx; nb++) {
+        for (nb = 0; nb < frame_idx; nb++) {
           for (dst = 0; (dst < 4 - nb); dst++) {
-            for (src = 0; src < q->frame_idx - nb; src++) {
+            for (src = 0; src < frame_idx - nb; src++) {
               ret = decode_frame(q, src, dst, nb + 1, nof_bits, nant);
               if (ret == 1) {
                 if (sfn_offset) {
-                  *sfn_offset = (int) dst - src + q->frame_idx - 1;
+                  *sfn_offset = (int) dst - src + frame_idx - 1;
                 }
                 if (nof_tx_ports) {
                   *nof_tx_ports = nant; 
@@ -525,7 +533,8 @@ int srslte_pbch_decode(srslte_pbch_t *q, cf_t *slot1_symbols, cf_t *ce_slot1[SRS
                 if (bch_payload) {
                   memcpy(bch_payload, q->data, sizeof(uint8_t) * SRSLTE_BCH_PAYLOAD_LEN);      
                 }
-                INFO("Decoded PBCH: src=%d, dst=%d, nb=%d, sfn_offset=%d\n", src, dst, nb+1, (int) dst - src + q->frame_idx - 1);
+                INFO("Decoded PBCH: src=%d, dst=%d, nb=%d, sfn_offset=%d\n", src, dst, nb+1, (int) dst - src + frame_idx - 1);
+                srslte_pbch_decode_reset(q);
                 return 1;
               }
             }

lib/src/phy/phch/pcfich.c

@@ -219,7 +219,7 @@ int srslte_pcfich_decode_multi(srslte_pcfich_t *q, cf_t *sf_symbols[SRSLTE_MAX_P
     /* in control channels, only diversity is supported */
     if (q->cell.nof_ports == 1) {
       /* no need for layer demapping */
-      srslte_predecoding_single_multi(q_symbols, q_ce[0], q->d, q->nof_rx_antennas, q->nof_symbols, 1.0f, noise_estimate);
+      srslte_predecoding_single_multi(q_symbols, q_ce[0], q->d, NULL, q->nof_rx_antennas, q->nof_symbols, 1.0f, noise_estimate);
     } else {
       srslte_predecoding_diversity_multi(q_symbols, q_ce, x, q->nof_rx_antennas, q->cell.nof_ports, q->nof_symbols, 1.0f);
       srslte_layerdemap_diversity(x, q->d, q->cell.nof_ports, q->nof_symbols / q->cell.nof_ports);

lib/src/phy/phch/pdcch.c

@@ -490,7 +490,7 @@ int srslte_pdcch_extract_llr_multi(srslte_pdcch_t *q, cf_t *sf_symbols[SRSLTE_MA
     /* in control channels, only diversity is supported */
     if (q->cell.nof_ports == 1) {
       /* no need for layer demapping */
-      srslte_predecoding_single_multi(q->symbols, q->ce[0], q->d, q->nof_rx_antennas, nof_symbols, 1.0f, noise_estimate/2);
+      srslte_predecoding_single_multi(q->symbols, q->ce[0], q->d, NULL, q->nof_rx_antennas, nof_symbols, 1.0f, noise_estimate/2);
     } else {
       srslte_predecoding_diversity_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, nof_symbols, 1.0f);
       srslte_layerdemap_diversity(x, q->d, q->cell.nof_ports, nof_symbols / q->cell.nof_ports);

lib/src/phy/phch/pdsch.c

@@ -294,6 +294,10 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
     if (q->d[i]) {
       free(q->d[i]);
     }
+
+    if (q->csi[i]) {
+      free(q->csi[i]);
+    }
   }
 
   /* Free sch objects */
@@ -394,6 +398,22 @@ void srslte_pdsch_set_power_allocation(srslte_pdsch_t *q, float rho_a) {
   }
 }
 
+int srslte_pdsch_enable_csi(srslte_pdsch_t *q, bool enable) {
+  if (enable) {
+    for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+      if (!q->csi[i]) {
+        q->csi[i] = srslte_vec_malloc(sizeof(float) * q->max_re);
+        if (!q->csi[i]) {
+          return SRSLTE_ERROR;
+        }
+      }
+    }
+  }
+  q->csi_enabled = enable;
+
+  return SRSLTE_SUCCESS;
+}
+
 void srslte_pdsch_free_rnti(srslte_pdsch_t* q, uint16_t rnti)
 {
   uint32_t rnti_idx = q->is_ue?0:rnti;
@@ -617,6 +637,41 @@ static int srslte_pdsch_codeword_decode(srslte_pdsch_t *q, srslte_pdsch_cfg_t *c
     /* Bit scrambling */
     srslte_scrambling_s_offset(seq, q->e[codeword_idx], 0, nbits->nof_bits);
 
+    uint32_t qm = nbits->nof_bits/nbits->nof_re;
+    switch(cfg->grant.mcs[tb_idx].mod) {
+
+      case SRSLTE_MOD_BPSK:
+        qm = 1;
+        break;
+      case SRSLTE_MOD_QPSK:
+        qm = 2;
+        break;
+      case SRSLTE_MOD_16QAM:
+        qm = 4;
+        break;
+      case SRSLTE_MOD_64QAM:
+        qm = 6;
+        break;
+      default:
+        ERROR("No modulation");
+    }
+
+    int16_t *e = q->e[codeword_idx];
+
+    if (q->csi_enabled) {
+      const uint32_t csi_max_idx = srslte_vec_max_fi(q->csi[codeword_idx], nbits->nof_bits / qm);
+      float csi_max = 1.0f;
+      if (csi_max_idx < nbits->nof_bits / qm) {
+        csi_max = q->csi[codeword_idx][csi_max_idx];
+      }
+      for (int i = 0; i < nbits->nof_bits / qm; i++) {
+        const float csi = q->csi[codeword_idx][i] / csi_max;
+        for (int k = 0; k < qm; k++) {
+          e[qm * i + k] = (int16_t) ((float) e[qm * i + k] * csi);
+        }
+      }
+    }
+
     /* Return  */
     ret = srslte_dlsch_decode2(&q->dl_sch, cfg, softbuffer, q->e[codeword_idx], data, tb_idx);
 
@@ -702,7 +757,7 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
     }
 
     // Pre-decoder
-    if (srslte_predecoding_type(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
+    if (srslte_predecoding_type(q->symbols, q->ce, x, q->csi[0], q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
                                       cfg->codebook_idx, cfg->nbits[0].nof_re, cfg->mimo_type, pdsch_scaling, noise_estimate)<0) {
       DEBUG("Error predecoding\n");
       return SRSLTE_ERROR;

lib/src/phy/phch/phich.c

@@ -239,7 +239,7 @@ int srslte_phich_decode(srslte_phich_t *q, cf_t *sf_symbols[SRSLTE_MAX_PORTS],
   /* in control channels, only diversity is supported */
   if (q->cell.nof_ports == 1) {
     /* no need for layer demapping */
-    srslte_predecoding_single_multi(q_sf_symbols, q_ce[0], q->d0, q->nof_rx_antennas, SRSLTE_PHICH_MAX_NSYMB, 1.0f, noise_estimate);
+    srslte_predecoding_single_multi(q_sf_symbols, q_ce[0], q->d0, NULL, q->nof_rx_antennas, SRSLTE_PHICH_MAX_NSYMB, 1.0f, noise_estimate);
   } else {
     srslte_predecoding_diversity_multi(q_sf_symbols, q_ce, x, q->nof_rx_antennas, q->cell.nof_ports, SRSLTE_PHICH_MAX_NSYMB, 1.0f);
     srslte_layerdemap_diversity(x, q->d0, q->cell.nof_ports, SRSLTE_PHICH_MAX_NSYMB / q->cell.nof_ports);

lib/src/phy/phch/pmch.c

@@ -152,7 +152,6 @@ int srslte_pmch_init(srslte_pmch_t *q, uint32_t max_prb)
 int srslte_pmch_init_multi(srslte_pmch_t *q, uint32_t max_prb, uint32_t nof_rx_antennas)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
-  int i;
 
   if (q != NULL                  &&
      nof_rx_antennas <= SRSLTE_MAX_PORTS) 
@@ -169,7 +168,7 @@ int srslte_pmch_init_multi(srslte_pmch_t *q, uint32_t max_prb, uint32_t nof_rx_a
     INFO("Init PMCH: %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;
       }
@@ -189,7 +188,7 @@ int srslte_pmch_init_multi(srslte_pmch_t *q, uint32_t max_prb, uint32_t nof_rx_a
       goto clean;
     }
 
-    for (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_re);
       if (!q->x[i]) {
         goto clean;
@@ -232,7 +231,7 @@ void srslte_pmch_free(srslte_pmch_t *q) {
   if (q->d) {
     free(q->d);
   }
-  for (i = 0; i < q->cell.nof_ports; i++) {
+  for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
     if (q->x[i]) {
       free(q->x[i]);
     }
@@ -378,7 +377,7 @@ int srslte_pmch_decode_multi(srslte_pmch_t *q,
     }
      
     // No tx diversity in MBSFN
-    srslte_predecoding_single_multi(q->symbols, q->ce[0], q->d, q->nof_rx_antennas, cfg->nbits[0].nof_re, 1.0f, noise_estimate);
+    srslte_predecoding_single_multi(q->symbols, q->ce[0], q->d, NULL, q->nof_rx_antennas, cfg->nbits[0].nof_re, 1.0f, noise_estimate);
     
     if (SRSLTE_VERBOSE_ISDEBUG()) {
       DEBUG("SAVED FILE subframe.dat: received subframe symbols\n");

lib/src/phy/phch/pucch.c

@@ -787,7 +787,7 @@ int srslte_pucch_decode(srslte_pucch_t* q, srslte_pucch_format_t format,
     }
     
     // Equalization
-    srslte_predecoding_single(q->z_tmp, q->ce, q->z, nof_re, 1.0f, noise_estimate);
+    srslte_predecoding_single(q->z_tmp, q->ce, q->z, NULL, nof_re, 1.0f, noise_estimate);
 
     // Perform ML-decoding 
     float corr=0, corr_max=-1e9;

lib/src/phy/phch/pusch.c

@@ -596,7 +596,7 @@ int srslte_pusch_decode(srslte_pusch_t *q,
     }
 
     // Equalization
-    srslte_predecoding_single(q->d, q->ce, q->z, cfg->nbits.nof_re, 1.0f, noise_estimate);
+    srslte_predecoding_single(q->d, q->ce, q->z, NULL, cfg->nbits.nof_re, 1.0f, noise_estimate);
     
     // DFT predecoding
     srslte_dft_precoding(&q->dft_precoding, q->z, q->d, cfg->grant.L_prb, cfg->nbits.nof_symb);

lib/src/phy/phch/sch.c

@@ -336,14 +336,17 @@ bool decode_tb_cb(srslte_sch_t *q,
     decoder_input[i] = NULL;
   }
   
+  uint32_t remaining_cb = 0;
   for (int i=0;i<nof_cb;i++) {
-    cb_map[i] = false; 
+    /* Do not process blocks with CRC Ok */
+    cb_map[i] = softbuffer->cb_crc[i];
+    if (softbuffer->cb_crc[i] == false) {
+      remaining_cb ++;
+    }
   }
     
   srslte_tdec_reset(&q->decoder, cb_len);
   
-  uint32_t remaining_cb = nof_cb;
-
   q->nof_iterations = 0;
 
   while(remaining_cb>0) {
@@ -401,7 +404,8 @@ bool decode_tb_cb(srslte_sch_t *q,
         // CRC is OK
         if (!srslte_crc_checksum_byte(crc_ptr, q->cb_in, len_crc)) {
 
-          memcpy(&data[(cb_idx[i]*rlen)/8], q->cb_in, rlen/8 * sizeof(uint8_t));
+          memcpy(softbuffer->data[cb_idx[i]], q->cb_in, rlen/8 * sizeof(uint8_t));
+          softbuffer->cb_crc[cb_idx[i]] = true;
 
           q->nof_iterations += srslte_tdec_get_nof_iterations_cb(&q->decoder, i);
 
@@ -418,15 +422,28 @@ bool decode_tb_cb(srslte_sch_t *q,
                 cb_idx[i], remaining_cb, i, first_cb, nof_cb);
 
           q->nof_iterations += q->max_iterations;
-          q->nof_iterations /= (nof_cb-remaining_cb+1);
+          srslte_tdec_reset_cb(&q->decoder, i);
-          return false;
+          remaining_cb--;
+          decoder_input[i] = NULL;
+          cb_idx[i] = 0;
+        }
+      }
     }    
   }
+
+  softbuffer->tb_crc = true;
+  for (int i = 0; i < nof_cb && softbuffer->tb_crc; i++) {
+    /* If one CB failed return false */
+    softbuffer->tb_crc = softbuffer->cb_crc[i];
+  }
+  if (softbuffer->tb_crc) {
+    for (int i = 0; i < nof_cb; i++) {
+      memcpy(&data[i * rlen / 8], softbuffer->data[i], rlen/8 * sizeof(uint8_t));
     }
   }
 
   q->nof_iterations /= nof_cb;
-  return true;  
+  return softbuffer->tb_crc;
 }
 
 /**

lib/src/phy/phch/test/pmch_test.c

@@ -157,6 +157,7 @@ int main(int argc, char **argv) {
   bzero(ce, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
   bzero(tx_slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
   bzero(rx_slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
+  bzero(t, 3 * sizeof(struct timeval));
 
   cell.nof_ports = 1;
 
@@ -469,5 +470,8 @@ quit:
   } else {
     printf("Ok\n");
   }
+
+  srslte_dft_exit();
+
   exit(ret);
 }

lib/src/phy/rf/rf_uhd_imp.c

@@ -80,8 +80,8 @@ static void log_overflow(rf_uhd_handler_t *h) {
 static void log_late(rf_uhd_handler_t *h, bool is_rx) {
   if (h->uhd_error_handler) {
     srslte_rf_error_t error;
-    error.opt = is_rx?1:0;
     bzero(&error, sizeof(srslte_rf_error_t));
+    error.opt = is_rx?1:0;
     error.type = SRSLTE_RF_ERROR_LATE;
     h->uhd_error_handler(error);
   }

lib/src/phy/sync/sync.c

@@ -152,21 +152,19 @@ clean_exit:
 void srslte_sync_free(srslte_sync_t *q)
 {
   if (q) {
-
     srslte_pss_free(&q->pss);
     srslte_sss_free(&q->sss);
     srslte_cfo_free(&q->cfo_corr_frame);
     srslte_cfo_free(&q->cfo_corr_symbol);
     srslte_cp_synch_free(&q->cp_synch);
 
-    if (q->cfo_i_initiated) {
+    for (int i = 0; i < 2; i++) {
-      for (int i=0;i<2;i++) {
       if (q->cfo_i_corr[i]) {
         free(q->cfo_i_corr[i]);
       }
       srslte_pss_free(&q->pss_i[i]);
     }
-    }
+
     if (q->temp) {
       free(q->temp);
     }

lib/src/phy/ue/ue_dl.c

@@ -71,7 +71,6 @@ int srslte_ue_dl_init(srslte_ue_dl_t *q,
     q->pmch_pkt_errors = 0;
     q->pmch_pkts_total = 0;
     q->pending_ul_dci_rnti = 0; 
-    q->sample_offset = 0; 
     q->nof_rx_antennas = nof_rx_antennas;
 
     for (int j = 0; j < SRSLTE_MAX_PORTS; j++) {
@@ -147,11 +146,6 @@ int srslte_ue_dl_init(srslte_ue_dl_t *q,
         goto clean_exit;
       }
     }
-    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[0].symbol_sz);
 
     ret = SRSLTE_SUCCESS;
   } else {
@@ -178,7 +172,6 @@ void srslte_ue_dl_free(srslte_ue_dl_t *q) {
     srslte_pdcch_free(&q->pdcch);
     srslte_pdsch_free(&q->pdsch);
     srslte_pmch_free(&q->pmch);
-    srslte_cfo_free(&q->sfo_correct);
     for (int i = 0; i < SRSLTE_MAX_TB; i++) {
       srslte_softbuffer_rx_free(q->softbuffers[i]);
       if (q->softbuffers[i]) {
@@ -209,7 +202,6 @@ int srslte_ue_dl_set_cell(srslte_ue_dl_t *q, srslte_cell_t 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) {
@@ -220,11 +212,6 @@ int srslte_ue_dl_set_cell(srslte_ue_dl_t *q, srslte_cell_t 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-5f/q->fft[0].symbol_sz);
       for (int port = 0; port < q->nof_rx_antennas; port++) {
         if (srslte_ofdm_rx_set_prb(&q->fft[port], q->cell.cp, q->cell.nof_prb)) {
           fprintf(stderr, "Error resizing FFT\n");
@@ -348,10 +335,6 @@ void srslte_ue_dl_reset(srslte_ue_dl_t *q) {
   bzero(&q->pdsch_cfg, sizeof(srslte_pdsch_cfg_t));
 }
 
-void srslte_ue_dl_set_sample_offset(srslte_ue_dl_t * q, float sample_offset) {
-  q->sample_offset = sample_offset; 
-}
-
 /** Applies the following operations to a subframe of synchronized samples: 
  *    - OFDM demodulation
  *    - Channel estimation 
@@ -395,17 +378,6 @@ int srslte_ue_dl_decode_fft_estimate_noguru(srslte_ue_dl_t *q, cf_t *input[SRSLT
     /* Run FFT for all subframe data */
     for (int j=0;j<q->nof_rx_antennas;j++) {
       srslte_ofdm_rx_sf_ng(&q->fft[j], input[j], q->sf_symbols_m[j]);
-
-      /* Correct SFO multiplying by complex exponential in the time domain */
-      if (q->sample_offset) {
-        int nsym = SRSLTE_CP_NSYMB(q->cell.cp);
-        for (int i=0;i<2*nsym;i++) {
-          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[j].symbol_sz);
-        }
-      }
     }
     return srslte_ue_dl_decode_estimate_mbsfn(q, sf_idx, cfi, SRSLTE_SF_NORM);
   } else {

lib/src/phy/utils/ringbuffer.c

@@ -13,10 +13,9 @@ int srslte_ringbuffer_init(srslte_ringbuffer_t *q, int capacity)
   }
   q->active     = true;
   q->capacity   = capacity;
-  srslte_ringbuffer_reset(q);
-  
   pthread_mutex_init(&q->mutex, NULL); 
   pthread_cond_init(&q->cvar, NULL);
+  srslte_ringbuffer_reset(q);
 
   return 0; 
 }
@@ -48,6 +47,11 @@ int srslte_ringbuffer_status(srslte_ringbuffer_t *q)
   return q->count;
 }
 
+int srslte_ringbuffer_space(srslte_ringbuffer_t *q)
+{
+  return q->capacity - q->count;
+}
+
 int srslte_ringbuffer_write(srslte_ringbuffer_t *q, void *p, int nof_bytes)
 {
   uint8_t *ptr = (uint8_t*) p;

lib/src/phy/utils/test/vector_test.c

@@ -265,6 +265,7 @@ TEST(srslte_vec_sum_fff,
 
          free(x);
          free(y);
+         free(z);
 )
 
 TEST(srslte_vec_sub_fff,
@@ -287,6 +288,7 @@ TEST(srslte_vec_sub_fff,
 
          free(x);
          free(y);
+         free(z);
 )
 
 TEST(srslte_vec_dot_prod_ccc,
@@ -354,6 +356,7 @@ TEST(srslte_vec_prod_ccc,
   }
 
   free(x);
+  free(y);
   free(z);
 )
 
@@ -407,6 +410,7 @@ TEST(srslte_vec_prod_conj_ccc,
   }
 
   free(x);
+  free(y);
   free(z);
 )
 

lib/src/phy/utils/vector.c

@@ -200,10 +200,16 @@ void srslte_vec_fprint_hex(FILE *stream, uint8_t *x, const uint32_t len) {
   fprintf(stream, "];\n");
 }
 
-void srslte_vec_sprint_hex(char *str, uint8_t *x, const uint32_t len) {
+void srslte_vec_sprint_hex(char *str, const uint32_t max_str_len, uint8_t *x, const uint32_t len) {
   uint32_t i, nbytes; 
   uint8_t byte;
   nbytes = len/8;
+  // check that hex string fits in buffer (every byte takes 3 characters, plus brackets)
+  if ((3*(len/8 + ((len%8)?1:0))) + 2 >= max_str_len) {
+    fprintf(stderr, "Buffer too small for printing hex string (max_str_len=%d, payload_len=%d).\n", max_str_len, len);
+    return;
+  }
+
   int n=0;
   n+=sprintf(&str[n], "[");
   for (i=0;i<nbytes;i++) {
@@ -215,6 +221,7 @@ void srslte_vec_sprint_hex(char *str, uint8_t *x, const uint32_t len) {
     n+=sprintf(&str[n], "%02x ", byte);
   }
   n+=sprintf(&str[n], "]");
+  str[max_str_len-1] = 0;
 }
 
 void srslte_vec_save_file(char *filename, const void *buffer, const uint32_t len) {

lib/src/radio/radio.cc

@@ -301,9 +301,7 @@ void radio::set_master_clock_rate(double rate)
 
 void radio::set_rx_srate(double srate)
 {
-  srslte_rf_stop_rx_stream(&rf_device);
   srslte_rf_set_rx_srate(&rf_device, srate);
-  srslte_rf_start_rx_stream(&rf_device, false);
 }
 
 void radio::set_tx_freq(double freq)

lib/src/upper/rlc_am.cc

@@ -198,7 +198,7 @@ uint32_t rlc_am::get_bearer()
 void rlc_am::write_sdu(byte_buffer_t *sdu)
 {
   tx_sdu_queue.write(sdu);
-  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU, tx_sdu_len=%d", rrc->get_rb_name(lcid).c_str(), tx_sdu_queue.size());
+  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU (%d B, tx_sdu_queue_len=%d)", rrc->get_rb_name(lcid).c_str(), sdu->N_bytes, tx_sdu_queue.size());
 }
 
 /****************************************************************************
@@ -273,6 +273,27 @@ uint32_t rlc_am::get_buffer_state()
     goto unlock_and_return;
   }
 
+  // check if pollRetx timer expired (Section 5.2.2.3 in TS 36.322)
+  if (poll_retx()) {
+    // if both tx and retx buffer are empty, retransmit next PDU to be ack'ed
+    log->info("Poll reTx timer expired (lcid=%d)\n", lcid);
+    if ((tx_window.size() > 0 && retx_queue.size() == 0 && tx_sdu_queue.size() == 0)) {
+      std::map<uint32_t, rlc_amd_tx_pdu_t>::iterator it = tx_window.find(vt_s - 1);
+      if (it != tx_window.end()) {
+        log->info("Schedule last PDU (SN=%d) for reTx.\n", vt_s - 1);
+        rlc_amd_retx_t retx;
+        retx.is_segment = false;
+        retx.so_start = 0;
+        retx.so_end = tx_window[vt_s - 1].buf->N_bytes;
+        retx.sn = vt_s - 1;
+        retx_queue.push_back(retx);
+      } else {
+        log->error("Found invalid PDU in tx_window.\n");
+      }
+      poll_retx_timeout.start(cfg.t_poll_retx);
+    }
+  }
+
   // Bytes needed for retx
   if(retx_queue.size() > 0) {
     rlc_amd_retx_t retx = retx_queue.front();
@@ -827,7 +848,7 @@ int  rlc_am::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
   // Set SN
   header.sn = vt_s;
   vt_s = (vt_s + 1)%MOD;
-  log->info("%s PDU scheduled for tx. SN: %d\n", rrc->get_rb_name(lcid).c_str(), header.sn);
+  log->info("%s PDU scheduled for tx. SN: %d (%d B)\n", rrc->get_rb_name(lcid).c_str(), header.sn, pdu->N_bytes);
 
   // Place PDU in tx_window, write header and TX
   tx_window[header.sn].buf        = pdu;

lib/src/upper/rlc_tm.cc

@@ -125,10 +125,14 @@ int rlc_tm::read_pdu(uint8_t *payload, uint32_t nof_bytes)
 void rlc_tm::write_pdu(uint8_t *payload, uint32_t nof_bytes)
 {
   byte_buffer_t *buf = pool_allocate;
+  if (buf) {
     memcpy(buf->msg, payload, nof_bytes);
     buf->N_bytes = nof_bytes;
     buf->set_timestamp();
     pdcp->write_pdu(lcid, buf);
+  } else {
+    log->error("Fatal Error: Couldn't allocate buffer in rlc_tm::write_pdu().\n");
+  }
 }
 
 } // namespace srsue

lib/src/upper/rlc_um.cc

@@ -173,7 +173,7 @@ uint32_t rlc_um::get_bearer()
 void rlc_um::write_sdu(byte_buffer_t *sdu)
 {
   tx_sdu_queue.write(sdu);
-  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU, tx_sdu_len=%d", rrc->get_rb_name(lcid).c_str(), tx_sdu_queue.size());
+  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU (% B ,tx_sdu_queue_len=%d)", rrc->get_rb_name(lcid).c_str(), sdu->N_bytes, tx_sdu_queue.size());
 }
 
 /****************************************************************************
@@ -448,8 +448,13 @@ void rlc_um::handle_data_pdu(uint8_t *payload, uint32_t nof_bytes)
 
 void rlc_um::reassemble_rx_sdus()
 {
-  if(!rx_sdu)
+  if(!rx_sdu) {
     rx_sdu = pool_allocate;
+    if (!rx_sdu) {
+      log->error("Fatal Error: Couldn't allocate buffer in rlc_um::reassemble_rx_sdus().\n");
+      return;
+    }
+  }
 
   // First catch up with lower edge of reordering window
   while(!inside_reordering_window(vr_ur))
@@ -474,6 +479,10 @@ void rlc_um::reassemble_rx_sdus()
           rx_sdu->set_timestamp();
           pdcp->write_pdu(lcid, rx_sdu);
           rx_sdu = pool_allocate;
+          if (!rx_sdu) {
+            log->error("Fatal Error: Couldn't allocate buffer in rlc_um::reassemble_rx_sdus().\n");
+            return;
+          }
         }
         pdu_lost = false;
       }
@@ -494,6 +503,10 @@ void rlc_um::reassemble_rx_sdus()
           rx_sdu->set_timestamp();
           pdcp->write_pdu(lcid, rx_sdu);
           rx_sdu = pool_allocate;
+          if (!rx_sdu) {
+            log->error("Fatal Error: Couldn't allocate buffer in rlc_um::reassemble_rx_sdus().\n");
+            return;
+          }
         }
         pdu_lost = false;
       }
@@ -528,6 +541,10 @@ void rlc_um::reassemble_rx_sdus()
         rx_sdu->set_timestamp();
         pdcp->write_pdu(lcid, rx_sdu);
         rx_sdu = pool_allocate;
+        if (!rx_sdu) {
+          log->error("Fatal Error: Couldn't allocate buffer in rlc_um::reassemble_rx_sdus().\n");
+          return;
+        }
       }
       pdu_lost = false;
     }
@@ -557,6 +574,10 @@ void rlc_um::reassemble_rx_sdus()
         rx_sdu->set_timestamp();
         pdcp->write_pdu(lcid, rx_sdu);
         rx_sdu = pool_allocate;
+        if (!rx_sdu) {
+          log->error("Fatal Error: Couldn't allocate buffer in rlc_um::reassemble_rx_sdus().\n");
+          return;
+        }
       }
       pdu_lost = false;
     }

lib/test/common/timeout_test.cc

@@ -64,7 +64,43 @@ private:
   pthread_mutex_t   mutex; 
 };
 
-int main(int argc, char **argv) {
+
+int timer_thread_test()
+{
+  bool result;
+  uint32_t id       = 0;
+  uint32_t duration_msec = 5;
+  uint32_t result_tolerance = 1;
+
+  callback c;
+  timeout t;
+
+  gettimeofday(&c.start_time[1], NULL);
+  t.start(duration_msec);
+
+  while (t.is_running() && !t.expired()) {
+    printf("time to expire=%dms\n", t.get_msec_to_expire());
+    usleep(1000);
+  }
+
+  gettimeofday(&c.start_time[2], NULL);
+  get_time_interval(c.start_time);
+  uint32_t diff_ms = c.start_time[0].tv_usec*1e-3;
+  printf("Target duration: %dms, started: %ld:%ld, ended: %ld:%ld, actual duration %dms\n",
+         duration_msec, c.start_time[1].tv_sec, c.start_time[1].tv_usec, c.start_time[2].tv_sec, c.start_time[2].tv_usec, diff_ms);
+
+  result = ((duration_msec - result_tolerance) < diff_ms || diff_ms < (duration_msec + result_tolerance));
+
+  if(result) {
+    printf("Timer thread test passed\n");
+    return 0;
+  }else{
+    return -1;
+  }
+}
+
+int single_thread_test()
+{
   bool result;
   uint32_t id       = 0;
   uint32_t duration_msec = 5;
@@ -84,10 +120,25 @@ int main(int argc, char **argv) {
   result = (diff_ms == duration_msec);
 
   if(result) {
-    printf("Passed\n");
+    printf("Single thread test passed\n");
-    exit(0);
+    return 0;
   }else{
-    printf("Failed\n;");
+    return -1;
-    exit(1);
   }
 }
+
+
+int main(int argc, char **argv)
+{
+  if (single_thread_test()) {
+    printf("Single thread test failed.\n");
+    return -1;
+  }
+
+  if (timer_thread_test()) {
+    printf("Timer thread test failed.\n");
+    return -1;
+  }
+
+  return 0;
+}

lib/test/upper/rlc_am_stress_test.cc

@@ -46,6 +46,7 @@ typedef struct {
   uint32_t sdu_gen_delay_usec;
   uint32_t pdu_tx_delay_usec;
   bool     reestablish;
+  uint32_t log_level;
 } stress_test_args_t;
 
 void parse_args(stress_test_args_t *args, int argc, char *argv[]) {
@@ -64,7 +65,8 @@ void parse_args(stress_test_args_t *args, int argc, char *argv[]) {
   ("sdu_gen_delay", bpo::value<uint32_t>(&args->sdu_gen_delay_usec)->default_value(10), "SDU generation delay (usec)")
   ("pdu_tx_delay",  bpo::value<uint32_t>(&args->pdu_tx_delay_usec)->default_value(10), "Delay in MAC for transfering PDU from tx'ing RLC to rx'ing RLC (usec)")
   ("error_rate",    bpo::value<float>(&args->error_rate)->default_value(0.1), "Rate at which RLC PDUs are dropped")
-  ("reestablish",   bpo::value<bool>(&args->reestablish)->default_value(false), "Mimic RLC reestablish during execution");
+  ("reestablish",   bpo::value<bool>(&args->reestablish)->default_value(false), "Mimic RLC reestablish during execution")
+  ("loglevel",      bpo::value<uint32_t>(&args->log_level)->default_value(srslte::LOG_LEVEL_DEBUG), "Log level (1=Error,2=Warning,3=Info,4=Debug");
 
   // these options are allowed on the command line
   bpo::options_description cmdline_options;
@@ -81,6 +83,11 @@ void parse_args(stress_test_args_t *args, int argc, char *argv[]) {
     cout << common << endl << general << endl;
     exit(0);
   }
+
+  if (args->log_level > 4) {
+    args->log_level = 4;
+    printf("Set log level to %d (%s)\n", args->log_level, srslte::log_level_text[args->log_level]);
+  }
 }
 
 class mac_reader
@@ -260,8 +267,8 @@ void stress_test(stress_test_args_t args)
 {
   srslte::log_filter log1("RLC_AM_1");
   srslte::log_filter log2("RLC_AM_2");
-  log1.set_level(srslte::LOG_LEVEL_DEBUG);
+  log1.set_level((LOG_LEVEL_ENUM)args.log_level);
-  log2.set_level(srslte::LOG_LEVEL_DEBUG);
+  log2.set_level((LOG_LEVEL_ENUM)args.log_level);
   log1.set_hex_limit(-1);
   log2.set_hex_limit(-1);
 

srsenb/src/upper/gtpu.cc

@@ -195,6 +195,10 @@ void gtpu::rem_user(uint16_t rnti)
 void gtpu::run_thread()
 {
   byte_buffer_t *pdu = pool_allocate;
+  if (!pdu) {
+    gtpu_log->error("Fatal Error: Couldn't allocate buffer in gtpu::run_thread().\n");
+    return;
+  }
   run_enable = true;
 
   running=true; 

srsenb/src/upper/s1ap.cc

@@ -88,6 +88,10 @@ void s1ap::get_metrics(s1ap_metrics_t &m)
 void s1ap::run_thread()
 {
   srslte::byte_buffer_t *pdu = pool_allocate;
+  if (!pdu) {
+    s1ap_log->error("Fatal Error: Couldn't allocate buffer in s1ap::run_thread().\n");
+    return;
+  }
 
   uint32_t sz = SRSLTE_MAX_BUFFER_SIZE_BYTES - SRSLTE_BUFFER_HEADER_OFFSET;
   running = true;
@@ -514,10 +518,15 @@ bool s1ap::handle_dlnastransport(LIBLTE_S1AP_MESSAGE_DOWNLINKNASTRANSPORT_STRUCT
   }
 
   srslte::byte_buffer_t *pdu = pool_allocate;
+  if (pdu) {
     memcpy(pdu->msg, msg->NAS_PDU.buffer, msg->NAS_PDU.n_octets);
     pdu->N_bytes = msg->NAS_PDU.n_octets;
     rrc->write_dl_info(rnti, pdu);
     return true;
+  } else {
+    s1ap_log->error("Fatal Error: Couldn't allocate buffer in s1ap::run_thread().\n");
+    return false;
+  }
 }
 
 bool s1ap::handle_initialctxtsetuprequest(LIBLTE_S1AP_MESSAGE_INITIALCONTEXTSETUPREQUEST_STRUCT *msg)
@@ -850,6 +859,11 @@ bool s1ap::send_initial_ctxt_setup_response(uint16_t rnti, LIBLTE_S1AP_MESSAGE_I
     return false;
   }
   srslte::byte_buffer_t *buf = pool_allocate;
+  if (!buf) {
+    s1ap_log->error("Fatal Error: Couldn't allocate buffer in s1ap::send_initial_ctxt_setup_response().\n");
+    return false;
+  }
+
   LIBLTE_S1AP_S1AP_PDU_STRUCT tx_pdu;
 
   tx_pdu.ext          = false;
@@ -896,6 +910,11 @@ bool s1ap::send_erab_setup_response(uint16_t rnti, LIBLTE_S1AP_MESSAGE_E_RABSETU
     return false;
   }
   srslte::byte_buffer_t *buf = pool_allocate;
+  if (!buf) {
+    s1ap_log->error("Fatal Error: Couldn't allocate buffer in s1ap::send_erab_setup_response().\n");
+    return false;
+  }
+
   LIBLTE_S1AP_S1AP_PDU_STRUCT tx_pdu;
 
   tx_pdu.ext          = false;
@@ -942,6 +961,11 @@ bool s1ap::send_initial_ctxt_setup_failure(uint16_t rnti)
     return false;
   }
   srslte::byte_buffer_t *buf = pool_allocate;
+  if (!buf) {
+    s1ap_log->error("Fatal Error: Couldn't allocate buffer in s1ap::send_initial_ctxt_setup_failure().\n");
+    return false;
+  }
+
   LIBLTE_S1AP_S1AP_PDU_STRUCT tx_pdu;
   tx_pdu.ext         = false;
   tx_pdu.choice_type = LIBLTE_S1AP_S1AP_PDU_CHOICE_UNSUCCESSFULOUTCOME;

srsue/hdr/mac/dl_harq.h

@@ -190,6 +190,9 @@ private:
     }
 
   private:
+
+    const static int RESET_DUPLICATE_TIMEOUT = 8*6;
+
     class dl_tb_process {
     public:
       dl_tb_process(void) {
@@ -273,6 +276,10 @@ private:
           grant.last_tti = cur_grant.tti;
           memcpy(&cur_grant, &grant, sizeof(Tgrant));
 
+          if (payload_buffer_ptr) {
+            Warning("DL PID %d: Allocating buffer already allocated\n", pid);
+          }
+
           // Instruct the PHY To combine the received data and attempt to decode it
           if (pid == HARQ_BCCH_PID) {
             payload_buffer_ptr = harq_entity->demux_unit->request_buffer_bcch(cur_grant.n_bytes[tid]);
@@ -294,8 +301,14 @@ private:
 
         } else {
           action->default_ack[tid] = true;
-          Warning("DL PID %d: Received duplicate TB. Discarting and retransmitting ACK (grant_tti=%d, ndi=%d, sz=%d)\n",
+          uint32_t interval = srslte_tti_interval(grant.tti, cur_grant.tti);
-                  pid, cur_grant.tti, cur_grant.ndi[tid], cur_grant.n_bytes[tid]);
+          Warning("DL PID %d: Received duplicate TB. Discarting and retransmitting ACK (grant_tti=%d, ndi=%d, sz=%d, reset=%s)\n",
+                  pid, cur_grant.tti, cur_grant.ndi[tid], cur_grant.n_bytes[tid], interval>RESET_DUPLICATE_TIMEOUT?"yes":"no");
+          if (interval > RESET_DUPLICATE_TIMEOUT) {
+            pthread_mutex_unlock(&mutex);
+            reset();
+            pthread_mutex_lock(&mutex);
+          }
         }
 
         if (pid == HARQ_BCCH_PID || harq_entity->timer_aligment_timer->is_expired()) {

srsue/hdr/phy/phch_recv.h

@@ -104,6 +104,8 @@ private:
   void   cell_search_inc();
   void   cell_reselect();
 
+  float  get_cfo();
+
   uint32_t new_earfcn;
   srslte_cell_t new_cell;
 
@@ -150,7 +152,7 @@ private:
     srslte_ue_mib_t   ue_mib;
     uint32_t          cnt;
     uint32_t          timeout;
-    const static uint32_t SYNC_SFN_TIMEOUT = 500;
+    const static uint32_t SYNC_SFN_TIMEOUT = 80;
   };
 
   // Class to perform cell measurements
@@ -168,7 +170,7 @@ private:
     void      set_cell(srslte_cell_t cell);
     ret_code  run_subframe(uint32_t sf_idx);
     ret_code  run_subframe_sync(srslte_ue_sync_t *ue_sync, uint32_t sf_idx);
-    ret_code  run_multiple_subframes(cf_t *buffer, uint32_t offset, uint32_t sf_idx, uint32_t nof_sf);
+    ret_code  run_multiple_subframes(cf_t *buffer, int offset, uint32_t sf_idx, uint32_t nof_sf);
     float     rssi();
     float     rsrp();
     float     rsrq();
@@ -199,11 +201,11 @@ private:
       uint32_t offset;
     } cell_info_t;
     void init(srslte::log *log_h, bool sic_pss_enabled, uint32_t max_sf_window);
+    void deinit();
     void reset();
     int find_cells(cf_t *input_buffer, float rx_gain_offset, srslte_cell_t current_cell, uint32_t nof_sf, cell_info_t found_cells[MAX_CELLS]);
   private:
 
-    cf_t               *input_cfo_corrected;
     cf_t               *sf_buffer[SRSLTE_MAX_PORTS];
     srslte::log        *log_h;
     srslte_sync_t       sync_find;
@@ -220,6 +222,7 @@ private:
   // Class to perform intra-frequency measurements
   class intra_measure : public thread {
   public:
+    ~intra_measure();
     void init(phch_common *common, rrc_interface_phy *rrc, srslte::log *log_h);
     void stop();
     void add_cell(int pci);
@@ -230,8 +233,6 @@ private:
     void write(uint32_t tti, cf_t *data, uint32_t nsamples);
   private:
     void run_thread();
-    const static int INTRA_FREQ_MEAS_LEN_MS    = 50;
-    const static int INTRA_FREQ_MEAS_PERIOD_MS = 200;
     const static int INTRA_FREQ_MEAS_PRIO      = DEFAULT_PRIORITY + 5;
 
     scell_recv         scell;

srsue/hdr/phy/phch_worker.h

@@ -54,7 +54,6 @@ public:
   void  set_tti(uint32_t tti, uint32_t tx_tti); 
   void  set_tx_time(srslte_timestamp_t tx_time, uint32_t next_offset);
   void  set_cfo(float cfo);
-  void  set_sample_offset(float sample_offset); 
 
   void  set_ul_params(bool pregen_disabled = false);
   void  set_crnti(uint16_t rnti);
@@ -64,8 +63,16 @@ public:
   void write_trace(std::string filename);
   
   int read_ce_abs(float *ce_abs, uint32_t tx_antenna, uint32_t rx_antenna);
-  uint32_t get_cell_nof_ports() {return cell.nof_ports;};
+  uint32_t get_cell_nof_ports() {
-  uint32_t get_rx_nof_antennas() {return ue_dl.nof_rx_antennas;};
+    if (cell_initiated) {
+      return cell.nof_ports;
+    } else {
+      return 1;
+    }
+  };
+  uint32_t get_rx_nof_antennas() {
+    return ue_dl.nof_rx_antennas;
+  };
   int read_pdsch_d(cf_t *pdsch_d);
   void start_plot();
 
@@ -74,7 +81,6 @@ public:
   float get_rsrp();
   float get_noise();
   float get_cfo();
-  float get_ul_cfo();
 
 private:
   /* Inherited from thread_pool::worker. Function called every subframe to run the DL/UL processing */

srsue/hdr/phy/phy.h

@@ -28,7 +28,7 @@
 #define UEPHY_H
 
 #include "srslte/srslte.h"
-#include "srslte/common/log.h"
+#include "srslte/common/log_filter.h"
 #include "phy/phy_metrics.h"
 #include "phy/phch_recv.h"
 #include "phy/prach.h"
@@ -53,7 +53,7 @@ public:
   bool init(srslte::radio_multi *radio_handler, 
             mac_interface_phy *mac, 
             rrc_interface_phy *rrc,
-            std::vector<srslte::log*> log_vec,
+            std::vector<srslte::log_filter*> log_vec,
             phy_args_t *args = NULL);
   
   void stop();
@@ -158,7 +158,7 @@ private:
   const static int WORKERS_THREAD_PRIO = 0; 
   
   srslte::radio_multi      *radio_handler;
-  std::vector<srslte::log*>        log_vec;
+  std::vector<srslte::log_filter*>        log_vec;
   srslte::log              *log_h;
   srslte::log              *log_phy_lib_h;
   srsue::mac_interface_phy *mac;

srsue/hdr/ue.h

@@ -103,7 +103,7 @@ private:
   srslte::logger        *logger;
 
   // rf_log is on ue_base
-  std::vector<srslte::log*>  phy_log;
+  std::vector<srslte::log_filter*>  phy_log;
   srslte::log_filter  mac_log;
   srslte::log_filter  rlc_log;
   srslte::log_filter  pdcp_log;

srsue/hdr/upper/nas.h

@@ -88,9 +88,10 @@ public:
   uint32_t get_ul_count();
   bool is_attached();
   bool is_attaching();
+  bool is_data_requested();
   bool get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi);
   bool get_k_asme(uint8_t *k_asme_, uint32_t n);
-  void plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code);
+  bool plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code);
   void plmn_search_end();
 
   // UE interface

srsue/hdr/upper/rrc.h

@@ -249,7 +249,7 @@ public:
 
   void enable_capabilities();
   void plmn_search();
-  void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id);
+  void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, bool connect_request);
 
   // PHY interface
   void in_sync();
@@ -311,6 +311,8 @@ private:
 
   uint16_t ho_src_rnti;
   cell_t   ho_src_cell;
+  uint32_t ho_target_pci;
+  bool     ho_syncing;
   phy_interface_rrc::phy_cfg_t ho_src_phy_cfg;
   mac_interface_rrc::mac_cfg_t ho_src_mac_cfg;
   bool pending_mob_reconf;
@@ -324,9 +326,6 @@ private:
   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 = 1000;
-
   uint8_t k_rrc_enc[32];
   uint8_t k_rrc_int[32];
   uint8_t k_up_enc[32];
@@ -342,7 +341,7 @@ private:
   srslte::mac_interface_timers *mac_timers;
   uint32_t n310_cnt, N310;
   uint32_t n311_cnt, N311;
-  uint32_t t301, t310, t311, t304;
+  uint32_t t300, t301, t310, t311, t304;
 
   // Radio bearers
   typedef enum{
@@ -401,7 +400,7 @@ private:
   uint16_t           sysinfo_index;
   uint32_t           last_win_start;
 
-  void select_next_cell_in_plmn();
+  bool select_next_cell_in_plmn();
   LIBLTE_RRC_PLMN_IDENTITY_STRUCT selected_plmn_id;
 
   bool thread_running;
@@ -545,6 +544,7 @@ private:
   // Helpers
   void          ho_failed();
   bool          ho_prepare();
+  void          ho_synced(uint32_t target_pci);
   void          rrc_connection_release();
   void          con_restablish_cell_reselected();
   void          radio_link_failure();

srsue/src/mac/demux.cc

@@ -169,7 +169,7 @@ void demux::process_sch_pdu(srslte::sch_pdu *pdu_msg)
           rlc->write_pdu(pdu_msg->get()->get_sdu_lcid(), pdu_msg->get()->get_sdu_ptr(), pdu_msg->get()->get_payload_size());
         } else {
           char tmp[1024];
-          srslte_vec_sprint_hex(tmp, pdu_msg->get()->get_sdu_ptr(), 32);
+          srslte_vec_sprint_hex(tmp, sizeof(tmp), pdu_msg->get()->get_sdu_ptr(), 32);
           Error("PDU size %d exceeds maximum PDU buffer size, lcid=%d, hex=[%s]\n",
                 pdu_msg->get()->get_payload_size(), pdu_msg->get()->get_sdu_lcid(), tmp);
         }

srsue/src/mac/mux.cc

@@ -166,8 +166,7 @@ uint8_t* mux::pdu_get(uint8_t *payload, uint32_t pdu_sz, uint32_t tx_tti, uint32
     }    
   }
   
-// Logical Channel Procedure
+  // Logical Channel Procedure
-
   bool is_rar = false;
 
   pdu_msg.init_tx(payload, pdu_sz, true);
@@ -325,7 +324,6 @@ bool mux::allocate_sdu(uint32_t lcid, srslte::sch_pdu* pdu_msg, int max_sdu_sz)
       if (pdu_msg->new_subh()) { // there is space for a new subheader
         sdu_len = pdu_msg->get()->set_sdu(lcid, sdu_len, rlc);
         if (sdu_len > 0) { // new SDU could be added
-          
           Debug("SDU:   allocated lcid=%d, rlc_buffer=%d, allocated=%d/%d, max_sdu_sz=%d, remaining=%d\n",
                  lcid, buffer_state, sdu_len, sdu_space, max_sdu_sz, pdu_msg->rem_size());
           return true;               

srsue/src/main.cc

@@ -203,6 +203,19 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
      bpo::value<string>(&args->expert.phy.equalizer_mode)->default_value("mmse"),
      "Equalizer mode")
 
+    ("expert.intra_freq_meas_len_ms",
+       bpo::value<uint32_t>(&args->expert.phy.intra_freq_meas_len_ms)->default_value(20),
+       "Duration of the intra-frequency neighbour cell measurement in ms.")
+
+    ("expert.intra_freq_meas_period_ms",
+       bpo::value<uint32_t>(&args->expert.phy.intra_freq_meas_period_ms)->default_value(200),
+       "Period of intra-frequency neighbour cell measurement in ms. Maximum as per 3GPP is 200 ms.")
+
+    ("expert.cfo_is_doppler",
+       bpo::value<bool>(&args->expert.phy.cfo_is_doppler)->default_value(false),
+       "Assume detected CFO is doppler and correct the UL in the same direction. If disabled, the CFO is assumed"
+        "to be caused by the local oscillator and the UL correction is in the opposite direction. Default assumes oscillator.")
+
     ("expert.cfo_integer_enabled",
      bpo::value<bool>(&args->expert.phy.cfo_integer_enabled)->default_value(false),
      "Enables integer CFO estimation and correction.")
@@ -215,12 +228,6 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
      bpo::value<float>(&args->expert.phy.cfo_pss_ema)->default_value(DEFAULT_CFO_EMA_TRACK),
      "CFO Exponential Moving Average coefficient for PSS estimation during TRACK.")
 
-    /* REF EMA is currently not used
-    ("expert.cfo_ref_ema",
-     bpo::value<float>(&args->expert.phy.cfo_ref_ema)->default_value(0.01),
-     "CFO Exponential Moving Average coefficient for RS estimation after PSS acquisition")
-    */
-
     ("expert.cfo_ref_mask",
      bpo::value<uint32_t>(&args->expert.phy.cfo_ref_mask)->default_value(1023),
      "Bitmask for subframes on which to run RS estimation (set to 0 to disable, default all sf)")
@@ -258,10 +265,6 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
      bpo::value<int>(&args->expert.phy.time_correct_period)->default_value(5),
      "Period for sampling time offset correction.")
 
-    ("expert.sfo_correct_disable",
-     bpo::value<bool>(&args->expert.phy.sfo_correct_disable)->default_value(false),
-     "Disables phase correction before channel estimation.")
-
     ("expert.sss_algorithm",
      bpo::value<string>(&args->expert.phy.sss_algorithm)->default_value("full"),
      "Selects the SSS estimation algorithm.")
@@ -270,6 +273,9 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
      bpo::value<float>(&args->expert.phy.estimator_fil_w)->default_value(0.1),
      "Chooses the coefficients for the 3-tap channel estimator centered filter.")
 
+    ("expert.pdsch_csi_enabled",
+     bpo::value<bool>(&args->expert.phy.pdsch_csi_enabled)->default_value(false),
+     "Stores the Channel State Information and uses it for weightening the softbits. It is only compatible with TM1.")
 
     ("rf_calibration.tx_corr_dc_gain", bpo::value<float>(&args->rf_cal.tx_corr_dc_gain)->default_value(0.0),
      "TX DC offset gain correction")
@@ -419,6 +425,9 @@ void *input_loop(void *m) {
           cout << "Enter t to restart trace." << endl;
         }
         metrics_screen.toggle_print(do_metrics);
+      } else
+      if ('q' == key) {
+        running = false;
       }
     }
   }

srsue/src/phy/phch_recv.cc

@@ -220,7 +220,7 @@ bool phch_recv::set_cell() {
 
   // Set cell in all objects
   if (srslte_ue_sync_set_cell(&ue_sync, cell)) {
-    Error("SYNC:  Setting cell: initiating ue_sync");
+    Error("SYNC:  Setting cell: initiating ue_sync\n");
     return false;
   }
   measure_p.set_cell(cell);
@@ -308,6 +308,11 @@ void phch_recv::cell_search_start() {
 
 bool phch_recv::cell_handover(srslte_cell_t cell)
 {
+  if (!srslte_cell_isvalid(&cell)) {
+    log_h->error("Received HO command to invalid cell. ID=%d, PRB=%d, ports=%d\n", cell.id, cell.nof_prb, cell.nof_ports);
+    return false;
+  }
+
   int cnt = 0;
   while(worker_com->is_any_pending_ack() && cnt < 10) {
     usleep(1000);
@@ -419,6 +424,25 @@ bool phch_recv::set_frequency()
   }
 }
 
+float phch_recv::get_cfo()
+{
+  float cfo = srslte_ue_sync_get_cfo(&ue_sync);
+
+  float ret = cfo*ul_dl_factor;
+
+  if (worker_com->args->cfo_is_doppler) {
+    ret *= -1;
+  }
+
+  if (radio_h->get_freq_offset() != 0.0f) {
+    /* Compensates the radio frequency offset applied equally to DL and UL */
+    const float offset_hz = (float) radio_h->get_freq_offset() * (1.0f - ul_dl_factor);
+    ret = cfo - offset_hz;
+  }
+
+  return ret/15000;
+}
+
 void phch_recv::set_sampling_rate()
 {
   current_srate = (float) srslte_sampling_freq_hz(cell.nof_prb);
@@ -565,8 +589,8 @@ void phch_recv::run_thread()
             }
             break;
           case sfn_sync::TIMEOUT:
-            log_h->warning("SYNC:  Timeout while synchronizing SFN. Going back to cell search\n");
+            log_h->warning("SYNC:  Timeout while synchronizing SFN\n");
-            phy_state = CELL_SEARCH;
+            rrc->out_of_sync();
             break;
           case sfn_sync::IDLE:
             break;
@@ -624,11 +648,9 @@ void phch_recv::run_thread()
 
               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(get_cfo());
               worker_com->set_sync_metrics(metrics);
 
-              worker->set_sample_offset(srslte_ue_sync_get_sfo(&ue_sync)/1000);
-
               /* Compute TX time: Any transmission happens in TTI+4 thus advance 4 ms the reception time */
               srslte_timestamp_t rx_time, tx_time, tx_time_prach;
               srslte_ue_sync_get_last_timestamp(&ue_sync, &rx_time);
@@ -652,7 +674,7 @@ void phch_recv::run_thread()
               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);
+                prach_buffer->send(radio_h, get_cfo(), 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);
@@ -702,6 +724,12 @@ void phch_recv::run_thread()
     mac->tti_clock(tti);
     tti = (tti+1) % 10240;
   }
+
+  for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
+    if (dummy_buffer[i]) {
+      free(dummy_buffer[i]);
+    }
+  }
 }
 
 void phch_recv::in_sync() {
@@ -717,8 +745,10 @@ void phch_recv::in_sync() {
 // Out of sync called by worker or phch_recv every 1 or 5 ms
 void phch_recv::out_of_sync() {
   // Send RRC out-of-sync signal after 200 ms consecutive subframes
+  Info("Out-of-sync %d/%d\n", out_of_sync_cnt, NOF_OUT_OF_SYNC_SF);
   out_of_sync_cnt++;
   if (out_of_sync_cnt >= NOF_OUT_OF_SYNC_SF) {
+    Info("Sending to RRC\n");
     rrc->out_of_sync();
     out_of_sync_cnt = 0;
     in_sync_cnt = 0;
@@ -1007,6 +1037,7 @@ void phch_recv::measure::init(cf_t *buffer[SRSLTE_MAX_PORTS], srslte::log *log_h
     Error("SYNC:  Initiating ue_dl_measure\n");
     return;
   }
+  srslte_chest_dl_set_rsrp_neighbour(&ue_dl.chest, true);
   reset();
 }
 
@@ -1070,7 +1101,7 @@ phch_recv::measure::ret_code phch_recv::measure::run_subframe_sync(srslte_ue_syn
 }
 
 phch_recv::measure::ret_code phch_recv::measure::run_multiple_subframes(cf_t *input_buffer,
-                                                                        uint32_t offset,
+                                                                        int offset,
                                                                         uint32_t sf_idx,
                                                                         uint32_t max_sf)
 {
@@ -1079,17 +1110,18 @@ phch_recv::measure::ret_code phch_recv::measure::run_multiple_subframes(cf_t *in
   ret_code ret = IDLE;
 
   offset = offset-sf_len/2;
-  if (offset < 0) {
+  while (offset < 0 && sf_idx < max_sf) {
     offset += sf_len;
     sf_idx ++;
   }
 
+
+  // Fine-tune offset using RS
+#ifdef FINE_TUNE_OFFSET_WITH_RS
   float max_rsrp = -200;
   int best_test_offset = 0;
   int test_offset = 0;
   bool found_best = false;
-
-  // Fine-tune offset using RS
   for (uint32_t n=0;n<5;n++) {
 
     test_offset = offset-2+n;
@@ -1113,11 +1145,14 @@ phch_recv::measure::ret_code phch_recv::measure::run_multiple_subframes(cf_t *in
     }
   }
 
+  Debug("INTRA: fine-tuning offset: %d, found_best=%d, rem_sf=%d\n", offset, found_best, nof_sf);
+
   offset = found_best?best_test_offset:offset;
-  if (offset >= 0 && offset < sf_len*max_sf) {
+#endif
+
+  if (offset >= 0 && offset < (int) sf_len*max_sf) {
     uint32_t nof_sf = (sf_len*max_sf - offset)/sf_len;
 
-    Debug("INTRA: fine-tuning offset: %d, found_best=%d, rem_sf=%d\n", offset, found_best, nof_sf);
 
     final_offset = offset;
 
@@ -1145,7 +1180,7 @@ phch_recv::measure::ret_code phch_recv::measure::run_subframe(uint32_t sf_idx)
     return ERROR;
   }
 
-  float rsrp   = srslte_chest_dl_get_rsrp(&ue_dl.chest);
+  float rsrp   = srslte_chest_dl_get_rsrp_neighbour(&ue_dl.chest);
   float rsrq   = srslte_chest_dl_get_rsrq(&ue_dl.chest);
   float snr    = srslte_chest_dl_get_snr(&ue_dl.chest);
   float rssi   = srslte_vec_avg_power_cf(buffer[0], SRSLTE_SF_LEN_PRB(current_prb));
@@ -1163,7 +1198,7 @@ phch_recv::measure::ret_code phch_recv::measure::run_subframe(uint32_t sf_idx)
   }
   cnt++;
 
-  log_h->debug("SYNC:  Measuring RSRP %d/%d, sf_idx=%d, RSRP=%.1f dBm, SNR=%.1f dB\n",
+  log_h->debug("SYNC:  Measuring RSRP %d/%d, sf_idx=%d, RSRP=%.1f dBm, corr-RSRP=%.1f dBm, SNR=%.1f dB\n",
               cnt, nof_subframes, sf_idx, rsrp, snr);
 
   if (cnt >= nof_subframes) {
@@ -1193,7 +1228,6 @@ void phch_recv::scell_recv::init(srslte::log *log_h, bool sic_pss_enabled, uint3
   uint32_t max_sf_size = SRSLTE_SF_LEN(max_fft_sz);
 
   sf_buffer[0]        = (cf_t*) srslte_vec_malloc(sizeof(cf_t)*max_sf_size);
-  input_cfo_corrected = (cf_t*) srslte_vec_malloc(sizeof(cf_t)*15*max_sf_size);
 
   measure_p.init(sf_buffer, log_h, 1, max_sf_window);
 
@@ -1202,6 +1236,7 @@ void phch_recv::scell_recv::init(srslte::log *log_h, bool sic_pss_enabled, uint3
     fprintf(stderr, "Error initiating sync_find\n");
     return;
   }
+  srslte_sync_set_sss_algorithm(&sync_find, SSS_FULL);
   srslte_sync_cp_en(&sync_find, false);
   srslte_sync_set_cfo_pss_enable(&sync_find, true);
   srslte_sync_set_threshold(&sync_find, 1.7);
@@ -1215,8 +1250,7 @@ void phch_recv::scell_recv::init(srslte::log *log_h, bool sic_pss_enabled, uint3
   srslte_sync_set_sss_eq_enable(&sync_find,    true);
 
   sync_find.pss.chest_on_filter = true;
-
+  sync_find.sss_channel_equalize = false;
-  sync_find.sss_channel_equalize = true;
 
   reset();
 }
@@ -1227,6 +1261,12 @@ void phch_recv::scell_recv::reset()
   measure_p.reset();
 }
 
+void phch_recv::scell_recv::deinit()
+{
+  srslte_sync_free(&sync_find);
+  free(sf_buffer[0]);
+}
+
 int phch_recv::scell_recv::find_cells(cf_t *input_buffer, float rx_gain_offset, srslte_cell_t cell, uint32_t nof_sf, cell_info_t cells[MAX_CELLS])
 {
   uint32_t fft_sz  = srslte_symbol_sz(cell.nof_prb);
@@ -1271,7 +1311,7 @@ int phch_recv::scell_recv::find_cells(cf_t *input_buffer, float rx_gain_offset,
 
         for (uint32_t sf5_cnt=0;sf5_cnt<nof_sf/5;sf5_cnt++) {
           sync_res = srslte_sync_find(&sync_find, input_buffer, sf5_cnt*5*sf_len, &peak_idx);
-          Info("INTRA: n_id_2=%d, cnt=%d/%d, sync_res=%d, sf_idx=%d, peak_idx=%d, peak_value=%f\n",
+          Debug("INTRA: n_id_2=%d, cnt=%d/%d, sync_res=%d, sf_idx=%d, peak_idx=%d, peak_value=%f\n",
                  n_id_2, sf5_cnt, nof_sf/5, sync_res, srslte_sync_get_sf_idx(&sync_find), peak_idx, sync_find.peak_value);
 
           if (sync_find.peak_value > max_peak && sync_res == SRSLTE_SYNC_FOUND) {
@@ -1301,28 +1341,21 @@ int phch_recv::scell_recv::find_cells(cf_t *input_buffer, float rx_gain_offset,
                 found_cell.nof_ports = 1;  // Use port 0 only for measurement
                 measure_p.set_cell(found_cell);
 
-                // Correct CFO
-                /*
-                srslte_cfo_correct(&sync_find.cfo_corr_frame,
-                                   input_buffer,
-                                   input_cfo_corrected,
-                                   -srslte_sync_get_cfo(&sync_find)/sync_find.fft_size);
-                */
-
                 switch(measure_p.run_multiple_subframes(input_buffer, peak_idx, sf_idx, nof_sf))
                 {
                   case measure::MEASURE_OK:
                     // Consider a cell to be detectable 8.1.2.2.1.1 from 36.133. Currently only using first condition
                     if (measure_p.rsrp() > ABSOLUTE_RSRP_THRESHOLD_DBM) {
+
                       cells[nof_cells].pci = found_cell.id;
                       cells[nof_cells].rsrp = measure_p.rsrp();
                       cells[nof_cells].rsrq = measure_p.rsrq();
                       cells[nof_cells].offset = measure_p.frame_st_idx();
 
                       Info(
-                          "INTRA: Found neighbour cell %d: PCI=%03d, RSRP=%5.1f dBm, peak_idx=%5d, peak_value=%3.2f, sf=%d, max_sf=%d, n_id_2=%d, CFO=%6.1f Hz\n",
+                          "INTRA: Found neighbour cell %d: PCI=%03d, RSRP=%5.1f dBm, peak_idx=%5d, peak_value=%3.2f, sf=%d, nof_sf=%d, n_id_2=%d, CFO=%6.1f Hz\n",
                           nof_cells, cell_id, measure_p.rsrp(), measure_p.frame_st_idx(), sync_find.peak_value,
-                          sf_idx, max_sf5, n_id_2, 15000 * srslte_sync_get_cfo(&sync_find));
+                          sf_idx, nof_sf, n_id_2, 15000 * srslte_sync_get_cfo(&sync_find));
 
                       nof_cells++;
 
@@ -1403,11 +1436,11 @@ void phch_recv::intra_measure::init(phch_common *common, rrc_interface_phy *rrc,
   receive_enabled = false;
 
   // Start scell
-  scell.init(log_h, common->args->sic_pss_enabled, INTRA_FREQ_MEAS_LEN_MS);
+  scell.init(log_h, common->args->sic_pss_enabled, common->args->intra_freq_meas_len_ms);
 
-  search_buffer = (cf_t*) srslte_vec_malloc(INTRA_FREQ_MEAS_LEN_MS*SRSLTE_SF_LEN_PRB(SRSLTE_MAX_PRB)*sizeof(cf_t));
+  search_buffer = (cf_t*) srslte_vec_malloc(common->args->intra_freq_meas_len_ms*SRSLTE_SF_LEN_PRB(SRSLTE_MAX_PRB)*sizeof(cf_t));
 
-  if (srslte_ringbuffer_init(&ring_buffer, sizeof(cf_t)*INTRA_FREQ_MEAS_LEN_MS*2*SRSLTE_SF_LEN_PRB(SRSLTE_MAX_PRB))) {
+  if (srslte_ringbuffer_init(&ring_buffer, sizeof(cf_t)*common->args->intra_freq_meas_len_ms*2*SRSLTE_SF_LEN_PRB(SRSLTE_MAX_PRB))) {
     return;
   }
 
@@ -1422,6 +1455,12 @@ void phch_recv::intra_measure::stop() {
   wait_thread_finish();
 }
 
+phch_recv::intra_measure::~intra_measure() {
+  srslte_ringbuffer_free(&ring_buffer);
+  scell.deinit();
+  free(search_buffer);
+}
+
 void phch_recv::intra_measure::set_primay_cell(uint32_t earfcn, srslte_cell_t cell) {
   this->current_earfcn = earfcn;
   current_sflen = SRSLTE_SF_LEN_PRB(cell.nof_prb);
@@ -1471,7 +1510,7 @@ void phch_recv::intra_measure::rem_cell(int pci) {
 
 void phch_recv::intra_measure::write(uint32_t tti, cf_t *data, uint32_t nsamples) {
   if (receive_enabled) {
-    if ((tti%INTRA_FREQ_MEAS_PERIOD_MS) == 0) {
+    if ((tti%common->args->intra_freq_meas_period_ms) == 0) {
       receiving   = true;
       receive_cnt = 0;
       measure_tti = tti;
@@ -1483,7 +1522,7 @@ void phch_recv::intra_measure::write(uint32_t tti, cf_t *data, uint32_t nsamples
         receiving = false;
       } else {
         receive_cnt++;
-        if (receive_cnt == INTRA_FREQ_MEAS_LEN_MS) {
+        if (receive_cnt == common->args->intra_freq_meas_len_ms) {
           tti_sync.increase();
           receiving = false; 
         }
@@ -1502,8 +1541,8 @@ void phch_recv::intra_measure::run_thread()
     if (running) {
 
       // Read data from buffer and find cells in it
-      srslte_ringbuffer_read(&ring_buffer, search_buffer, INTRA_FREQ_MEAS_LEN_MS*current_sflen*sizeof(cf_t));
+      srslte_ringbuffer_read(&ring_buffer, search_buffer, common->args->intra_freq_meas_len_ms*current_sflen*sizeof(cf_t));
-      int found_cells = scell.find_cells(search_buffer, common->rx_gain_offset, primary_cell, INTRA_FREQ_MEAS_LEN_MS, info);
+      int found_cells = scell.find_cells(search_buffer, common->rx_gain_offset, primary_cell, common->args->intra_freq_meas_len_ms, info);
       receiving = false;
 
       for (int i=0;i<found_cells;i++) {

srsue/src/phy/phch_worker.cc

@@ -137,6 +137,7 @@ bool phch_worker::init(uint32_t max_prb, srslte::log *log_h, srslte::log *log_ph
   srslte_chest_dl_cfo_estimate_enable(&ue_dl.chest, phy->args->cfo_ref_mask!=0, phy->args->cfo_ref_mask);
   srslte_ue_ul_set_normalization(&ue_ul, true);
   srslte_ue_ul_set_cfo_enable(&ue_ul, true);
+  srslte_pdsch_enable_csi(&ue_dl.pdsch, phy->args->pdsch_csi_enabled);
 
   mem_initiated = true;
 
@@ -189,14 +190,6 @@ void phch_worker::set_cfo(float cfo_)
   cfo = cfo_;
 }
 
-void phch_worker::set_sample_offset(float sample_offset)
-{
-  if (phy->args->sfo_correct_disable) {
-    sample_offset = 0; 
-  }
-  srslte_ue_dl_set_sample_offset(&ue_dl, sample_offset);
-}
-
 void phch_worker::set_crnti(uint16_t rnti)
 {
   srslte_ue_dl_set_rnti(&ue_dl, rnti);
@@ -230,20 +223,6 @@ float phch_worker::get_cfo()
   return cfo;
 }
 
-float phch_worker::get_ul_cfo() {
-  srslte::radio *radio = phy->get_radio();
-
-  if (radio->get_freq_offset() != 0.0f) {
-    /* Compensates the radio frequency offset applied equally to DL and UL */
-    const float ul_dl_ratio = (float) radio->get_tx_freq() / (float) radio->get_rx_freq();
-    const float offset_hz = (float) radio->get_freq_offset() * (1.0f - ul_dl_ratio);
-    return cfo - offset_hz / (15000);
-  } else {
-    return cfo;
-  }
-
-}
-
 void phch_worker::work_imp()
 {
   if (!cell_initiated) {
@@ -367,7 +346,7 @@ void phch_worker::work_imp()
   }
 
   /* Set UL CFO before transmission */  
-  srslte_ue_ul_set_cfo(&ue_ul, get_ul_cfo());
+  srslte_ue_ul_set_cfo(&ue_ul, cfo);
 
   /* Transmit PUSCH, PUCCH or SRS */
   bool signal_ready = false; 
@@ -439,7 +418,9 @@ void phch_worker::compute_ri(uint8_t *ri, uint8_t *pmi, float *sinr) {
       /* If 2 ort more receiving antennas, select RI */
       float cn = 0.0f;
       srslte_ue_dl_ri_select(&ue_dl, ri, &cn);
+      if (ri) {
         Debug("TM3 RI select %d layers, κ=%fdB\n", (*ri) + 1, cn);
+      }
     } else {
       /* If only one receiving antenna, force RI for 1 layer */
       if (ri) {
@@ -529,7 +510,9 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
     if (srslte_ue_dl_find_dl_dci_type(&ue_dl, phy->config->dedicated.antenna_info_explicit_value.tx_mode, cfi, tti%10,
                                       dl_rnti, type, &dci_msg) != 1) {
       if (type == SRSLTE_RNTI_RAR) {
-        Info("RAR not found\n");
+        Info("RAR not found, SNR=%.1f dB, tti=%d, cfi=%d, tx_mode=%d, cell_id=%d\n",
+             10*log10(srslte_chest_dl_get_snr(&ue_dl.chest)), tti, cfi,
+             phy->config->dedicated.antenna_info_explicit_value.tx_mode, cell.id);
       }
       return false;
     }
@@ -571,10 +554,10 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
 
     last_dl_pdcch_ncce = srslte_ue_dl_get_ncce(&ue_dl);
 
-    char hexstr[16];
+    char hexstr[512];
     hexstr[0]='\0';
     if (log_h->get_level() >= srslte::LOG_LEVEL_INFO) {
-      srslte_vec_sprint_hex(hexstr, dci_msg.data, dci_msg.nof_bits);
+      srslte_vec_sprint_hex(hexstr, sizeof(hexstr), dci_msg.data, dci_msg.nof_bits);
     }
     Info("PDCCH: DL DCI %s cce_index=%2d, L=%d, n_data_bits=%d, hex=%s\n", srslte_dci_format_string(dci_msg.format), 
          last_dl_pdcch_ncce, (1<<ue_dl.last_location.L), dci_msg.nof_bits, hexstr);
@@ -809,10 +792,10 @@ bool phch_worker::decode_pdcch_ul(mac_interface_phy::mac_grant_t* grant)
       grant->has_cqi_request = dci_unpacked.cqi_request;
       ret = true; 
       
-      char hexstr[16];
+      char hexstr[512];
       hexstr[0]='\0';
       if (log_h->get_level() >= srslte::LOG_LEVEL_INFO) {
-        srslte_vec_sprint_hex(hexstr, dci_msg.data, dci_msg.nof_bits);
+        srslte_vec_sprint_hex(hexstr, sizeof(hexstr), dci_msg.data, dci_msg.nof_bits);
       }
       // Change to last_location_ul
       Info("PDCCH: UL DCI Format0  cce_index=%d, L=%d, n_data_bits=%d, hex=%s\n", 

srsue/src/phy/phy.cc

@@ -97,7 +97,6 @@ void phy::set_default_args(phy_args_t *args)
   args->cfo_integer_enabled = false; 
   args->cfo_correct_tol_hz  = 50; 
   args->time_correct_period = 5; 
-  args->sfo_correct_disable = false; 
   args->sss_algorithm       = "full";
   args->estimator_fil_w     = 0.1; 
 }
@@ -120,7 +119,7 @@ bool phy::check_args(phy_args_t *args)
 }
 
 bool phy::init(srslte::radio_multi* radio_handler, mac_interface_phy *mac, rrc_interface_phy *rrc,
-               std::vector<srslte::log*> log_vec, phy_args_t *phy_args) {
+               std::vector<srslte::log_filter*> log_vec, phy_args_t *phy_args) {
 
   mlockall(MCL_CURRENT | MCL_FUTURE);
 

srsue/src/ue.cc

@@ -44,6 +44,9 @@ ue::ue()
 
 ue::~ue()
 {
+  for (uint32_t i = 0; i < phy_log.size(); i++) {
+    delete(phy_log[i]);
+  }
 }
 
 bool ue::init(all_args_t *args_)
@@ -272,7 +275,7 @@ void ue::stop()
 
 bool ue::is_attached()
 {
-  return (RRC_STATE_CONNECTED == rrc.get_state());
+  return rrc.is_connected();
 }
 
 void ue::start_plot() {

srsue/src/upper/gw.cc

@@ -243,6 +243,10 @@ void gw::run_thread()
   uint32          idx = 0;
   int32           N_bytes;
   srslte::byte_buffer_t *pdu = pool_allocate;
+  if (!pdu) {
+    gw_log->error("Fatal Error: Couldn't allocate PDU in run_thread().\n");
+    return;
+  }
 
   const static uint32_t ATTACH_TIMEOUT_MS   = 10000;
   const static uint32_t ATTACH_MAX_ATTEMPTS = 3;
@@ -307,7 +311,7 @@ void gw::run_thread()
             do {
               pdu = pool_allocate;
               if (!pdu) {
-                printf("Not enough buffers in pool\n");
+                gw_log->error("Fatal Error: Couldn't allocate PDU in run_thread().\n");
                 usleep(100000);
               }
             } while(!pdu);

srsue/src/upper/nas.cc

@@ -107,8 +107,8 @@ void nas::attach_request() {
       selecting_plmn = current_plmn;
     }
   } else if (state == EMM_STATE_REGISTERED) {
-    nas_log->info("NAS state is registered, connecting to same PLMN\n");
+    nas_log->info("NAS state is registered, selecting current PLMN\n");
-    rrc->plmn_select(current_plmn);
+    rrc->plmn_select(current_plmn, true);
   } else {
     nas_log->info("Attach request ignored. State = %s\n", emm_state_text[state]);
   }
@@ -123,12 +123,7 @@ void nas::deattach_request() {
  * RRC interface
  ******************************************************************************/
 
-void nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) {
+bool nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) {
-
-  // Do not process new PLMN if already selected
-  if (plmn_selection == PLMN_SELECTED) {
-    return;
-  }
 
   // Check if already registered
   for (uint32_t i=0;i<known_plmns.size();i++) {
@@ -136,10 +131,11 @@ void nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_
       nas_log->info("Found known PLMN Id=%s\n", plmn_id_to_string(plmn_id).c_str());
       if (plmn_id.mcc == home_plmn.mcc && plmn_id.mnc == home_plmn.mnc) {
         nas_log->info("Connecting Home PLMN Id=%s\n", plmn_id_to_string(plmn_id).c_str());
-        rrc->plmn_select(plmn_id);
+        rrc->plmn_select(plmn_id, state == EMM_STATE_REGISTERED_INITIATED);
         selecting_plmn = plmn_id;
+        return true;
       }
-      return;
+      return false;
     }
   }
 
@@ -152,10 +148,11 @@ void nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_
                 tracking_area_code);
 
   if (plmn_id.mcc == home_plmn.mcc && plmn_id.mnc == home_plmn.mnc) {
-    rrc->plmn_select(plmn_id);
+    rrc->plmn_select(plmn_id, state == EMM_STATE_REGISTERED_INITIATED);
     selecting_plmn = plmn_id;
+    return true;
   }
-
+  return false;
 }
 
 // RRC indicates that the UE has gone through all EARFCN and finished PLMN selection
@@ -170,7 +167,7 @@ void nas::plmn_search_end() {
                        plmn_id_to_string(home_plmn).c_str(),
                        plmn_id_to_string(known_plmns[0]).c_str());
     }
-    rrc->plmn_select(known_plmns[0]);
+    rrc->plmn_select(known_plmns[0], state == EMM_STATE_REGISTERED_INITIATED);
   } else {
     nas_log->info("Finished searching PLMN in current EARFCN set but no networks were found.\n");
     if (state == EMM_STATE_REGISTERED_INITIATED && plmn_selection == PLMN_NOT_SELECTED) {
@@ -538,6 +535,7 @@ void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu) {
 
     state = EMM_STATE_REGISTERED;
     current_plmn = selecting_plmn;
+    plmn_selection = PLMN_SELECTED;
 
     ctxt.rx_count++;
 
@@ -812,6 +810,11 @@ void nas::parse_emm_information(uint32_t lcid, byte_buffer_t *pdu) {
 void nas::send_attach_request() {
   LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT attach_req;
   byte_buffer_t *msg = pool_allocate;
+  if (!msg) {
+    nas_log->error("Fatal Error: Couldn't allocate PDU in send_attach_request().\n");
+    return;
+  }
+
   u_int32_t i;
 
   attach_req.eps_attach_type = LIBLTE_MME_EPS_ATTACH_TYPE_EPS_ATTACH;
@@ -913,6 +916,10 @@ void nas::gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg) {
 
 void nas::send_security_mode_reject(uint8_t cause) {
   byte_buffer_t *msg = pool_allocate;
+  if (!msg) {
+    nas_log->error("Fatal Error: Couldn't allocate PDU in send_security_mode_reject().\n");
+    return;
+  }
 
   LIBLTE_MME_SECURITY_MODE_REJECT_MSG_STRUCT sec_mode_rej;
   sec_mode_rej.emm_cause = cause;
@@ -928,6 +935,10 @@ void nas::send_identity_response() {}
 
 void nas::send_service_request() {
   byte_buffer_t *msg = pool_allocate;
+  if (!msg) {
+    nas_log->error("Fatal Error: Couldn't allocate PDU in send_service_request().\n");
+    return;
+  }
 
   // Pack the service request message directly
   msg->msg[0] = (LIBLTE_MME_SECURITY_HDR_TYPE_SERVICE_REQUEST << 4) | (LIBLTE_MME_PD_EPS_MOBILITY_MANAGEMENT);

srsue/src/upper/rrc.cc

@@ -98,6 +98,9 @@ void rrc::init(phy_interface_rrc *phy_,
   mac_timers = mac_timers_;
   state = RRC_STATE_IDLE;
   si_acquire_state = SI_ACQUIRE_IDLE;
+  last_win_start = 0;
+
+  ho_syncing = false;
 
   thread_running = true;
   start();
@@ -111,6 +114,7 @@ void rrc::init(phy_interface_rrc *phy_,
   args.nof_supported_bands = 1;
   args.feature_group = 0xe6041c00;
 
+  t300 = mac_timers->timer_get_unique_id();
   t301 = mac_timers->timer_get_unique_id();
   t310 = mac_timers->timer_get_unique_id();
   t311 = mac_timers->timer_get_unique_id();
@@ -126,6 +130,8 @@ void rrc::init(phy_interface_rrc *phy_,
 
   pending_mob_reconf = false;
 
+  connection_requested = false;
+
   // Set default values for all layers
   set_rrc_default();
   set_phy_default();
@@ -150,7 +156,7 @@ rrc_state_t rrc::get_state() {
 }
 
 bool rrc::is_connected() {
-  return (RRC_STATE_CONNECTED == state);
+  return (state >= RRC_STATE_CONNECTED && state < RRC_STATE_LEAVE_CONNECTED);
 }
 
 bool rrc::have_drb() {
@@ -173,10 +179,6 @@ void rrc::run_thread() {
 
   while (thread_running) {
 
-    if (state >= RRC_STATE_IDLE && state < RRC_STATE_CONNECTING) {
-      run_si_acquisition_procedure();
-    }
-
     switch(state) {
       /* Procedures in IDLE state 36.304 Sec 4 */
       case RRC_STATE_IDLE:
@@ -184,10 +186,8 @@ void rrc::run_thread() {
         if (phy->sync_status()) {
           // If attempting to attach, reselect cell
           if (nas->is_attaching()) {
-            sleep(1);
+            rrc_log->info("RRC IDLE: NAS has pending data and camping on cell, connecting...\n");
-            rrc_log->info("RRC IDLE: NAS is attaching and camping on cell, reselecting...\n");
             plmn_select_rrc(selected_plmn_id);
-            connection_requested = true; 
           }
         // If not camping on a cell
         } else {
@@ -196,7 +196,6 @@ void rrc::run_thread() {
             rrc_log->info("RRC IDLE: NAS is attached, PHY not synchronized. Re-selecting cell...\n");
             plmn_select_rrc(selected_plmn_id);
           } else if (nas->is_attaching()) {
-            sleep(1);
             rrc_log->info("RRC IDLE: NAS is attaching, searching again PLMN\n");
             plmn_search();
           }
@@ -230,23 +229,16 @@ void rrc::run_thread() {
             si_acquire_state = SI_ACQUIRE_SIB2;
           } else {
             apply_sib2_configs(serving_cell->sib2ptr());
-            si_acquire_state = SI_ACQUIRE_IDLE;
             state = RRC_STATE_CELL_SELECTED;
           }
-        }
+          run_si_acquisition_procedure();
-        // Don't time out during reestablishment (T311 running)
-        if (!mac_timers->timer_get(t311)->is_running() || !phy->sync_status()) {
-          select_cell_timeout++;
-          if (select_cell_timeout >= RRC_SELECT_CELL_TIMEOUT) {
-            rrc_log->info("RRC Cell Selecting: timeout expired. Starting Cell Search...\n");
-            select_cell_timeout = 0;
-            state = RRC_STATE_PLMN_START;
-            serving_cell->in_sync = false;
-          }
         }
         break;
       case RRC_STATE_CELL_SELECTED:
 
+        si_acquire_state = SI_ACQUIRE_IDLE;
+        last_win_start = 0;
+
         /* The cell is selected when the SIBs are received and applied.
          * If we were in RRC_CONNECTED and arrive here it means a RLF occurred and we are in Reestablishment procedure.
          * If T311 is running means there is a reestablishment in progress, send ConnectionReestablishmentRequest.
@@ -258,9 +250,9 @@ void rrc::run_thread() {
           con_restablish_cell_reselected();
           state = RRC_STATE_CONNECTING;
           connecting_timeout = 0;
-        } else if (connection_requested) {
+        } else if (nas->is_attaching() || connection_requested) {
-          connection_requested = false;
           rrc_log->info("RRC Cell Selected: Sending connection request...\n");
+          connection_requested = false;
           send_con_request();
           state = RRC_STATE_CONNECTING;
           connecting_timeout = 0;
@@ -275,6 +267,8 @@ void rrc::run_thread() {
         if (connecting_timeout >= RRC_CONNECTING_TIMEOUT) {
           // Select another cell
           rrc_log->info("RRC Connecting: timeout expired. Selecting next cell\n");
+          si_acquire_state = SI_ACQUIRE_IDLE;
+          last_win_start = 0;
           state = RRC_STATE_CELL_SELECTING;
         }
         break;
@@ -366,6 +360,7 @@ void rrc::run_si_acquisition_procedure()
           if (state == RRC_STATE_CELL_SELECTING) {
             select_next_cell_in_plmn();
             si_acquire_state = SI_ACQUIRE_IDLE;
+            last_win_start = 0;
           } else if (state == RRC_STATE_PLMN_SELECTION) {
             phy->cell_search_next();
           }
@@ -392,13 +387,14 @@ void rrc::run_si_acquisition_procedure()
           last_win_start = si_win_start;
 
           mac->bcch_start_rx(si_win_start, si_win_len);
-          rrc_log->debug("Instructed MAC to search for system info, win_start=%d, win_len=%d\n",
+          rrc_log->info("Instructed MAC to search for system info=%d, win_start=%d, win_len=%d\n",
-                         si_win_start, si_win_len);
+                        sysinfo_index, si_win_start, si_win_len);
         }
 
       } else {
         // We've received all SIBs, move on to connection request
         si_acquire_state = SI_ACQUIRE_IDLE;
+        last_win_start = 0;
         state = RRC_STATE_CELL_SELECTED;
       }
       break;
@@ -442,8 +438,8 @@ void rrc::plmn_search() {
 /* This is the NAS interface. When NAS requests to select a PLMN we have to
  * connect to either register or because there is pending higher layer traffic.
  */
-void rrc::plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {
+void rrc::plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, bool connect_request) {
-  connection_requested = true;
+  connection_requested = connect_request;
   plmn_select_rrc(plmn_id);
 }
 
@@ -451,37 +447,47 @@ void rrc::plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {
  * selected PLMN
  */
 void rrc::plmn_select_rrc(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {
+  pthread_mutex_lock(&mutex);
+
   // If already camping on the selected PLMN, select this cell
   if (state == RRC_STATE_IDLE || state == RRC_STATE_CONNECTED || state == RRC_STATE_PLMN_SELECTION) {
     if (phy->sync_status() && selected_plmn_id.mcc == plmn_id.mcc && selected_plmn_id.mnc == plmn_id.mnc) {
-      rrc_log->info("Already camping on selected PLMN, connecting...\n");
+      rrc_log->info("Already camping on selected PLMN\n");
     } else {
       selected_plmn_id = plmn_id;
 
-      if (serving_cell->plmn_equals(selected_plmn_id)) {
+      if (serving_cell->plmn_equals(selected_plmn_id) && serving_cell->in_sync) {
+        rrc_log->info("PLMN Id=%s selected, Selecting serving cell earfcn=%d, pci=%d, status=%d\n",
+                      plmn_id_to_string(plmn_id).c_str(), serving_cell->get_earfcn(), serving_cell->phy_cell.id,
+                      phy->sync_status());
+        if (!phy->sync_status()) {
           phy->cell_select(serving_cell->get_earfcn(), serving_cell->phy_cell);
+        }
       } else {
         bool found = false;
         for (uint32_t i=0;i<neighbour_cells.size() && !found;i++) {
           if (neighbour_cells[i]->plmn_equals(selected_plmn_id)) {
-            rrc_log->info("PLMN Id=%s selected, PCI=%d\n", plmn_id_to_string(plmn_id).c_str(), neighbour_cells[i]->get_pci());
+            rrc_log->info("PLMN Id=%s selected, Selecting neighbour cell PCI=%d\n", plmn_id_to_string(plmn_id).c_str(), neighbour_cells[i]->get_pci());
             phy->cell_select(neighbour_cells[i]->get_earfcn(), neighbour_cells[i]->phy_cell);
             found = true;
           }
         }
         if (!found) {
-          rrc_log->warning("Could not find any cell for the selected PLMN\n");
+          rrc_log->warning("Could not find any cell for the selected PLMN. Searching another PLMN\n");
-          state = RRC_STATE_IDLE;
+          plmn_search();
+          pthread_mutex_unlock(&mutex);
           return;
         }
       }
     }
 
+    si_acquire_state = SI_ACQUIRE_IDLE;
+    last_win_start = 0;
     state = RRC_STATE_CELL_SELECTING;
-    select_cell_timeout = 0;
   } else {
     rrc_log->warning("Requested PLMN select in incorrect state %s\n", rrc_state_text[state]);
   }
+  pthread_mutex_unlock(&mutex);
 }
 
 void rrc::set_serving_cell(uint32_t earfcn, uint32_t pci) {
@@ -494,6 +500,7 @@ void rrc::set_serving_cell(uint32_t earfcn, uint32_t pci) {
 }
 
 void rrc::set_serving_cell(uint32_t cell_idx) {
+
   if (cell_idx < neighbour_cells.size())
   {
     // Remove future serving cell from neighbours to make space for current serving cell
@@ -529,29 +536,39 @@ void rrc::set_serving_cell(uint32_t cell_idx) {
   }
 }
 
-void rrc::select_next_cell_in_plmn() {
+bool rrc::select_next_cell_in_plmn() {
   // Neighbour cells are sorted in descending order of RSRP
   for (uint32_t i = 0; i < neighbour_cells.size(); i++) {
-    if (neighbour_cells[i]->plmn_equals(selected_plmn_id) &&
+    if (/*TODO: CHECK that PLMN matches. Currently we don't receive SIB1 of neighbour cells
+         * neighbour_cells[i]->plmn_equals(selected_plmn_id) && */
         neighbour_cells[i]->in_sync) // matches S criteria
+    {
+      // If currently connected, verify cell selection criteria
+      if (!serving_cell->in_sync ||
+          (cell_selection_eval(neighbour_cells[i]->get_rsrp())  &&
+          neighbour_cells[i]->get_rsrp() > serving_cell->get_rsrp() + 5))
       {
         // Try to select Cell
-      phy->cell_select(neighbour_cells[i]->get_earfcn(), neighbour_cells[i]->phy_cell);
         set_serving_cell(i);
-      rrc_log->info("Selected cell PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
+        rrc_log->info("Selected cell idx=%d, PCI=%d, EARFCN=%d\n",
-                    serving_cell->phy_cell.id, serving_cell->get_earfcn(),
+                      i, serving_cell->phy_cell.id, serving_cell->get_earfcn());
-                    serving_cell->get_cell_id());
+        rrc_log->console("Selected cell PCI=%d, EARFCN=%d\n",
-      rrc_log->console("Selected cell PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
+                         serving_cell->phy_cell.id, serving_cell->get_earfcn());
-                       serving_cell->phy_cell.id, serving_cell->get_earfcn(),
+        phy->cell_select(serving_cell->get_earfcn(), serving_cell->phy_cell);
-                       serving_cell->get_cell_id());
+        si_acquire_state = SI_ACQUIRE_IDLE;
-      return;
+        last_win_start = 0;
+        state = RRC_STATE_CELL_SELECTING;
+        return true;
       }
     }
-  rrc_log->info("No more known cells. Starting again\n");
+  }
+  return false;
 }
 
 void rrc::new_phy_meas(float rsrp, float rsrq, uint32_t tti, int earfcn_i, int pci_i) {
 
+  pthread_mutex_lock(&mutex);
+
   if (earfcn_i < 0 || pci_i < 0) {
     earfcn_i = serving_cell->get_earfcn();
     pci_i    = serving_cell->phy_cell.id;
@@ -582,16 +599,22 @@ void rrc::new_phy_meas(float rsrp, float rsrq, uint32_t tti, int earfcn_i, int p
       }
     }
 
-    // Verify cell selection criteria with strongest neighbour cell (always first)
+    // Evaluate if we need to select a new cell
-    if (neighbour_cells.size() > 1                           &&
+    select_next_cell_in_plmn();
-        cell_selection_eval(neighbour_cells[0]->get_rsrp())  &&
-        neighbour_cells[0]->get_rsrp() > serving_cell->get_rsrp() + 5)
-    {
-      set_serving_cell(0);
-      rrc_log->info("Selecting best neighbour cell PCI=%d, rsrp=%.1f dBm\n", serving_cell->phy_cell.id, serving_cell->get_rsrp());
-      state = RRC_STATE_CELL_SELECTING;
-      phy->cell_select(serving_cell->get_earfcn(), serving_cell->phy_cell);
   }
+  pthread_mutex_unlock(&mutex);
+}
+
+// PHY indicates that has gone through all known EARFCN
+void rrc::earfcn_end() {
+  rrc_log->info("Finished searching cells in EARFCN set while in state %s\n", rrc_state_text[state]);
+
+  // If searching for PLMN, indicate NAS we scanned all frequencies
+  if (state == RRC_STATE_PLMN_SELECTION) {
+    nas->plmn_search_end();
+  } else {
+    rrc_log->info("Restarting Cell search...\n");
+    phy->cell_search_start();
   }
 }
 
@@ -600,51 +623,56 @@ void rrc::new_phy_meas(float rsrp, float rsrq, uint32_t tti, int earfcn_i, int p
  * new cell as current serving cell */
 void rrc::cell_camping(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
 
-  bool found = false;
   int cell_idx = -1;
+  bool found = true;
+
+  pthread_mutex_lock(&mutex);
 
   if (serving_cell->equals(earfcn, phy_cell.id)) {
     serving_cell->set_rsrp(rsrp);
-    found = true;
   } else {
     // Check if cell is in our list of neighbour cells 
     cell_idx = find_neighbour_cell(earfcn, phy_cell.id);
     if (cell_idx >= 0) {
       set_serving_cell(cell_idx);
       serving_cell->set_rsrp(rsrp);
-      found = true;
+    } else {
+      found = false;
+      if (!add_neighbour_cell(earfcn, phy_cell, rsrp)) {
+        rrc_log->info(
+            "No more space for neighbour cells (detected cell RSRP=%.1f dBm worse than current %d neighbours)\n",
+            rsrp,
+            NOF_NEIGHBOUR_CELLS);
+      } else {
+        set_serving_cell(earfcn, phy_cell.id);
+        serving_cell->set_rsrp(rsrp);
+      }
     }
   }
-  if (found) {
 
-    if (!serving_cell->has_sib1()) {
+  pthread_mutex_unlock(&mutex);
-      si_acquire_state = SI_ACQUIRE_SIB1;
+
-    } else if (state == RRC_STATE_PLMN_SELECTION) {
+  if (state == RRC_STATE_PLMN_SELECTION && serving_cell->has_sib1()) {
+    bool ret = false;
     for (uint32_t j = 0; j < serving_cell->sib1ptr()->N_plmn_ids; j++) {
-        nas->plmn_found(serving_cell->sib1ptr()->plmn_id[j].id, serving_cell->sib1ptr()->tracking_area_code);
+      ret |= nas->plmn_found(serving_cell->sib1ptr()->plmn_id[j].id, serving_cell->sib1ptr()->tracking_area_code);
     }
-      usleep(5000);
+    // If any of the PLMNs in this cell is selected, search next cell
+    if (!ret) {
       phy->cell_search_next();
     }
-  } else {
+  } else if (state < RRC_STATE_CONNECTING) {
-    // add to list of known cells and set current_cell
+    state = RRC_STATE_CELL_SELECTING;
-    if (!add_neighbour_cell(earfcn, phy_cell, rsrp)) {
-      rrc_log->info("No more space for neighbour cells (detected cell RSRP=%.1f dBm worse than current %d neighbours)\n", 
-                    rsrp, NOF_NEIGHBOUR_CELLS);
-      usleep(5000);
-      phy->cell_search_next();
-    } else {
-      set_serving_cell(earfcn, phy_cell.id);
-      si_acquire_state = SI_ACQUIRE_SIB1;
-    }
   }
 
   rrc_log->info("%s %s cell EARFCN=%d, PCI=%d, RSRP=%.1f dBm\n",
                 found?"Updating":"Adding",
-                cell_idx>=0?"neighbour":"serving",
+                cell_idx>=0?"neighbour":"serving", earfcn, phy_cell.id, rsrp);
-                serving_cell->get_earfcn(),
+
-                serving_cell->phy_cell.id,
+  if (ho_syncing && phy_cell.id == ho_target_pci) {
-                serving_cell->get_rsrp());
+    ho_synced(ho_target_pci);
+  }
+
 }
 
 bool sort_rsrp(cell_t *u1, cell_t *u2) {
@@ -668,6 +696,8 @@ void rrc::clean_neighbours()
   struct timeval now;
   gettimeofday(&now, NULL);
 
+  pthread_mutex_lock(&mutex);
+
   std::vector<cell_t*>::iterator it = neighbour_cells.begin();
   while(it != neighbour_cells.end()) {
     if ((*it)->timeout_secs(now) > NEIGHBOUR_TIMEOUT) {
@@ -677,6 +707,7 @@ void rrc::clean_neighbours()
       ++it;
     }
   }
+  pthread_mutex_unlock(&mutex);
 }
 
 // Sort neighbour cells by decreasing order of RSRP
@@ -695,6 +726,7 @@ void rrc::sort_neighbour_cells()
 
   std::sort(neighbour_cells.begin(), neighbour_cells.end(), sort_rsrp);
 
+  if (neighbour_cells.size() > 0) {
     char ordered[512];
     int n=0;
     n += snprintf(ordered, 512, "[pci=%d, rsrsp=%.2f", neighbour_cells[0]->phy_cell.id, neighbour_cells[0]->get_rsrp());
@@ -702,6 +734,9 @@ void rrc::sort_neighbour_cells()
       n += snprintf(&ordered[n], 512-n, " | pci=%d, rsrp=%.2f", neighbour_cells[i]->get_pci(), neighbour_cells[i]->get_rsrp());
     }
     rrc_log->info("Neighbours: %s]\n", ordered);
+  } else {
+    rrc_log->info("Neighbours: Empty\n");
+  }
 }
 
 bool rrc::add_neighbour_cell(cell_t *new_cell) {
@@ -762,18 +797,6 @@ int rrc::find_neighbour_cell(uint32_t earfcn, uint32_t pci) {
   return -1;
 }
 
-// PHY indicates that has gone through all known EARFCN
-void rrc::earfcn_end() {
-  rrc_log->info("Finished searching cells in EARFCN set while in state %s\n", rrc_state_text[state]);
-
-  // If searching for PLMN, indicate NAS we scanned all frequencies
-  if (state == RRC_STATE_PLMN_SELECTION) {
-    nas->plmn_search_end();
-  } else if (state == RRC_STATE_CONNECTED) {
-    leave_connected();
-  }
-}
-
 // Cell reselection in IDLE Section 5.2.4 of 36.304
 void rrc::cell_reselection_eval(float rsrp, float rsrq)
 {
@@ -826,7 +849,10 @@ float rrc::get_squal(float Qqualmeas) {
 
 // Detection of physical layer problems in RRC_CONNECTED (5.3.11.1)
 void rrc::out_of_sync() {
-  serving_cell->in_sync = false;
+  rrc_log->info("Received out-of-sync state %s. n310=%d, t311=%s, t310=%s\n",
+                rrc_state_text[state], n310_cnt,
+                mac_timers->timer_get(t311)->is_running()?"running":"stop",
+                mac_timers->timer_get(t310)->is_running()?"running":"stop");
   if (state == RRC_STATE_CONNECTED) {
     if (!mac_timers->timer_get(t311)->is_running() && !mac_timers->timer_get(t310)->is_running()) {
       n310_cnt++;
@@ -838,9 +864,23 @@ void rrc::out_of_sync() {
         n310_cnt = 0;
       }
     }
-  } else {
+  } else if (state != RRC_STATE_LEAVE_CONNECTED) {
+    if (!mac_timers->timer_get(t311)->is_running()) {
+      if (serving_cell->in_sync) {
+        rrc_log->info("Detected out-of-sync while in IDLE. Resetting sync\n");
         phy->sync_reset();
+      } else {
+        rrc_log->info("Detected out-of-sync while in IDLE. Selecting another cell in the PLMN\n");
+        if (!select_next_cell_in_plmn()) {
+          rrc_log->info("Could not find any available cell in this PLMN. Searching PLMN again.\n");
+          plmn_search();
+        }
+      }
+    } else {
+      rrc_log->info("Detected out-of-sync while T311 is running\n");
+    }
   }
+  serving_cell->in_sync = false;
 }
 
 // Recovery of physical layer problems (5.3.11.2)
@@ -899,6 +939,13 @@ void rrc::timer_expired(uint32_t timeout_id) {
   } else if (timeout_id == t311) {
     rrc_log->info("Timer T311 expired: Going to RRC IDLE\n");
     state = RRC_STATE_LEAVE_CONNECTED;
+  } else if (timeout_id == t300) {
+    rrc_log->info("Timer T300 expired: ConnectionRequest failed. Reset MAC and restablished RLC.\n");
+    rlc->reestablish();
+    mac->reset();
+    set_mac_default();
+    state = RRC_STATE_IDLE;
+    nas->plmn_search_end();
   } else if (timeout_id == t301) {
     if (state == RRC_STATE_IDLE) {
       rrc_log->info("Timer T301 expired: Already in IDLE.\n");
@@ -954,6 +1001,8 @@ void rrc::send_con_request() {
 
   ul_ccch_msg.msg.rrc_con_req.cause = LIBLTE_RRC_CON_REQ_EST_CAUSE_MO_SIGNALLING;
 
+  mac_timers->timer_get(t300)->reset();
+  mac_timers->timer_get(t300)->run();
   send_ul_ccch_msg();
 
 }
@@ -1021,6 +1070,8 @@ void rrc::send_con_restablish_request(LIBLTE_RRC_CON_REEST_REQ_CAUSE_ENUM cause,
   mac->reset();
   set_mac_default();
   phy->sync_reset();
+  si_acquire_state = SI_ACQUIRE_IDLE;
+  last_win_start = 0;
   state = RRC_STATE_CELL_SELECTING;
 }
 
@@ -1109,7 +1160,9 @@ bool rrc::ho_prepare() {
     int target_cell_idx = find_neighbour_cell(serving_cell->get_earfcn(), mob_reconf.mob_ctrl_info.target_pci);
     if (target_cell_idx < 0) {
       rrc_log->console("Received HO command to unknown PCI=%d\n", mob_reconf.mob_ctrl_info.target_pci);
-      rrc_log->error("Could not find target cell earfcn=%d, pci=%d\n", serving_cell->get_earfcn(), mob_reconf.mob_ctrl_info.target_pci);
+      rrc_log->error("Could not find target cell earfcn=%d, pci=%d\n",
+                     serving_cell->get_earfcn(),
+                     mob_reconf.mob_ctrl_info.target_pci);
       return false;
     }
 
@@ -1143,10 +1196,21 @@ bool rrc::ho_prepare() {
 
     rrc_log->info("Selecting new cell pci=%d\n", neighbour_cells[target_cell_idx]->get_pci());
     if (!phy->cell_handover(neighbour_cells[target_cell_idx]->phy_cell)) {
-      rrc_log->error("Could not synchronize with target cell pci=%d\n", neighbour_cells[target_cell_idx]->get_pci());
+      rrc_log->error("Could not synchronize with target cell pci=%d. Trying to return to source PCI\n",
+                     neighbour_cells[target_cell_idx]->get_pci());
+      ho_failed();
       return false;
     }
+    ho_target_pci = neighbour_cells[target_cell_idx]->phy_cell.id;
+    ho_syncing = true;
+  }
+  return true;
+}
 
+void rrc::ho_synced(uint32_t current_pci)
+{
+  ho_syncing = false;
+  if (current_pci == ho_target_pci) {
     if (mob_reconf.mob_ctrl_info.rach_cnfg_ded_present) {
       rrc_log->info("Starting non-contention based RA with preamble_idx=%d, mask_idx=%d\n",
                     mob_reconf.mob_ctrl_info.rach_cnfg_ded.preamble_index,
@@ -1163,7 +1227,7 @@ bool rrc::ho_prepare() {
       ncc = mob_reconf.sec_cnfg_ho.intra_lte.next_hop_chaining_count;
       if (mob_reconf.sec_cnfg_ho.intra_lte.key_change_ind) {
         rrc_log->console("keyChangeIndicator in securityConfigHO not supported\n");
-        return false;
+        return;
       }
       if (mob_reconf.sec_cnfg_ho.intra_lte.sec_alg_cnfg_present) {
         cipher_algo = (CIPHERING_ALGORITHM_ID_ENUM) mob_reconf.sec_cnfg_ho.intra_lte.sec_alg_cnfg.cipher_alg;
@@ -1180,8 +1244,11 @@ bool rrc::ho_prepare() {
 
     pdcp->config_security_all(k_rrc_enc, k_rrc_int, cipher_algo, integ_algo);
     send_rrc_con_reconfig_complete();
+  } else {
+    rrc_log->error("HO: Synchronized with incorrect cell. Target PCI=%d, current PCI=%d\n", ho_target_pci, current_pci);
+    ho_failed();
   }
-  return true;
+  return;
 }
 
 void rrc::ho_ra_completed(bool ra_successful) {
@@ -1219,7 +1286,8 @@ void rrc::ho_failed() {
 
   // Instruct PHY to resync with source PCI
   if (!phy->cell_handover(ho_src_cell.phy_cell)) {
-    rrc_log->error("Could not synchronize with target cell pci=%d\n", ho_src_cell.get_pci());
+    rrc_log->error("Could not synchronize with target cell pci=%d. Going to PLMN Search\n", ho_src_cell.get_pci());
+    plmn_search();
     return;
   }
 
@@ -1265,9 +1333,14 @@ void rrc::handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGU
     byte_buffer_t *nas_sdu;
     for (i = 0; i < reconfig->N_ded_info_nas; i++) {
       nas_sdu = pool_allocate;
+      if (nas_sdu) {
         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);
+      } else {
+        rrc_log->error("Fatal Error: Couldn't allocate PDU in handle_rrc_con_reconfig().\n");
+        return;
+      }
     }
   }
 }
@@ -1284,6 +1357,8 @@ void rrc::leave_connected()
 {
   rrc_log->console("RRC IDLE\n");
   rrc_log->info("Leaving RRC_CONNECTED state\n");
+  si_acquire_state = SI_ACQUIRE_IDLE;
+  last_win_start = 0;
   drb_up = false;
   measurements.reset();
   pdcp->reset();
@@ -1361,6 +1436,7 @@ void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu) {
 
   if(serving_cell->has_sib2()) {
     sysinfo_index++;
+    rrc_log->info("Increasing sysinfo_index=%d\n", sysinfo_index);
   }
 }
 
@@ -1623,6 +1699,8 @@ void rrc::parse_dl_ccch(byte_buffer_t *pdu) {
     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);
+      // Stop T300 timer
+      mac_timers->timer_get(t300)->stop();
       state = RRC_STATE_LEAVE_CONNECTED;
       break;
     case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_SETUP:
@@ -1925,14 +2003,15 @@ void rrc::apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2) {
                 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->timer_get(t300)->set(this, liblte_rrc_t300_num[sib2->ue_timers_and_constants.t300]);
   mac_timers->timer_get(t301)->set(this, liblte_rrc_t301_num[sib2->ue_timers_and_constants.t301]);
   mac_timers->timer_get(t310)->set(this, liblte_rrc_t310_num[sib2->ue_timers_and_constants.t310]);
   mac_timers->timer_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",
+  rrc_log->info("Set Constants and Timers: N310=%d, N311=%d, t300=%d, t301=%d, t310=%d, t311=%d\n",
-                N310, N311, mac_timers->timer_get(t301)->get_timeout(),
+                N310, N311, mac_timers->timer_get(t300)->get_timeout(), mac_timers->timer_get(t301)->get_timeout(),
                 mac_timers->timer_get(t310)->get_timeout(), mac_timers->timer_get(t311)->get_timeout());
 
 }
@@ -2169,6 +2248,10 @@ void rrc::apply_rr_config_dedicated(LIBLTE_RRC_RR_CONFIG_DEDICATED_STRUCT *cnfg)
 }
 
 void rrc::handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup) {
+
+  // Stop T300 timer
+  mac_timers->timer_get(t300)->stop();
+
   // Apply the Radio Resource configuration
   apply_rr_config_dedicated(&setup->rr_cnfg);
 }
@@ -2379,7 +2462,10 @@ void rrc::rrc_meas::reset()
   filter_k_rsrp = liblte_rrc_filter_coefficient_num[LIBLTE_RRC_FILTER_COEFFICIENT_FC4];
   filter_k_rsrq = liblte_rrc_filter_coefficient_num[LIBLTE_RRC_FILTER_COEFFICIENT_FC4];
   objects.clear();
-  active.clear();
+  std::map<uint32_t, meas_t>::iterator iter = active.begin();
+  while (iter != active.end()) {
+    remove_meas_id(iter++);
+  }
   reports_cfg.clear();
   phy->meas_reset();
   bzero(&pcell_measurement, sizeof(meas_value_t));
@@ -2750,7 +2836,7 @@ void rrc::rrc_meas::remove_meas_id(uint32_t measId) {
   if (active.count(measId)) {
     mac_timers->timer_get(active[measId].periodic_timer)->stop();
     mac_timers->timer_release_id(active[measId].periodic_timer);
-    log_h->info("MEAS: Removed measId=%d\n", measId);
+    log_h->info("MEAS: Removed measId=%d, timer_id=%d\n", measId, active[measId].periodic_timer);
     active.erase(measId);
   } else {
     log_h->warning("MEAS: Removing unexistent measId=%d\n", measId);
@@ -2760,7 +2846,7 @@ void rrc::rrc_meas::remove_meas_id(uint32_t measId) {
 void rrc::rrc_meas::remove_meas_id(std::map<uint32_t, meas_t>::iterator it) {
   mac_timers->timer_get(it->second.periodic_timer)->stop();
   mac_timers->timer_release_id(it->second.periodic_timer);
-  log_h->info("MEAS: Removed measId=%d\n", it->first);
+  log_h->info("MEAS: Removed measId=%d, timer_id=%d\n", it->first, it->second.periodic_timer);
   active.erase(it);
 }
 
@@ -2900,12 +2986,15 @@ void rrc::rrc_meas::parse_meas_config(LIBLTE_RRC_MEAS_CONFIG_STRUCT *cfg)
       } else {
         is_new = true;
         active[measId->meas_id].periodic_timer   = mac_timers->timer_get_unique_id();
+        if (!active[measId->meas_id].periodic_timer) {
+          log_h->error("Could not get unique timer id\n");
+        }
       }
       active[measId->meas_id].object_id = measId->meas_obj_id;
       active[measId->meas_id].report_id = measId->rep_cnfg_id;
-      log_h->info("MEAS: %s measId=%d, measObjectId=%d, reportConfigId=%d, nof_values=%d\n",
+      log_h->info("MEAS: %s measId=%d, measObjectId=%d, reportConfigId=%d, timer_id=%d, nof_values=%d\n",
                   is_new?"Added":"Updated", measId->meas_id, measId->meas_obj_id, measId->rep_cnfg_id,
-                  active[measId->meas_id].cell_values.size());
+                  active[measId->meas_id].periodic_timer, active[measId->meas_id].cell_values.size());
     }
   }
 

srsue/test/mac/mac_test.cc

@@ -451,12 +451,12 @@ int main(int argc, char *argv[])
     exit(1);
   }
 
-  std::vector<srslte::log*> phy_log;
+  std::vector<srslte::log_filter*> phy_log;
 
   srslte::log_filter *mylog = new srslte::log_filter("PHY");
   char tmp[16];
   sprintf(tmp, "PHY%d",0);
-  phy_log.push_back((srslte::log*) mylog);
+  phy_log.push_back(mylog);
 
   switch (prog_args.verbose) {
     case 1:

srsue/test/upper/nas_test.cc

@@ -76,12 +76,13 @@ public:
     printf("NAS generated SDU (len=%d):\n", sdu->N_bytes);
     last_sdu_len = sdu->N_bytes;
     srslte_vec_fprint_byte(stdout, sdu->msg, sdu->N_bytes);
+    byte_buffer_pool::get_instance()->deallocate(sdu);
   }
   std::string get_rb_name(uint32_t lcid) { return std::string("lcid"); }
   uint32_t get_last_sdu_len() { return last_sdu_len; }
 
   void plmn_search() {};
-  void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {};
+  void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, bool con_req) {};
 
   uint16_t get_mcc() { return mcc; }
   uint16_t get_mnc() { return mnc; }

srsue/ue.conf.example

@@ -172,6 +172,8 @@ enable = false
 # cfo_correct_tol_hz:   Tolerance (in Hz) for digial CFO compensation. Lower tolerance means that
 #                       a new table will be generated more often.
 #
+# cfo_is_doppler:        Assume detected CFO is doppler and correct the UL in the same direction. If disabled, the CFO is assumed
+#                        to be caused by the local oscillator and the UL correction is in the opposite direction. Default assumes oscillator.
 # cfo_pss_ema:           CFO Exponential Moving Average coefficient for PSS estimation during TRACK.
 # cfo_ref_ema:           CFO Exponential Moving Average coefficient for RS estimation after PSS acquisition
 # cfo_ref_mask:          Bitmask for subframes on which to run RS estimation (set to 0 to disable, default sf=[1, 5])
@@ -182,6 +184,9 @@ enable = false
 # cfo_loop_pss_timeout:  After the PSS estimation is below cfo_loop_pss_tol for cfo_loop_pss_timeout times consecutively,
 #                        RS adjustments are allowed.
 #
+# pdsch_csi_enabled:     Stores the Channel State Information and uses it for weightening the softbits. It is only
+#                        compatible with TM1. It is False by default.
+#
 #####################################################################
 [expert]
 #ip_netmask          = 255.255.255.0
@@ -205,8 +210,10 @@ enable = false
 #pregenerate_signals = false
 #metrics_csv_enable  = false
 #metrics_csv_filename = /tmp/ue_metrics.csv
+#pdsch_csi_enabled  = true     # Caution! Only TM1 supported!
 
 # CFO related values
+#cfo_is_doppler      = false
 #cfo_integer_enabled = false
 #cfo_correct_tol_hz  = 1.0
 #cfo_pss_ema         = 0.05