Merge latest next into next. Small conflict due to a debug print.

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

@@ -30,6 +30,8 @@
 #include <pthread.h>
 #include <vector>
 #include <stack>
+#include <map>
+#include <string>
 #include <algorithm>
 
 /*******************************************************************************
@@ -70,14 +72,23 @@ public:
       delete available.top();
       available.pop();
     }
+
+    for (uint32_t i = 0; i < used.size(); i++) {
+      delete used[i];
+    }
   }
   
   void print_all_buffers()
   {
     printf("%d buffers in queue\n", (int) used.size());
 #ifdef SRSLTE_BUFFER_POOL_LOG_ENABLED
+    std::map<std::string, uint32_t> buffer_cnt;
     for (uint32_t i=0;i<used.size();i++) {
-      printf("%s\n", strlen(used[i]->debug_name)?used[i]->debug_name:"Undefined");
+      buffer_cnt[strlen(used[i]->debug_name)?used[i]->debug_name:"Undefined"]++;
+    }
+    std::map<std::string, uint32_t>::iterator it;
+    for (it = buffer_cnt.begin(); it != buffer_cnt.end(); it++) {
+      printf(" - %dx %s\n", it->second, it->first.c_str());
     }
 #endif
   }
@@ -166,6 +177,9 @@ public:
     pool->deallocate(b);
     b = NULL;
   }
+  void print_all_buffers() {
+    pool->print_all_buffers();
+  }
 private:
   buffer_pool<byte_buffer_t> *pool; 
 };

lib/include/srslte/common/common.h

@@ -63,7 +63,7 @@
 #define SRSLTE_MAX_BUFFER_SIZE_BYTES 12756
 #define SRSLTE_BUFFER_HEADER_OFFSET  1024
 
-//#define SRSLTE_BUFFER_POOL_LOG_ENABLED
+#define SRSLTE_BUFFER_POOL_LOG_ENABLED
 
 #ifdef SRSLTE_BUFFER_POOL_LOG_ENABLED
 #define pool_allocate (pool->allocate(__FUNCTION__))
@@ -96,6 +96,8 @@ static const char error_text[ERROR_N_ITEMS][20] = { "None",
                                                     "Can't start",
                                                     "Already started"};
 
+//#define ENABLE_TIMESTAMP
+
 /******************************************************************************
  * Byte and Bit buffers
  *
@@ -147,33 +149,27 @@ public:
     }
     long get_latency_us()
     {
+#ifdef ENABLE_TIMESTAMP
       if(!timestamp_is_set)
         return 0;
       gettimeofday(&timestamp[2], NULL); 
       get_time_interval(timestamp);
       return timestamp[0].tv_usec;
+#else
+      return 0;
+#endif
     }
     
     void set_timestamp() 
     {
+#ifdef ENABLE_TIMESTAMP
       gettimeofday(&timestamp[1], NULL);
       timestamp_is_set = true;
+#endif
     }
 
 private:
 
-  
-  void get_time_interval(struct timeval * tdata) {
-
-    tdata[0].tv_sec = tdata[2].tv_sec - tdata[1].tv_sec;
-    tdata[0].tv_usec = tdata[2].tv_usec - tdata[1].tv_usec;
-    if (tdata[0].tv_usec < 0) {
-      tdata[0].tv_sec--;
-      tdata[0].tv_usec += 1000000;
-    }
-  }
-
-  
     struct timeval timestamp[3];
     bool           timestamp_is_set; 
     byte_buffer_t *next;
@@ -215,15 +211,21 @@ struct bit_buffer_t{
     }
     long get_latency_us()
     {
+#ifdef ENABLE_TIMESTAMP
       if(!timestamp_is_set)
         return 0;
       gettimeofday(&timestamp[2], NULL); 
       return timestamp[0].tv_usec;
+#else
+      return 0;
+#endif
     }
     void set_timestamp() 
     {
+#ifdef ENABLE_TIMESTAMP
       gettimeofday(&timestamp[1], NULL);
       timestamp_is_set = true;
+#endif
     }
 
 private: 

lib/include/srslte/common/interfaces_common.h

@@ -30,6 +30,7 @@
 #include "srslte/common/timers.h"
 #include "srslte/common/security.h"
 #include "srslte/asn1/liblte_rrc.h"
+#include <string>
 
 
 namespace srslte {
@@ -37,11 +38,13 @@ namespace srslte {
 class srslte_nas_config_t
 {
 public:
-  srslte_nas_config_t(uint32_t lcid_ = 0)
+  srslte_nas_config_t(uint32_t lcid_ = 0, std::string apn_ = "")
-    :lcid(lcid_)
+    :lcid(lcid_),
+     apn(apn_)
     {}
 
   uint32_t    lcid;
+  std::string apn;
 };
 
 

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;
 };
 
@@ -160,7 +160,7 @@ public:
   virtual void in_sync() = 0;
   virtual void out_of_sync() = 0;
   virtual void earfcn_end() = 0;
-  virtual void cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) = 0;
+  virtual void cell_camping(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp = NAN) = 0;
   virtual void new_phy_meas(float rsrp, float rsrq, uint32_t tti, int earfcn = -1, int pci = -1) = 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,12 @@ 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;
 } phy_args_t; 
 
 
@@ -580,9 +581,8 @@ public:
 
   /* Cell search and selection procedures */
   virtual void cell_search_start() = 0;
-  virtual void cell_search_stop() = 0;
   virtual void cell_search_next() = 0;
-  virtual bool cell_select(uint32_t earfcn, srslte_cell_t cell) = 0;
+  virtual void cell_select(uint32_t earfcn, srslte_cell_t cell) = 0;
   virtual bool cell_handover(srslte_cell_t cell) = 0;
 
   /* Is the PHY downlink synchronized? */

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/rf/rf.h

@@ -60,6 +60,7 @@ typedef struct {
     SRSLTE_RF_ERROR_LATE,
     SRSLTE_RF_ERROR_UNDERFLOW,
     SRSLTE_RF_ERROR_OVERFLOW,
+    SRSLTE_RF_ERROR_RX,
     SRSLTE_RF_ERROR_OTHER
   } type;
   int opt;

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/simd.h

@@ -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/include/srslte/radio/radio.h

@@ -72,6 +72,8 @@ namespace srslte {
         trace_enabled           = false; 
         tti                     = 0; 
         agc_enabled             = false;
+        radio_is_streaming      = false;
+        is_initialized          = false;
       };
       
       bool init(char *args = NULL, char *devname = NULL, uint32_t nof_channels = 1);
@@ -119,13 +121,13 @@ namespace srslte {
       
       void start_trace();
       void write_trace(std::string filename);
-      void start_rx(bool now = false);
-      void stop_rx();
 
       void set_tti(uint32_t tti);
 
       bool is_first_of_burst();
 
+      bool is_init();
+
       void register_error_handler(srslte_rf_error_handler_t h);
       
     protected:
@@ -168,6 +170,9 @@ namespace srslte {
       uint32_t tti;
       bool agc_enabled;
 
+      bool is_initialized = true;;
+      bool radio_is_streaming;
+
       uint32_t saved_nof_channels;
       char saved_args[128];
       char saved_devname[128];

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/log_filter.cc

@@ -40,6 +40,7 @@ log_filter::log_filter()
   do_tti      = false;
   time_src    = NULL;
   time_format = TIME;
+  logger_h    = NULL;
 }
 
 log_filter::log_filter(std::string layer)

lib/src/common/logger_file.cc

@@ -35,6 +35,7 @@ namespace srslte{
 
 logger_file::logger_file()
   :inited(false)
+  ,logfile(NULL)
   ,not_done(true)
   ,cur_length(0)
   ,max_length(0)
@@ -46,9 +47,11 @@ logger_file::~logger_file() {
   if(inited) {
     wait_thread_finish();
     flush();
+    if (logfile) {
       fclose(logfile);
     }
   }
+}
 
 void logger_file::init(std::string file, int max_length_) {
   pthread_mutex_init(&mutex, NULL); 

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/common/pdu_queue.cc

@@ -76,10 +76,14 @@ void pdu_queue::deallocate(uint8_t* pdu)
  */ 
 void pdu_queue::push(uint8_t *ptr, uint32_t len, uint32_t tstamp)
 {
+  if (ptr) {
     pdu_t *pdu  = (pdu_t*) ptr;
     pdu->len    = len;
     pdu->tstamp = tstamp;
     pdu_q.push(pdu);
+  } else {
+    log_h->warning("Error pushing pdu: ptr is empty\n");
+  }
 }
 
 bool pdu_queue::process_pdus()

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,6 +208,7 @@ 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 (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));
     }
@@ -216,6 +219,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)
 {
@@ -540,27 +545,32 @@ void chest_interpolate_noise_est(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, ui
       }
     } 
   }
-
-  /* Compute RSRP for the channel estimates in this port */
-  uint32_t npilots = SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id);
-  q->rsrp[rxant_id][port_id] = srslte_vec_avg_power_cf(q->pilot_estimates, npilots);
-  q->rssi[rxant_id][port_id] = srslte_chest_dl_rssi(q, input, port_id);
 }
 
 int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, uint32_t sf_idx, uint32_t port_id, uint32_t rxant_id)
 {
+  uint32_t npilots = SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id);
+
   /* Get references from the input signal */
   srslte_refsignal_cs_get_sf(q->cell, port_id, input, q->pilot_recv_signal);
   
   /* Use the known CSR signal to compute Least-squares estimates */
   srslte_vec_prod_conj_ccc(q->pilot_recv_signal, q->csr_refs.pilots[port_id/2][sf_idx], 
-              q->pilot_estimates, SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id)); 
+              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_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)
 {
 
@@ -619,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;
@@ -706,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) {
@@ -716,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/dft/dft_fftw.c

@@ -45,6 +45,7 @@
 #define FFTW_TYPE 0
 #endif
 
+pthread_mutex_t fft_mutex = PTHREAD_MUTEX_INITIALIZER;
 
 void srslte_dft_load() {
 #ifdef FFTW_WISDOM_FILE
@@ -58,10 +59,12 @@ void srslte_dft_exit() {
 #ifdef FFTW_WISDOM_FILE
   fftwf_export_wisdom_to_filename(FFTW_WISDOM_FILE);
 #endif
+  fftwf_cleanup();
 }
 
 int srslte_dft_plan(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir,
              srslte_dft_mode_t mode) {
+  bzero(plan, sizeof(srslte_dft_plan_t));
   if(mode == SRSLTE_DFT_COMPLEX){
     return srslte_dft_plan_c(plan,dft_points,dir);
   } else {
@@ -99,10 +102,15 @@ int srslte_dft_replan_guru_c(srslte_dft_plan_t *plan, const int new_dft_points,
   const fftwf_iodim iodim = {new_dft_points, istride, ostride};
   const fftwf_iodim howmany_dims = {how_many, idist, odist};
 
+  pthread_mutex_lock(&fft_mutex);
+
   /* Destroy current plan */
   fftwf_destroy_plan(plan->p);
 
   plan->p = fftwf_plan_guru_dft(1, &iodim, 1, &howmany_dims, in_buffer, out_buffer, sign, FFTW_TYPE);
+
+  pthread_mutex_unlock(&fft_mutex);
+
   if (!plan->p) {
     return -1;
   }
@@ -114,11 +122,15 @@ int srslte_dft_replan_guru_c(srslte_dft_plan_t *plan, const int new_dft_points,
 
 int srslte_dft_replan_c(srslte_dft_plan_t *plan, const int new_dft_points) {
   int sign = (plan->dir == SRSLTE_DFT_FORWARD) ? FFTW_FORWARD : FFTW_BACKWARD;
+
+  pthread_mutex_lock(&fft_mutex);
   if (plan->p) {
     fftwf_destroy_plan(plan->p);
     plan->p = NULL;
   }
   plan->p = fftwf_plan_dft_1d(new_dft_points, plan->in, plan->out, sign, FFTW_TYPE);
+  pthread_mutex_unlock(&fft_mutex);
+
   if (!plan->p) {
     return -1;
   }
@@ -134,10 +146,14 @@ int srslte_dft_plan_guru_c(srslte_dft_plan_t *plan, const int dft_points, srslte
   const fftwf_iodim iodim = {dft_points, istride, ostride};
   const fftwf_iodim howmany_dims = {how_many, idist, odist};
 
+  pthread_mutex_lock(&fft_mutex);
+
   plan->p = fftwf_plan_guru_dft(1, &iodim, 1, &howmany_dims, in_buffer, out_buffer, sign, FFTW_TYPE);
   if (!plan->p) {
     return -1;
   }
+  pthread_mutex_unlock(&fft_mutex);
+
   plan->size = dft_points;
   plan->init_size = plan->size;
   plan->mode = SRSLTE_DFT_COMPLEX;
@@ -154,8 +170,14 @@ int srslte_dft_plan_guru_c(srslte_dft_plan_t *plan, const int dft_points, srslte
 
 int srslte_dft_plan_c(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir) {
   allocate(plan,sizeof(fftwf_complex),sizeof(fftwf_complex), dft_points);
+
+  pthread_mutex_lock(&fft_mutex);
+
   int sign = (dir == SRSLTE_DFT_FORWARD) ? FFTW_FORWARD : FFTW_BACKWARD;
   plan->p = fftwf_plan_dft_1d(dft_points, plan->in, plan->out, sign, FFTW_TYPE);
+
+  pthread_mutex_unlock(&fft_mutex);
+
   if (!plan->p) {
     return -1;
   }
@@ -175,11 +197,15 @@ int srslte_dft_plan_c(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_
 
 int srslte_dft_replan_r(srslte_dft_plan_t *plan, const int new_dft_points) {
   int sign = (plan->dir == SRSLTE_DFT_FORWARD) ? FFTW_R2HC : FFTW_HC2R;
+
+  pthread_mutex_lock(&fft_mutex);
   if (plan->p) {
     fftwf_destroy_plan(plan->p);
     plan->p = NULL;
   }
   plan->p = fftwf_plan_r2r_1d(new_dft_points, plan->in, plan->out, sign, FFTW_TYPE);
+  pthread_mutex_unlock(&fft_mutex);
+
   if (!plan->p) {
     return -1;
   }
@@ -190,7 +216,11 @@ int srslte_dft_replan_r(srslte_dft_plan_t *plan, const int new_dft_points) {
 int srslte_dft_plan_r(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir) {
   allocate(plan,sizeof(float),sizeof(float), dft_points);
   int sign = (dir == SRSLTE_DFT_FORWARD) ? FFTW_R2HC : FFTW_HC2R;
+
+  pthread_mutex_lock(&fft_mutex);
   plan->p = fftwf_plan_r2r_1d(dft_points, plan->in, plan->out, sign, FFTW_TYPE);
+  pthread_mutex_unlock(&fft_mutex);
+
   if (!plan->p) {
     return -1;
   }
@@ -309,11 +339,15 @@ void srslte_dft_run_r(srslte_dft_plan_t *plan, const float *in, float *out) {
 void srslte_dft_plan_free(srslte_dft_plan_t *plan) {
   if (!plan) return;
   if (!plan->size) return;
+
+  pthread_mutex_lock(&fft_mutex);
   if (!plan->is_guru) {
     if (plan->in) fftwf_free(plan->in);
     if (plan->out) fftwf_free(plan->out);
   }
   if (plan->p) fftwf_destroy_plan(plan->p);
+  pthread_mutex_unlock(&fft_mutex);
+
   bzero(plan, sizeof(srslte_dft_plan_t));
 }
 

lib/src/phy/dft/ofdm.c

@@ -128,6 +128,8 @@ int srslte_ofdm_init_mbsfn_(srslte_ofdm_t *q, srslte_cp_t cp, cf_t *in_buffer, c
   if (sf_type == SRSLTE_SF_MBSFN) {
     q->mbsfn_subframe = true;
     q->non_mbsfn_region = 2; // default set to 2
+  } else {
+    q->mbsfn_subframe = false;
   }
   
   return SRSLTE_SUCCESS;

lib/src/phy/dft/test/ofdm_test.c

@@ -171,5 +171,8 @@ int main(int argc, char **argv) {
 
     n_prb++;
   }
+
+  srslte_dft_exit();
+
   exit(0);
 }

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/rm_turbo_test.c

@@ -198,6 +198,8 @@ int main(int argc, char **argv) {
   }
 
   srslte_rm_turbo_free_tables();
+  free(rm_bits_s);
+  free(rm_bits_f);
   free(rm_bits);
   free(rm_bits2);
   free(rm_bits2_bytes);

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/cqi.c

@@ -407,7 +407,7 @@ float srslte_cqi_to_coderate(uint32_t cqi) {
  * Table III. 
 */
 // From paper
-static float cqi_to_snr_table[15] = { 1.95, 4, 6, 8, 10, 11.95, 14.05, 16, 17.9, 19.9, 21.5, 23.45, 25.0, 27.30, 29};
+static float cqi_to_snr_table[15] = { 1.95, 4, 6, 8, 10, 11.95, 14.05, 16, 17.9, 20.9, 22.5, 24.75, 25.5, 27.30, 29};
 
 // From experimental measurements @ 5 MHz 
 //static float cqi_to_snr_table[15] = { 1, 1.75, 3, 4, 5, 6, 7.5, 9, 11.5, 13.0, 15.0, 18, 20, 22.5, 26.5};

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);
   }
@@ -96,6 +96,19 @@ static void log_underflow(rf_uhd_handler_t *h) {
   }
 }
 
+static void log_rx_error(rf_uhd_handler_t *h) {
+  if (h->uhd_error_handler) {
+    char error_string[512];
+    uhd_usrp_last_error(h->usrp, error_string, 512);
+    fprintf(stderr, "USRP reported the following error: %s\n", error_string);
+
+    srslte_rf_error_t error;
+    bzero(&error, sizeof(srslte_rf_error_t));
+    error.type = SRSLTE_RF_ERROR_RX;
+    h->uhd_error_handler(error);
+  }
+}
+
 static void* async_thread(void *h) {
   rf_uhd_handler_t *handler = (rf_uhd_handler_t*) h; 
   uhd_async_metadata_handle md; 
@@ -334,6 +347,7 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
       perror("malloc");
       return -1; 
     }
+    bzero(handler, sizeof(rf_uhd_handler_t));
     *h = handler; 
 
     /* Set priority to UHD threads */
@@ -557,11 +571,12 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
     uhd_meta_range_free(&gain_range);
 
     // Start low priority thread to receive async commands
+    /*
     handler->async_thread_running = true;
     if (pthread_create(&handler->async_thread, NULL, async_thread, handler)) {
       perror("pthread_create");
       return -1; 
-    }
+    }*/
 
     /* Restore priorities  */
     uhd_set_thread_priority(0, false);
@@ -738,6 +753,7 @@ int rf_uhd_recv_with_time_multi(void *h,
                                              num_rx_samples, md, 1.0, false, &rxd_samples);
       if (error) {
         fprintf(stderr, "Error receiving from UHD: %d\n", error);
+        log_rx_error(handler);
         return -1; 
       }
 
@@ -760,8 +776,12 @@ int rf_uhd_recv_with_time_multi(void *h,
       }
     }
   } else {
-    return uhd_rx_streamer_recv(handler->rx_stream, data, 
+    uhd_error error = uhd_rx_streamer_recv(handler->rx_stream, data, nsamples, md, 0.0, false, &rxd_samples);
-                                nsamples, md, 0.0, false, &rxd_samples);
+    if (error) {
+      fprintf(stderr, "Error receiving from UHD: %d\n", error);
+      log_rx_error(handler);
+      return -1;
+    }
   }
   if (secs && frac_secs) {
     uhd_rx_metadata_time_spec(handler->rx_md_first, secs, frac_secs);

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++) {
       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/sync/test/pss_usrp.c

@@ -74,7 +74,7 @@ void usage(char *prog) {
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "adgetvsfil")) != -1) {
+  while ((opt = getopt(argc, argv, "adgetvnsfil")) != -1) {
     switch (opt) {
     case 'a':
       rf_args = argv[optind];

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/ue/ue_sync.c

@@ -756,6 +756,8 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
             srslte_agc_process(&q->agc, input_buffer[0], q->sf_len);        
           }
 
+          INFO("SYNC FIND: sf_idx=%d, ret=%d, next_state=%d\n", q->sf_idx, ret, q->state);
+
         break;
         case SF_TRACK:
          
@@ -817,6 +819,9 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
 
             q->frame_total_cnt++;
           }
+
+          INFO("SYNC TRACK: sf_idx=%d, ret=%d, next_state=%d\n", q->sf_idx, ret, q->state);
+
         break;
       }
     }

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

@@ -157,6 +157,8 @@ int main(int argc, char **argv) {
   if(test_dft(in) != 0)
     return -1;
 
+  srslte_dft_exit();
+
   free(in);
 	printf("Done\n");
 	exit(0);

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

@@ -71,9 +71,14 @@ bool radio::init(char *args, char *devname, uint32_t nof_channels)
   }
   saved_nof_channels = nof_channels;
 
+  is_initialized = true;
   return true;
 }
 
+bool radio::is_init() {
+  return is_initialized;
+}
+
 void radio::stop() 
 {
   srslte_rf_close(&rf_device);
@@ -82,11 +87,8 @@ void radio::stop()
 void radio::reset()
 {
   printf("Resetting Radio...\n");
-  srslte_rf_close(&rf_device);
+  srslte_rf_stop_rx_stream(&rf_device);
-  sleep(3);
+  radio_is_streaming = false;
-  if (srslte_rf_open_devname(&rf_device, saved_devname, saved_args, saved_nof_channels)) {
-    fprintf(stderr, "Error opening RF device\n");
-  }
 }
 
 void radio::set_manual_calibration(rf_cal_t* calibration)
@@ -141,6 +143,10 @@ bool radio::rx_at(void* buffer, uint32_t nof_samples, srslte_timestamp_t rx_time
 
 bool radio::rx_now(void* buffer[SRSLTE_MAX_PORTS], uint32_t nof_samples, srslte_timestamp_t* rxd_time)
 {
+  if (!radio_is_streaming) {
+    srslte_rf_start_rx_stream(&rf_device, false);
+    radio_is_streaming = true;
+  }
   if (srslte_rf_recv_with_time_multi(&rf_device, buffer, nof_samples, true,
     rxd_time?&rxd_time->full_secs:NULL, rxd_time?&rxd_time->frac_secs:NULL) > 0) {
     return true; 
@@ -295,7 +301,9 @@ 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)
@@ -446,16 +454,6 @@ void radio::set_tx_srate(double srate)
   tx_adv_sec = nsamples/cur_tx_srate;
 }
 
-void radio::start_rx(bool now)
-{
-  srslte_rf_start_rx_stream(&rf_device, now);
-}
-
-void radio::stop_rx()
-{
-  srslte_rf_stop_rx_stream(&rf_device);
-}
-
 void radio::register_error_handler(srslte_rf_error_handler_t h)
 {
   srslte_rf_register_error_handler(&rf_device, h);

lib/src/radio/radio_multi.cc

@@ -37,6 +37,7 @@ bool radio_multi::init_multi(uint32_t nof_rx_antennas, char* args, char* devname
     strncpy(saved_devname, devname, 127);
   }
 
+  is_initialized = true;
   return true;    
 }
 
@@ -46,6 +47,10 @@ bool radio_multi::rx_now(cf_t *buffer[SRSLTE_MAX_PORTS], uint32_t nof_samples, s
   for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
     ptr[i] = buffer[i];
   }
+  if (!radio_is_streaming) {
+    srslte_rf_start_rx_stream(&rf_device, false);
+    radio_is_streaming = true;
+  }
   if (srslte_rf_recv_with_time_multi(&rf_device, ptr, nof_samples, true,
     rxd_time?&rxd_time->full_secs:NULL, rxd_time?&rxd_time->frac_secs:NULL) > 0) {
     return true; 

lib/src/upper/pdcp.cc

@@ -92,8 +92,12 @@ bool pdcp::is_drb_enabled(uint32_t lcid)
 
 void pdcp::write_sdu(uint32_t lcid, byte_buffer_t *sdu)
 {
-  if(valid_lcid(lcid))
+  if(valid_lcid(lcid)) {
     pdcp_array[lcid].write_sdu(sdu);
+  } else {
+    pdcp_log->warning("Writing sdu: lcid=%d. Deallocating sdu\n", lcid);
+    byte_buffer_pool::get_instance()->deallocate(sdu);
+  }
 }
 
 void pdcp::add_bearer(uint32_t lcid, srslte_pdcp_config_t cfg)
@@ -149,8 +153,12 @@ void pdcp::enable_encryption(uint32_t lcid)
 *******************************************************************************/
 void pdcp::write_pdu(uint32_t lcid, byte_buffer_t *pdu)
 {
-  if(valid_lcid(lcid))
+  if(valid_lcid(lcid)) {
     pdcp_array[lcid].write_pdu(pdu);
+  } else {
+    pdcp_log->warning("Writing pdu: lcid=%d. Deallocating pdu\n", lcid);
+    byte_buffer_pool::get_instance()->deallocate(pdu);
+  }
 }
 
 void pdcp::write_pdu_bcch_bch(byte_buffer_t *sdu)

lib/src/upper/rlc.cc

@@ -187,10 +187,14 @@ void rlc::write_pdu_bcch_bch(uint8_t *payload, uint32_t nof_bytes)
   rlc_log->info_hex(payload, nof_bytes, "BCCH BCH message received.");
   dl_tput_bytes[0] += 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_bcch_bch(buf);
+  } else {
+    rlc_log->error("Fatal error: Out of buffers from the pool in write_pdu_bcch_bch()\n");
+  }
 }
 
 void rlc::write_pdu_bcch_dlsch(uint8_t *payload, uint32_t nof_bytes)
@@ -198,10 +202,14 @@ void rlc::write_pdu_bcch_dlsch(uint8_t *payload, uint32_t nof_bytes)
   rlc_log->info_hex(payload, nof_bytes, "BCCH TXSCH message received.");
   dl_tput_bytes[0] += 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_bcch_dlsch(buf);
+  } else {
+    rlc_log->error("Fatal error: Out of buffers from the pool in write_pdu_bcch_dlsch()\n");
+  }
 }
 
 void rlc::write_pdu_pcch(uint8_t *payload, uint32_t nof_bytes)
@@ -209,10 +217,14 @@ void rlc::write_pdu_pcch(uint8_t *payload, uint32_t nof_bytes)
   rlc_log->info_hex(payload, nof_bytes, "PCCH message received.");
   dl_tput_bytes[0] += 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_pcch(buf);
+  } else {
+    rlc_log->error("Fatal error: Out of buffers from the pool in write_pdu_pcch()\n");
+  }
 }
 
 /*******************************************************************************
@@ -281,6 +293,7 @@ void rlc::add_bearer(uint32_t lcid, srslte_rlc_config_t cnfg)
 bool rlc::valid_lcid(uint32_t lcid)
 {
   if(lcid >= SRSLTE_N_RADIO_BEARERS) {
+    rlc_log->warning("Invalid LCID=%d\n", lcid);
     return false;
   } else if(!rlc_array[lcid].active()) {
     return false;

lib/src/upper/rlc_am.cc

@@ -72,6 +72,14 @@ rlc_am::~rlc_am()
 {
   // reset RLC and dealloc SDUs
   stop();
+
+  if(rx_sdu) {
+    pool->deallocate(rx_sdu);
+  }
+
+  if(tx_sdu) {
+    pool->deallocate(tx_sdu);
+  }
 }
 
 void rlc_am::init(srslte::log                 *log_,
@@ -190,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());
 }
 
 /****************************************************************************
@@ -265,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();
@@ -819,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

@@ -136,11 +136,11 @@ void rlc_um::reset()
   vr_uh    = 0;
   pdu_lost = false;
   if(rx_sdu) {
-    rx_sdu->reset();
+    pool->deallocate(rx_sdu);
   }
 
   if(tx_sdu) {
-    tx_sdu->reset();
+    pool->deallocate(tx_sdu);
   }
 
   if(mac_timers) {
@@ -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/CMakeLists.txt

@@ -31,7 +31,8 @@ target_link_libraries(rlc_am_test srslte_upper srslte_phy srslte_common)
 add_test(rlc_am_test rlc_am_test)
 
 add_executable(rlc_am_stress_test rlc_am_stress_test.cc)
-target_link_libraries(rlc_am_stress_test srslte_upper srslte_phy srslte_common)
+target_link_libraries(rlc_am_stress_test srslte_upper srslte_phy srslte_common ${Boost_LIBRARIES})
+add_test(rlc_am_stress_test rlc_am_stress_test --duration 10)
 
 add_executable(rlc_um_data_test rlc_um_data_test.cc)
 target_link_libraries(rlc_um_data_test srslte_upper srslte_phy srslte_common)

lib/test/upper/rlc_am_stress_test.cc

@@ -31,23 +31,77 @@
 #include "srslte/common/logger_stdout.h"
 #include "srslte/common/threads.h"
 #include "srslte/upper/rlc.h"
+#include <boost/program_options.hpp>
+#include <boost/program_options/parsers.hpp>
 #include <assert.h>
-#define NBUFS 5
 
+using namespace std;
 using namespace srsue;
 using namespace srslte;
+namespace bpo = boost::program_options;
+
+typedef struct {
+  uint32_t test_duration_sec;
+  float    error_rate;
+  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[]) {
+
+  // Command line only options
+  bpo::options_description general("General options");
+
+  general.add_options()
+  ("help,h", "Produce help message")
+  ("version,v", "Print version information and exit");
+
+  // Command line or config file options
+  bpo::options_description common("Configuration options");
+  common.add_options()
+  ("duration",      bpo::value<uint32_t>(&args->test_duration_sec)->default_value(10), "Duration (sec)")
+  ("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")
+  ("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;
+  cmdline_options.add(common).add(general);
+
+  // parse the command line and store result in vm
+  bpo::variables_map vm;
+  bpo::store(bpo::command_line_parser(argc, argv).options(cmdline_options).run(), vm);
+  bpo::notify(vm);
+
+  // help option was given - print usage and exit
+  if (vm.count("help")) {
+    cout << "Usage: " << argv[0] << " [OPTIONS] config_file" << endl << endl;
+    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
     :public thread
 {
 public:
-  mac_reader(rlc_interface_mac *rlc1_, rlc_interface_mac *rlc2_, float fail_rate_)
+  mac_reader(rlc_interface_mac *rlc1_, rlc_interface_mac *rlc2_, float fail_rate_, uint32_t pdu_tx_delay_usec_)
   {
     rlc1 = rlc1_;
     rlc2 = rlc2_;
     fail_rate = fail_rate_;
     run_enable = true;
     running = false;
+    pdu_tx_delay_usec = pdu_tx_delay_usec_;
   }
 
   void stop()
@@ -82,7 +136,7 @@ private:
       if(((float)rand()/RAND_MAX > fail_rate) && read>0) {
         rlc2->write_pdu(1, pdu->msg, opp_size);
       }
-      usleep(100);
+      usleep(pdu_tx_delay_usec);
     }
     running = false;
     byte_buffer_pool::get_instance()->deallocate(pdu);
@@ -91,6 +145,7 @@ private:
   rlc_interface_mac *rlc1;
   rlc_interface_mac *rlc2;
   float fail_rate;
+  uint32_t pdu_tx_delay_usec;
 
   bool run_enable;
   bool running;
@@ -100,9 +155,9 @@ class mac_dummy
     :public srslte::mac_interface_timers
 {
 public:
-  mac_dummy(rlc_interface_mac *rlc1_, rlc_interface_mac *rlc2_, float fail_rate_)
+  mac_dummy(rlc_interface_mac *rlc1_, rlc_interface_mac *rlc2_, float fail_rate_, uint32_t pdu_tx_delay)
-    :r1(rlc1_, rlc2_, fail_rate_)
+    :r1(rlc1_, rlc2_, fail_rate_, pdu_tx_delay)
-    ,r2(rlc2_, rlc1_, fail_rate_)
+    ,r2(rlc2_, rlc1_, fail_rate_, pdu_tx_delay)
   {
   }
 
@@ -140,12 +195,13 @@ class rlc_am_tester
     ,public thread
 {
 public:
-  rlc_am_tester(rlc_interface_pdcp *rlc_, std::string name_=""){
+  rlc_am_tester(rlc_interface_pdcp *rlc_, std::string name_, uint32_t sdu_gen_delay_usec_){
     rlc = rlc_;
     run_enable = true;
     running = false;
     rx_pdus = 0;
     name = name_;
+    sdu_gen_delay_usec = sdu_gen_delay_usec_;
   }
 
   void stop()
@@ -191,7 +247,7 @@ private:
       pdu->N_bytes = 1500;
       pdu->msg[0]   = sn++;
       rlc->write_sdu(1, pdu);
-      usleep(100);
+      usleep(sdu_gen_delay_usec);
     }
     running = false;
   }
@@ -202,26 +258,26 @@ private:
 
   std::string name;
 
+  uint32_t sdu_gen_delay_usec;
+
   rlc_interface_pdcp *rlc;
 };
 
-void stress_test()
+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);
 
-  float fail_rate = 0.1;
-
   rlc rlc1;
   rlc rlc2;
 
-  rlc_am_tester tester1(&rlc1, "tester1");
+  rlc_am_tester tester1(&rlc1, "tester1", args.sdu_gen_delay_usec);
-  rlc_am_tester tester2(&rlc2, "tester2");
+  rlc_am_tester tester2(&rlc2, "tester2", args.sdu_gen_delay_usec);
-  mac_dummy     mac(&rlc1, &rlc2, fail_rate);
+  mac_dummy     mac(&rlc1, &rlc2, args.error_rate, args.pdu_tx_delay_usec);
   ue_interface  ue;
 
   rlc1.init(&tester1, &tester1, &ue, &log1, &mac, 0);
@@ -245,7 +301,14 @@ void stress_test()
   tester2.start(7);
   mac.start();
 
-  usleep(100e6);
+  for (uint32_t i = 0; i < args.test_duration_sec; i++) {
+    // if enabled, mimic reestablishment every second
+    if (args.reestablish) {
+      rlc1.reestablish();
+      rlc2.reestablish();
+    }
+    usleep(1e6);
+  }
 
   tester1.stop();
   tester2.stop();
@@ -254,6 +317,9 @@ void stress_test()
 
 
 int main(int argc, char **argv) {
-  stress_test();
+  stress_test_args_t args;
+  parse_args(&args, argc, argv);
+
+  stress_test(args);
   byte_buffer_pool::get_instance()->cleanup();
 }

srsenb/enb.conf.example

@@ -13,6 +13,8 @@
 # mme_addr:       IP address of MME for S1 connnection
 # gtp_bind_addr:  Local IP address to bind for GTP connection
 # n_prb:          Number of Physical Resource Blocks (6,15,25,50,75,100)
+# tm:             Transmission mode 1-4 (TM1 default)
+# nof_ports:      Number of Tx ports (1 port default, set to 2 for TM2/3/4)
 #
 #####################################################################
 [enb]
@@ -25,6 +27,9 @@ mnc = 01
 mme_addr = 127.0.1.100
 gtp_bind_addr = 127.0.0.1
 n_prb = 50
+#tm = 4
+#nof_ports = 2
+
 
 #####################################################################
 # eNB configuration files 

srsenb/src/phy/txrx.cc

@@ -104,8 +104,6 @@ void txrx::run_thread()
   
   log_h->info("Starting RX/TX thread nof_prb=%d, sf_len=%d\n",worker_com->cell.nof_prb, sf_len);
 
-  // Start streaming RX samples
-  radio_h->start_rx();
 
   // Set TTI so that first TX is at tti=0
   tti = 10235; 

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;

srsepc/epc.conf.example

@@ -11,6 +11,7 @@
 # mcc:              Mobile Country Code
 # mnc:              Mobile Network Code
 # mme_bindx_addr:   IP subnet to listen for eNB S1 connnections
+# apn:		        Set Access Point Name (APN)
 #
 #####################################################################
 [mme]
@@ -20,6 +21,7 @@ tac = 0x0007
 mcc = 001
 mnc = 01
 mme_bind_addr = 127.0.1.100
+apn = test123
 
 #####################################################################
 # HSS configuration

srsepc/hdr/mme/s1ap_common.h

@@ -49,6 +49,7 @@ typedef struct{
   uint16_t      mnc;        // BCD-coded with 0xF filler
   std::string   mme_bind_addr;
   std::string   mme_name;
+  std::string   mme_apn;
 } s1ap_args_t;
 
 typedef struct{
@@ -96,6 +97,7 @@ typedef struct{
   LIBLTE_MME_MS_NETWORK_CAPABILITY_STRUCT ms_network_cap;
   bool eit;
   uint8_t procedure_transaction_id;
+  uint8_t attach_type;
 } ue_ctx_t;
 }//namespace
 #endif

srsepc/hdr/mme/s1ap_nas_transport.h

@@ -84,6 +84,7 @@ public:
   bool pack_identity_request(srslte::byte_buffer_t *reply_msg, uint32_t enb_ue_s1ap_id, uint32_t mme_ue_s1ap_id);
 
   bool pack_emm_information(srslte::byte_buffer_t *reply_msg, uint32_t enb_ue_s1ap_id, uint32_t mme_ue_s1ap_id);
+  bool pack_service_reject(srslte::byte_buffer_t *reply_msg, uint8_t emm_cause, uint32_t enb_ue_s1ap_id);
 
   void log_unhandled_attach_request_ies(const LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT *attach_req);
   void log_unhandled_pdn_con_request_ies(const LIBLTE_MME_PDN_CONNECTIVITY_REQUEST_MSG_STRUCT *pdn_con_req);

srsepc/src/main.cc

@@ -81,6 +81,7 @@ parse_args(all_args_t *args, int argc, char* argv[]) {
   string mcc;
   string mnc;
   string mme_bind_addr;
+  string mme_apn;
   string spgw_bind_addr;
   string sgi_if_addr;
   string hss_db_file;
@@ -105,6 +106,7 @@ parse_args(all_args_t *args, int argc, char* argv[]) {
     ("mme.mcc",             bpo::value<string>(&mcc)->default_value("001"),                          "Mobile Country Code")
     ("mme.mnc",             bpo::value<string>(&mnc)->default_value("01"),                           "Mobile Network Code")
     ("mme.mme_bind_addr",   bpo::value<string>(&mme_bind_addr)->default_value("127.0.0.1"),"IP address of MME for S1 connnection")
+    ("mme.apn",             bpo::value<string>(&mme_apn)->default_value(""),                   "Set Access Point Name (APN) for data services")
     ("hss.db_file",         bpo::value<string>(&hss_db_file)->default_value("ue_db.csv"),".csv file that stores UE's keys")
     ("hss.auth_algo",       bpo::value<string>(&hss_auth_algo)->default_value("milenage"),"HSS uthentication algorithm.")
     ("spgw.gtpu_bind_addr", bpo::value<string>(&spgw_bind_addr)->default_value("127.0.0.1"),"IP address of SP-GW for the S1-U connection")
@@ -204,6 +206,7 @@ parse_args(all_args_t *args, int argc, char* argv[]) {
   }
 
   args->mme_args.s1ap_args.mme_bind_addr = mme_bind_addr;
+  args->mme_args.s1ap_args.mme_apn = mme_apn;
   args->spgw_args.gtpu_bind_addr = spgw_bind_addr;
   args->spgw_args.sgi_if_addr = sgi_if_addr;
   args->hss_args.db_file = hss_db_file;

srsepc/src/mme/mme_gtpc.cc

@@ -118,7 +118,8 @@ mme_gtpc::send_create_session_request(uint64_t imsi, uint32_t mme_ue_s1ap_id)
   m_mme_gtpc_log->console("Creating Session Response -- IMSI: %015lu \n", imsi);
   m_mme_gtpc_log->console("Creating Session Response -- MME control TEID: %lu \n", cs_req->sender_f_teid.teid);
   // APN
-  memcpy(cs_req->apn, "internet", sizeof("internet"));
+  strncpy(cs_req->apn, m_s1ap->m_s1ap_args.mme_apn.c_str(), sizeof(cs_req->apn)-1);
+  cs_req->apn[sizeof(cs_req->apn)-1] = 0;
   // RAT Type
   //cs_req->rat_type = srslte::GTPC_RAT_TYPE::EUTRAN;
 

srsepc/src/mme/s1ap.cc

@@ -38,7 +38,9 @@ boost::mutex   s1ap_instance_mutex;
 
 s1ap::s1ap():
   m_s1mme(-1),
-  m_next_mme_ue_s1ap_id(1)
+  m_next_mme_ue_s1ap_id(1),
+  m_mme_gtpc(NULL),
+  m_pool(NULL)
 {
 }
 

srsepc/src/mme/s1ap_nas_transport.cc

@@ -121,7 +121,12 @@ s1ap_nas_transport::handle_initial_ue_message(LIBLTE_S1AP_MESSAGE_INITIALUEMESSA
     m_s1ap_log->info("Received Service Request \n");
     m_s1ap_log->console("Received Service Request \n");
     liblte_mme_unpack_service_request_msg((LIBLTE_BYTE_MSG_STRUCT*) nas_msg, &service_req);
-    return false;
+
+    m_s1ap_log->info("Service Request not implemented. Sending Service Reject.");
+    m_s1ap_log->console("Service Request not implemented. Sending Service Reject.");
+    /* Force UE to re-attach */
+    pack_service_reject(reply_buffer, LIBLTE_MME_EMM_CAUSE_IMPLICITLY_DETACHED, enb_ue_s1ap_id);
+    *reply_flag = true;
   }
   m_pool->deallocate(nas_msg);
 
@@ -200,11 +205,8 @@ s1ap_nas_transport::handle_uplink_nas_transport(LIBLTE_S1AP_MESSAGE_UPLINKNASTRA
 
   if(*reply_flag == true)
   {
-    if(ue_ctx != NULL)
+    m_s1ap_log->info("DL NAS: Sent Downlink NAS message\n");
-    {
+    m_s1ap_log->console("DL NAS: Sent Downlink NAS Message\n");
-      m_s1ap_log->console("DL NAS: Sent Downlink NAs Message. DL NAS Count=%d, UL NAS Count=%d\n",ue_ctx->security_ctxt.dl_nas_count, ue_ctx->security_ctxt.ul_nas_count);
-      m_s1ap_log->info("DL NAS: Sent Downlink NAS message. DL NAS Count=%d, UL NAS Count=%d\n",ue_ctx->security_ctxt.dl_nas_count, ue_ctx->security_ctxt.ul_nas_count);
-    }
   }
   m_pool->deallocate(nas_msg);
 
@@ -291,6 +293,10 @@ s1ap_nas_transport::handle_nas_imsi_attach_request(uint32_t enb_ue_s1ap_id,
 
   m_s1ap_log->console("Attach request -- IMSI: %015lu\n", ue_ctx.imsi);
   m_s1ap_log->info("Attach request -- IMSI: %015lu\n", ue_ctx.imsi);
+
+  m_s1ap_log->console("Attach request -- Attach type: %d\n", attach_req.eps_attach_type);
+  m_s1ap_log->info("Attach request -- Attach type: %d\n", attach_req.eps_attach_type);
+
   m_s1ap_log->console("Attach request -- eNB-UE S1AP Id: %d, MME-UE S1AP Id: %d\n", ue_ctx.enb_ue_s1ap_id, ue_ctx.mme_ue_s1ap_id);
   m_s1ap_log->console("Attach Request -- UE Network Capabilities EEA: %d%d%d%d%d%d%d%d\n",
                       attach_req.ue_network_cap.eea[0],
@@ -315,6 +321,9 @@ s1ap_nas_transport::handle_nas_imsi_attach_request(uint32_t enb_ue_s1ap_id,
   m_s1ap_log->console("PDN Connectivity Request -- Procedure Transaction Id: %d\n", pdn_con_req.proc_transaction_id);
   m_s1ap_log->console("PDN Connectivity Request -- ESM Information Transfer requested: %s\n", pdn_con_req.esm_info_transfer_flag_present ? "true" : "false");
 
+  //Save attach request type
+  ue_ctx.attach_type = attach_req.eps_attach_type;
+
   //Get Authentication Vectors from HSS
   if(!m_hss->gen_auth_info_answer(ue_ctx.imsi, ue_ctx.security_ctxt.k_asme, autn, rand, ue_ctx.security_ctxt.xres))
   {
@@ -329,8 +338,8 @@ s1ap_nas_transport::handle_nas_imsi_attach_request(uint32_t enb_ue_s1ap_id,
   
   //Send reply to eNB
   *reply_flag = true;
-  m_s1ap_log->info("Downlink NAS: Sending Athentication Request\n");
+  m_s1ap_log->info("Downlink NAS: Sending Authentication Request\n");
-  m_s1ap_log->console("Downlink NAS: Sending Athentication Request\n");
+  m_s1ap_log->console("Downlink NAS: Sending Authentication Request\n");
   return true;
 }
 
@@ -363,6 +372,9 @@ s1ap_nas_transport::handle_nas_guti_attach_request(uint32_t enb_ue_s1ap_id,
     uint8_t eps_bearer_id = pdn_con_req.eps_bearer_id;             //TODO: Unused
     ue_ctx.procedure_transaction_id = pdn_con_req.proc_transaction_id;
 
+    //Save attach request type
+    ue_ctx.attach_type = attach_req.eps_attach_type;
+
     //Save whether ESM information transfer is necessary
     ue_ctx.eit = pdn_con_req.esm_info_transfer_flag_present;
     //m_s1ap_log->console("EPS Bearer id: %d\n", eps_bearer_id);
@@ -415,6 +427,8 @@ s1ap_nas_transport::handle_nas_guti_attach_request(uint32_t enb_ue_s1ap_id,
     ue_ctx_t *ue_ctx_ptr = m_s1ap->find_ue_ctx(it->second);
     if(ue_ctx_ptr!=NULL)
     {
+      //Save attach request type
+      ue_ctx_ptr->attach_type = attach_req.eps_attach_type;
       m_s1ap_log->console("Found UE context. IMSI: %015lu\n",ue_ctx_ptr->imsi);
       m_mme_gtpc->send_create_session_request(ue_ctx_ptr->imsi, ue_ctx_ptr->mme_ue_s1ap_id);
       *reply_flag = false; //No reply needed
@@ -470,7 +484,7 @@ s1ap_nas_transport::handle_nas_authentication_response(srslte::byte_buffer_t *na
 
     m_s1ap_log->console("UE Authentication Rejected.\n");
     m_s1ap_log->warning("UE Authentication Rejected.\n");
-    //Send back Athentication Reject
+    //Send back Authentication Reject
     pack_authentication_reject(reply_buffer, ue_ctx->enb_ue_s1ap_id, ue_ctx->mme_ue_s1ap_id);
     *reply_flag = true;
     m_s1ap_log->console("Downlink NAS: Sending Authentication Reject.\n");
@@ -514,6 +528,7 @@ s1ap_nas_transport::handle_nas_security_mode_complete(srslte::byte_buffer_t *nas
   {
     pack_esm_information_request(reply_buffer, ue_ctx);
     m_s1ap_log->console("Sending ESM information request\n");
+    m_s1ap_log->info("Sending ESM information request\n");
     *reply_flag = true;
   }
   else
@@ -535,7 +550,7 @@ s1ap_nas_transport::handle_nas_attach_complete(srslte::byte_buffer_t *nas_msg, u
   srslte::byte_buffer_t *esm_msg = m_pool->allocate();
   LIBLTE_MME_ACTIVATE_DEFAULT_EPS_BEARER_CONTEXT_ACCEPT_MSG_STRUCT act_bearer;
 
-  m_s1ap_log->info_hex(nas_msg->msg, nas_msg->N_bytes, "NAS Attach complte");
+  m_s1ap_log->info_hex(nas_msg->msg, nas_msg->N_bytes, "NAS Attach complete");
 
   //Get NAS authentication response
   LIBLTE_ERROR_ENUM err = liblte_mme_unpack_attach_complete_msg((LIBLTE_BYTE_MSG_STRUCT *) nas_msg, &attach_comp);
@@ -551,7 +566,7 @@ s1ap_nas_transport::handle_nas_attach_complete(srslte::byte_buffer_t *nas_msg, u
   }
 
   m_s1ap_log->console("Unpacked Attached Complete Message\n");
-  m_s1ap_log->console("Unpacked Activavate Default EPS Bearer message. EPS Bearer id %d\n",act_bearer.eps_bearer_id);
+  m_s1ap_log->console("Unpacked Activate Default EPS Bearer message. EPS Bearer id %d\n",act_bearer.eps_bearer_id);
   //ue_ctx->erabs_ctx[act_bearer->eps_bearer_id].enb_fteid;
   if(act_bearer.eps_bearer_id < 5 || act_bearer.eps_bearer_id > 15)
   {
@@ -579,6 +594,11 @@ s1ap_nas_transport::handle_esm_information_response(srslte::byte_buffer_t *nas_m
     m_s1ap_log->info("ESM Info: APN %s\n",esm_info_resp.eps_bearer_id);
     m_s1ap_log->console("ESM Info: APN %s\n",esm_info_resp.eps_bearer_id);
   }
+  if(esm_info_resp.protocol_cnfg_opts_present)
+  {
+    m_s1ap_log->info("ESM Info: %d Protocol Configuration Options %s\n",esm_info_resp.protocol_cnfg_opts.N_opts);
+    m_s1ap_log->console("ESM Info: %d Protocol Configuration Options %s\n",esm_info_resp.protocol_cnfg_opts.N_opts);
+  }
 
   //FIXME The packging of GTP-C messages is not ready.
   //This means that GTP-U tunnels are created with function calls, as opposed to GTP-C.
@@ -797,8 +817,8 @@ s1ap_nas_transport::pack_authentication_request(srslte::byte_buffer_t *reply_msg
   LIBLTE_ERROR_ENUM err = liblte_mme_pack_authentication_request_msg(&auth_req, (LIBLTE_BYTE_MSG_STRUCT *) nas_buffer);
   if(err != LIBLTE_SUCCESS)
   {
-    m_s1ap_log->error("Error packing Athentication Request\n");
+    m_s1ap_log->error("Error packing Authentication Request\n");
-    m_s1ap_log->console("Error packing Athentication Request\n");
+    m_s1ap_log->console("Error packing Authentication Request\n");
     return false;
   }
 
@@ -810,8 +830,8 @@ s1ap_nas_transport::pack_authentication_request(srslte::byte_buffer_t *reply_msg
   err = liblte_s1ap_pack_s1ap_pdu(&tx_pdu, (LIBLTE_BYTE_MSG_STRUCT *) reply_msg);
   if(err != LIBLTE_SUCCESS)
   {
-    m_s1ap_log->error("Error packing Athentication Request\n");
+    m_s1ap_log->error("Error packing Authentication Request\n");
-    m_s1ap_log->console("Error packing Athentication Request\n");
+    m_s1ap_log->console("Error packing Authentication Request\n");
     return false;
   }
    
@@ -848,8 +868,8 @@ s1ap_nas_transport::pack_authentication_reject(srslte::byte_buffer_t *reply_msg,
   LIBLTE_ERROR_ENUM err = liblte_mme_pack_authentication_reject_msg(&auth_rej, (LIBLTE_BYTE_MSG_STRUCT *) nas_buffer);
   if(err != LIBLTE_SUCCESS)
   {
-    m_s1ap_log->error("Error packing Athentication Reject\n");
+    m_s1ap_log->error("Error packing Authentication Reject\n");
-    m_s1ap_log->console("Error packing Athentication Reject\n");
+    m_s1ap_log->console("Error packing Authentication Reject\n");
     return false;
   }
 
@@ -861,8 +881,8 @@ s1ap_nas_transport::pack_authentication_reject(srslte::byte_buffer_t *reply_msg,
   err = liblte_s1ap_pack_s1ap_pdu(&tx_pdu, (LIBLTE_BYTE_MSG_STRUCT *) reply_msg);
   if(err != LIBLTE_SUCCESS)
   {
-    m_s1ap_log->error("Error packing Dw NAS Transport: Athentication Reject\n");
+    m_s1ap_log->error("Error packing Dw NAS Transport: Authentication Reject\n");
-    m_s1ap_log->console("Error packing Downlink NAS Transport: Athentication Reject\n");
+    m_s1ap_log->console("Error packing Downlink NAS Transport: Authentication Reject\n");
     return false;
   } 
 
@@ -951,7 +971,7 @@ s1ap_nas_transport::pack_security_mode_command(srslte::byte_buffer_t *reply_msg,
   LIBLTE_ERROR_ENUM err = liblte_mme_pack_security_mode_command_msg(&sm_cmd,sec_hdr_type, ue_ctx->security_ctxt.dl_nas_count,(LIBLTE_BYTE_MSG_STRUCT *) nas_buffer);
   if(err != LIBLTE_SUCCESS)
   {
-    m_s1ap_log->console("Error packing Athentication Request\n");
+    m_s1ap_log->console("Error packing Authentication Request\n");
     return false;
   }
 
@@ -985,7 +1005,7 @@ s1ap_nas_transport::pack_security_mode_command(srslte::byte_buffer_t *reply_msg,
   err = liblte_s1ap_pack_s1ap_pdu(&tx_pdu, (LIBLTE_BYTE_MSG_STRUCT *) reply_msg);
   if(err != LIBLTE_SUCCESS)
   {
-    m_s1ap_log->console("Error packing Athentication Request\n");
+    m_s1ap_log->console("Error packing Authentication Request\n");
     return false;
   }
   m_s1ap_log->debug_hex(reply_msg->msg, reply_msg->N_bytes, "Security Mode Command: ");
@@ -1051,8 +1071,8 @@ s1ap_nas_transport::pack_esm_information_request(srslte::byte_buffer_t *reply_ms
   err = liblte_s1ap_pack_s1ap_pdu(&tx_pdu, (LIBLTE_BYTE_MSG_STRUCT *) reply_msg);
   if(err != LIBLTE_SUCCESS)
   {
-    m_s1ap_log->error("Error packing Dw NAS Transport: Athentication Reject\n");
+    m_s1ap_log->error("Error packing Dw NAS Transport: Authentication Reject\n");
-    m_s1ap_log->console("Error packing Downlink NAS Transport: Athentication Reject\n");
+    m_s1ap_log->console("Error packing Downlink NAS Transport: Authentication Reject\n");
     return false;
   }
 
@@ -1090,7 +1110,7 @@ s1ap_nas_transport::pack_attach_accept(ue_ctx_t *ue_ctx, LIBLTE_S1AP_E_RABTOBESE
   }
 
   //Attach accept
-  attach_accept.eps_attach_result = LIBLTE_MME_EPS_ATTACH_RESULT_EPS_ONLY;
+  attach_accept.eps_attach_result = ue_ctx->attach_type;
   //Mandatory
   //FIXME: Set t3412 from config
   attach_accept.t3412.unit = LIBLTE_MME_GPRS_TIMER_UNIT_1_MINUTE;   // GPRS 1 minute unit
@@ -1143,8 +1163,7 @@ s1ap_nas_transport::pack_attach_accept(ue_ctx_t *ue_ctx, LIBLTE_S1AP_E_RABTOBESE
   act_def_eps_bearer_context_req.eps_qos.mbr_dl_ext = 250; //FIXME check
   //set apn
   //act_def_eps_bearer_context_req.apn
-  std::string apn("test123");
+  act_def_eps_bearer_context_req.apn.apn = m_s1ap->m_s1ap_args.mme_apn;
-  act_def_eps_bearer_context_req.apn.apn = apn; //FIXME
   act_def_eps_bearer_context_req.proc_transaction_id = ue_ctx->procedure_transaction_id; //FIXME
 
   //Set DNS server
@@ -1164,6 +1183,7 @@ s1ap_nas_transport::pack_attach_accept(ue_ctx_t *ue_ctx, LIBLTE_S1AP_E_RABTOBESE
   act_def_eps_bearer_context_req.packet_flow_id_present = false;
   act_def_eps_bearer_context_req.apn_ambr_present = false;
   act_def_eps_bearer_context_req.esm_cause_present = false;
+  act_def_eps_bearer_context_req.connectivity_type_present = false;
 
   uint8_t sec_hdr_type =2;
   ue_ctx->security_ctxt.dl_nas_count++;
@@ -1233,8 +1253,8 @@ s1ap_nas_transport::pack_identity_request(srslte::byte_buffer_t *reply_msg, uint
   err = liblte_s1ap_pack_s1ap_pdu(&tx_pdu, (LIBLTE_BYTE_MSG_STRUCT *) reply_msg);
   if(err != LIBLTE_SUCCESS)
   {
-    m_s1ap_log->error("Error packing Dw NAS Transport: Athentication Reject\n");
+    m_s1ap_log->error("Error packing Dw NAS Transport: Authentication Reject\n");
-    m_s1ap_log->console("Error packing Downlink NAS Transport: Athentication Reject\n");
+    m_s1ap_log->console("Error packing Downlink NAS Transport: Authentication Reject\n");
     return false;
   } 
 
@@ -1311,6 +1331,59 @@ s1ap_nas_transport::pack_emm_information(srslte::byte_buffer_t *reply_msg, uint3
   return true;
 }
 
+bool
+s1ap_nas_transport::pack_service_reject(srslte::byte_buffer_t *reply_msg, uint8_t emm_cause, uint32_t enb_ue_s1ap_id)
+{
+  srslte::byte_buffer_t *nas_buffer = m_pool->allocate();
+
+  //Setup initiating message
+  LIBLTE_S1AP_S1AP_PDU_STRUCT tx_pdu;
+  bzero(&tx_pdu, sizeof(LIBLTE_S1AP_S1AP_PDU_STRUCT));
+
+  tx_pdu.ext          = false;
+  tx_pdu.choice_type  = LIBLTE_S1AP_S1AP_PDU_CHOICE_INITIATINGMESSAGE;
+
+  LIBLTE_S1AP_INITIATINGMESSAGE_STRUCT *init = &tx_pdu.choice.initiatingMessage;
+  init->procedureCode = LIBLTE_S1AP_PROC_ID_DOWNLINKNASTRANSPORT;
+  init->choice_type   = LIBLTE_S1AP_INITIATINGMESSAGE_CHOICE_DOWNLINKNASTRANSPORT;
+
+  //Setup Dw NAS structure
+  LIBLTE_S1AP_MESSAGE_DOWNLINKNASTRANSPORT_STRUCT *dw_nas = &init->choice.DownlinkNASTransport;
+  dw_nas->ext=false;
+  dw_nas->MME_UE_S1AP_ID.MME_UE_S1AP_ID = m_s1ap->get_next_mme_ue_s1ap_id();
+  dw_nas->eNB_UE_S1AP_ID.ENB_UE_S1AP_ID = enb_ue_s1ap_id;
+  dw_nas->HandoverRestrictionList_present=false;
+  dw_nas->SubscriberProfileIDforRFP_present=false;
+  LIBLTE_MME_SERVICE_REJECT_MSG_STRUCT service_rej;
+  service_rej.t3442_present = true;
+  service_rej.t3442.unit = LIBLTE_MME_GPRS_TIMER_DEACTIVATED;
+  service_rej.t3442.value = 0;
+  service_rej.t3446_present = true;
+  service_rej.t3446 = 0;
+  service_rej.emm_cause = emm_cause;
+
+  LIBLTE_ERROR_ENUM err = liblte_mme_pack_service_reject_msg(&service_rej, LIBLTE_MME_SECURITY_HDR_TYPE_PLAIN_NAS, 0, (LIBLTE_BYTE_MSG_STRUCT *) nas_buffer);
+  if(err != LIBLTE_SUCCESS)
+  {
+    m_s1ap_log->error("Error packing Service Reject\n");
+    m_s1ap_log->console("Error packing Service Reject\n");
+    return false;
+  }
+
+  //Copy NAS PDU to Downlink NAS Trasport message buffer
+  memcpy(dw_nas->NAS_PDU.buffer, nas_buffer->msg, nas_buffer->N_bytes);
+  dw_nas->NAS_PDU.n_octets = nas_buffer->N_bytes;
+
+  //Pack Downlink NAS Transport Message
+  err = liblte_s1ap_pack_s1ap_pdu(&tx_pdu, (LIBLTE_BYTE_MSG_STRUCT *) reply_msg);
+  if(err != LIBLTE_SUCCESS)
+  {
+    m_s1ap_log->error("Error packing Dw NAS Transport: Service Reject\n");
+    m_s1ap_log->console("Error packing Downlink NAS Transport: Service Reject\n");
+    return false;
+  }
+  return true;
+}
 /*Helper functions*/
 void
 s1ap_nas_transport::log_unhandled_attach_request_ies(const LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT *attach_req)

srsue/hdr/mac/dl_harq.h

@@ -190,12 +190,17 @@ private:
     }
 
   private:
+
+    const static int RESET_DUPLICATE_TIMEOUT = 8*6;
+
     class dl_tb_process {
     public:
       dl_tb_process(void) {
         is_initiated = false;
         ack = false;
         bzero(&cur_grant, sizeof(Tgrant));
+        payload_buffer_ptr = NULL; 
+        pthread_mutex_init(&mutex, NULL);
       }
 
       ~dl_tb_process() {
@@ -220,16 +225,26 @@ private:
       }
 
       void reset(void) {
+        pthread_mutex_lock(&mutex);
         is_first_tb = true;
         ack = false;
+        if (payload_buffer_ptr) {
+          if (pid != HARQ_BCCH_PID) {
+            harq_entity->demux_unit->deallocate(payload_buffer_ptr);
+          }
           payload_buffer_ptr = NULL;
+        }
         bzero(&cur_grant, sizeof(Tgrant));
         if (is_initiated) {
           srslte_softbuffer_rx_reset(&softbuffer);
         }
+        pthread_mutex_unlock(&mutex);
       }
 
       void new_grant_dl(Tgrant grant, Taction *action) {
+
+        pthread_mutex_lock(&mutex);
+
         // Compute RV for BCCH when not specified in PDCCH format
         if (pid == HARQ_BCCH_PID && grant.rv[tid] == -1) {
           uint32_t k;
@@ -253,13 +268,17 @@ private:
           n_retx = 0;
         }
 
+        // If data has not yet been successfully decoded
+        if (!ack) {
+
           // Save grant
           grant.last_ndi[tid] = cur_grant.ndi[tid];
           grant.last_tti = cur_grant.tti;
           memcpy(&cur_grant, &grant, sizeof(Tgrant));
 
-        // If data has not yet been successfully decoded
+          if (payload_buffer_ptr) {
-        if (!ack) {
+            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) {
@@ -271,6 +290,7 @@ private:
           if (!action->payload_ptr[tid]) {
             action->decode_enabled[tid] = false;
             Error("Can't get a buffer for TBS=%d\n", cur_grant.n_bytes[tid]);
+            pthread_mutex_unlock(&mutex);
             return;
           }
           action->decode_enabled[tid]= true;
@@ -281,7 +301,14 @@ private:
 
         } else {
           action->default_ack[tid] = true;
-          Warning("DL PID %d: Received duplicate TB. Discarting and retransmitting ACK\n", pid);
+          uint32_t interval = srslte_tti_interval(grant.tti, cur_grant.tti);
+          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()) {
@@ -298,9 +325,12 @@ private:
             Debug("Generating ACK\n");
           }
         }
+
+        pthread_mutex_unlock(&mutex);
       }
 
       void tb_decoded(bool ack_) {
+        pthread_mutex_lock(&mutex);
         ack = ack_;
         if (ack) {
           if (pid == HARQ_BCCH_PID) {
@@ -327,15 +357,19 @@ private:
                                                          harq_entity->nof_pkts++);
             }
           }
-        } else {
+        } else if (pid != HARQ_BCCH_PID) {
           harq_entity->demux_unit->deallocate(payload_buffer_ptr);
         }
 
+        payload_buffer_ptr = NULL;
+
         Info("DL %d (TB %d):  %s tbs=%d, rv=%d, ack=%s, ndi=%d (%d), tti=%d (%d)\n",
              pid, tid, is_new_transmission ? "newTX" : "reTX ",
              cur_grant.n_bytes[tid], cur_grant.rv[tid], ack ? "OK" : "KO",
              cur_grant.ndi[tid], cur_grant.last_ndi[tid], cur_grant.tti, cur_grant.last_tti);
 
+        pthread_mutex_unlock(&mutex);
+
         if (ack && pid == HARQ_BCCH_PID) {
           reset();
         }
@@ -363,6 +397,8 @@ private:
         return is_new_transmission;
       }
 
+      pthread_mutex_t mutex;
+
       bool is_initiated;
       dl_harq_entity *harq_entity;
       srslte::log *log_h;

srsue/hdr/mac/mac.h

@@ -162,14 +162,25 @@ private:
   void            timer_alignment_expire();
   srslte::timers  timers;
 
-
   // pointer to MAC PCAP object
   srslte::mac_pcap* pcap;
   bool is_first_ul_grant;
 
-
   mac_metrics_t metrics;
 
+  /* Class to run Timers in a dedicated thread */
+  class mac_timers : public periodic_thread {
+   public:
+    void init(srslte::timers *timers, srslte::log *log_h);
+   private:
+    void run_period();
+    srslte::timers *timers;
+    bool running;
+    srslte::log *log_h;
+  };
+
+  mac_timers mactimers;
+
   /* Class to process MAC PDUs from DEMUX unit */
   class pdu_process : public thread {
   public: 

srsue/hdr/mac/mux.h

@@ -104,6 +104,7 @@ private:
   /* Msg3 Buffer */
   static const uint32_t MSG3_BUFF_SZ = 1024;
   uint8_t               msg3_buff[MSG3_BUFF_SZ];
+  uint8_t              *msg3_buff_start_pdu;
 
   /* PDU Buffer */
   srslte::sch_pdu    pdu_msg; 

srsue/hdr/mac/ul_harq.h

@@ -246,7 +246,6 @@ private:
             } else {
               Warning("UL RAR grant available but no Msg3 on buffer\n");
             }
-            printf("Transmitted Msg3\n");
 
             // Normal UL grant
           } else {

srsue/hdr/phy/phch_recv.h

@@ -61,9 +61,8 @@ public:
   
   void    reset_sync();
   void    cell_search_start();
-  void    cell_search_stop();
   void    cell_search_next(bool reset = false);
-  bool    cell_select(uint32_t earfcn, srslte_cell_t cell);
+  void    cell_select(uint32_t earfcn, srslte_cell_t cell);
   bool    cell_handover(srslte_cell_t cell);
 
   void    meas_reset();
@@ -95,7 +94,6 @@ private:
 
   void   reset();
   void   radio_error();
-  bool   wait_radio_reset();
   void   set_ue_sync_opts(srslte_ue_sync_t *q, float cfo);
   void   run_thread();
 
@@ -104,14 +102,13 @@ private:
   bool   set_cell();
 
   void   cell_search_inc();
-  void   resync_sfn(bool is_connected = false, bool rx_now = false);
+  void   cell_reselect();
-  bool   stop_sync();
 
-  void   stop_rx();
+  float  get_cfo();
-  void   start_rx(bool now = false);
+
-  bool   radio_is_rx;
+  uint32_t new_earfcn;
+  srslte_cell_t new_cell;
 
-  bool   radio_is_resetting;
   bool   running;
 
   // Class to run cell search
@@ -155,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
@@ -167,13 +164,14 @@ private:
     typedef enum {IDLE, MEASURE_OK, ERROR} ret_code;
 
     ~measure();
-    void      init(cf_t *buffer[SRSLTE_MAX_PORTS], srslte::log *log_h, srslte::radio *radio_h,
+    void      init(cf_t *buffer[SRSLTE_MAX_PORTS], srslte::log *log_h,
                    uint32_t nof_rx_antennas, uint32_t nof_subframes = RSRP_MEASURE_NOF_FRAMES);
     void      reset();
     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();
     float     snr();
@@ -183,7 +181,6 @@ private:
     srslte::log      *log_h;
     srslte_ue_dl_t    ue_dl;
     cf_t              *buffer[SRSLTE_MAX_PORTS];
-    srslte::radio    *radio_h;
     uint32_t cnt;
     uint32_t nof_subframes;
     uint32_t current_prb;
@@ -204,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;
@@ -225,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);
@@ -303,17 +301,18 @@ private:
   const static uint32_t NOF_IN_SYNC_SF     = 100;
 
   // State for primary cell
-  enum {
+  typedef enum {
     IDLE = 0,
     CELL_SEARCH,
     CELL_SELECT,
     CELL_RESELECT,
     CELL_MEASURE,
     CELL_CAMP,
-    IDLE_RX
+  } phy_state_t;
-  } phy_state;
+
+  phy_state_t phy_state, prev_state;
 
-  bool is_in_idle, is_in_idle_rx;
+  bool is_in_idle;
 
   // Sampling rate mode (find is 1.96 MHz, camp is the full cell BW)
   enum {
@@ -335,7 +334,6 @@ private:
   float         ul_dl_factor;
   uint32_t      current_earfcn;
   int           cur_earfcn_index;
-  bool          cell_search_in_progress;
 
   float         dl_freq;
   float         ul_freq;

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);
@@ -74,7 +73,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 */
@@ -120,6 +118,7 @@ private:
   srslte::trace<uint32_t> tr_exec;
   bool trace_enabled; 
 
+  pthread_mutex_t mutex;
   
   /* Common objects */  
   phch_common    *phy;

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();
@@ -85,9 +85,8 @@ public:
   void    sync_reset();
   void    configure_ul_params(bool pregen_disabled = false);
   void    cell_search_start();
-  void    cell_search_stop();
   void    cell_search_next();
-  bool    cell_select(uint32_t earfcn, srslte_cell_t phy_cell);
+  void    cell_select(uint32_t earfcn, srslte_cell_t phy_cell);
   bool    cell_handover(srslte_cell_t cell);
 
   void    meas_reset();
@@ -159,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

@@ -72,6 +72,8 @@ public:
   bool is_attached();
   void start_plot();
 
+  void print_pool();
+
   static void rf_msg(srslte_rf_error_t error);
 
   // UE metrics interface
@@ -101,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;
@@ -110,8 +112,6 @@ private:
   srslte::log_filter  gw_log;
   srslte::log_filter  usim_log;
 
-  srslte::byte_buffer_pool *pool;
-
   all_args_t       *args;
   bool              started;
 

srsue/hdr/ue_base.h

@@ -126,6 +126,7 @@ typedef struct {
   usim_args_t   usim;
   rrc_args_t    rrc;
   std::string   ue_category_str;
+  std::string   apn;
   expert_args_t expert;
 }all_args_t;
 
@@ -146,7 +147,7 @@ class ue_base
 {
 public:
   ue_base();
-  virtual ~ue_base() {}
+  virtual ~ue_base();
 
   static ue_base* get_instance(srsue_instance_type_t type);
 
@@ -157,6 +158,8 @@ public:
   virtual bool is_attached() = 0;
   virtual void start_plot() = 0;
 
+  virtual void print_pool() = 0;
+
   virtual void radio_overflow() = 0;
   
   void handle_rf_msg(srslte_rf_error_t error);
@@ -173,6 +176,9 @@ public:
   std::string get_build_mode();
   std::string get_build_info();
   std::string get_build_string();
+
+private:
+  srslte::byte_buffer_pool *pool;
 };
 
 } // namespace srsue

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

@@ -37,6 +37,7 @@
 #include "srslte/common/security.h"
 #include "srslte/common/threads.h"
 
+#include <math.h>
 #include <map>
 #include <queue>
 
@@ -65,14 +66,16 @@ class cell_t
     return earfcn == this->earfcn && pci == phy_cell.id;
   }
   bool greater(cell_t *x) {
-    return x->rsrp > rsrp;
+    return rsrp > x->rsrp;
   }
   bool plmn_equals(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {
+    if (has_valid_sib1) {
       for (uint32_t i = 0; i < sib1.N_plmn_ids; i++) {
         if (plmn_id.mcc == sib1.plmn_id[i].id.mcc && plmn_id.mnc == sib1.plmn_id[i].id.mnc) {
           return true;
         }
       }
+    }
     return false;
   }
   cell_t() {
@@ -80,6 +83,7 @@ class cell_t
     cell_t(tmp, 0, 0);
   }
   cell_t(srslte_cell_t phy_cell, uint32_t earfcn, float rsrp) {
+    gettimeofday(&last_update, NULL);
     this->has_valid_sib1 = false;
     this->has_valid_sib2 = false;
     this->has_valid_sib3 = false;
@@ -94,14 +98,108 @@ class cell_t
     bzero(&sib13, sizeof(sib13));
   }
 
-  uint32_t earfcn;
+  uint32_t get_earfcn() {
+    return earfcn;
+  }
+
+  uint32_t get_pci() {
+    return phy_cell.id;
+  }
+
+  void set_rsrp(float rsrp) {
+    if (~isnan(rsrp)) {
+      this->rsrp = rsrp;
+    }
+    in_sync = true;
+    gettimeofday(&last_update, NULL);
+  }
+
+  float get_rsrp() {
+    return rsrp;
+  }
+
+  void set_sib1(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT *sib1) {
+    memcpy(&this->sib1, sib1, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT));
+    has_valid_sib1 = true;
+  }
+  void set_sib2(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2) {
+    memcpy(&this->sib2, sib2, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT));
+    has_valid_sib2 = true;
+  }
+  void set_sib3(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_3_STRUCT *sib3) {
+    memcpy(&this->sib3, sib3, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_3_STRUCT));
+    has_valid_sib3 = true;
+  }
+  void set_sib13(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13_STRUCT *sib13) {
+    memcpy(&this->sib13, sib13, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13_STRUCT));
+    has_valid_sib13 = true;
+  }
+
+  uint32_t timeout_secs(struct timeval now) {
+    struct timeval t[3];
+    memcpy(&t[2], &now, sizeof(struct timeval));
+    memcpy(&t[1], &last_update, sizeof(struct timeval));
+    get_time_interval(t);
+    return t[0].tv_sec;
+  }
+
+  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT *sib1ptr() {
+    return &sib1;
+  }
+  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2ptr() {
+    return &sib2;
+  }
+  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_3_STRUCT *sib3ptr() {
+    return &sib3;
+  }
+
+  uint32_t get_cell_id() {
+    return sib1.cell_id;
+  }
+
+  bool has_sib1() {
+    return has_valid_sib1;
+  }
+  bool has_sib2() {
+    return has_valid_sib2;
+  }
+  bool has_sib3() {
+    return has_valid_sib3;
+  }
+  bool has_sib13() {
+    return has_valid_sib13;
+  }
+
+  uint16_t get_mcc() {
+    if (has_valid_sib1) {
+      if (sib1.N_plmn_ids > 0) {
+        return sib1.plmn_id[0].id.mcc;
+      }
+    }
+    return 0;
+  }
+
+  uint16_t get_mnc() {
+    if (has_valid_sib1) {
+      if (sib1.N_plmn_ids > 0) {
+        return sib1.plmn_id[0].id.mnc;
+      }
+    }
+    return 0;
+  }
+
   srslte_cell_t phy_cell;
+  bool     in_sync;
+
+ private:
   float    rsrp;
+  uint32_t earfcn;
+  struct timeval last_update;
+
   bool     has_valid_sib1;
   bool     has_valid_sib2;
   bool     has_valid_sib3;
   bool     has_valid_sib13;
-  bool     in_sync;
   LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT  sib1;
   LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT  sib2;
   LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_3_STRUCT  sib3;
@@ -151,13 +249,13 @@ 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();
   void out_of_sync();
   void earfcn_end();
-  void cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp);
+  void cell_camping(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp);
   void new_phy_meas(float rsrp, float rsrq, uint32_t tti, int earfcn, int pci);
 
   // MAC interface
@@ -197,9 +295,9 @@ private:
   LIBLTE_RRC_DL_CCCH_MSG_STRUCT dl_ccch_msg;
   LIBLTE_RRC_DL_DCCH_MSG_STRUCT dl_dcch_msg;
 
-  byte_buffer_t* byte_align_and_pack(byte_buffer_t *pdu = NULL);
+  byte_buffer_t* byte_align_and_pack();
-  void send_ul_ccch_msg(byte_buffer_t *pdu = NULL);
+  void send_ul_ccch_msg();
-  void send_ul_dcch_msg(byte_buffer_t *pdu = NULL);
+  void send_ul_dcch_msg();
   srslte::bit_buffer_t          bit_buf;
 
   pthread_mutex_t mutex;
@@ -213,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;
@@ -226,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 = 2000;
-
   uint8_t k_rrc_enc[32];
   uint8_t k_rrc_int[32];
   uint8_t k_up_enc[32];
@@ -244,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{
@@ -274,6 +371,7 @@ private:
   }
 
   // List of strongest neighbour cell
+  const static int NEIGHBOUR_TIMEOUT   = 5;
   const static int NOF_NEIGHBOUR_CELLS = 8;
   std::vector<cell_t*> neighbour_cells;
   cell_t *serving_cell;
@@ -285,6 +383,9 @@ private:
   bool add_neighbour_cell(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp);
   bool add_neighbour_cell(cell_t *cell);
   void sort_neighbour_cells();
+  void clean_neighbours();
+  std::vector<cell_t*>::iterator delete_neighbour(std::vector<cell_t*>::iterator it);
+  void delete_neighbour(uint32_t cell_idx);
 
   typedef enum {
     SI_ACQUIRE_IDLE = 0,
@@ -299,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;
@@ -315,6 +416,7 @@ private:
     void run_tti(uint32_t tti);
     bool timer_expired(uint32_t timer_id);
     void ho_finish();
+    void delete_report(uint32_t earfcn, uint32_t pci);
   private:
 
     const static int NOF_MEASUREMENTS = 3;
@@ -429,10 +531,10 @@ private:
   void          send_con_restablish_request(LIBLTE_RRC_CON_REEST_REQ_CAUSE_ENUM cause, uint16_t crnti);
   void          send_con_restablish_complete();
   void          send_con_setup_complete(byte_buffer_t *nas_msg);
-  void          send_ul_info_transfer(uint32_t lcid, byte_buffer_t *sdu);
+  void          send_ul_info_transfer(byte_buffer_t *nas_msg);
-  void          send_security_mode_complete(uint32_t lcid, byte_buffer_t *pdu);
+  void          send_security_mode_complete();
-  void          send_rrc_con_reconfig_complete(byte_buffer_t *pdu);
+  void          send_rrc_con_reconfig_complete();
-  void          send_rrc_ue_cap_info(byte_buffer_t *pdu);
+  void          send_rrc_ue_cap_info();
 
   // Parsers
   void          parse_dl_ccch(byte_buffer_t *pdu);
@@ -442,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();
@@ -459,7 +562,7 @@ private:
   void          apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2);
   void          handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup);
   void          handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup);
-  void          handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig, byte_buffer_t *pdu);
+  void          handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig);
   void          add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg);
   void          add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg);
   void          release_drb(uint8_t lcid);

srsue/hdr/upper/rrc_common.h

@@ -33,6 +33,7 @@ namespace srsue {
 // RRC states (3GPP 36.331 v10.0.0)
 typedef enum {
   RRC_STATE_IDLE = 0,
+  RRC_STATE_PLMN_START,
   RRC_STATE_PLMN_SELECTION,
   RRC_STATE_CELL_SELECTING,
   RRC_STATE_CELL_SELECTED,
@@ -44,11 +45,14 @@ typedef enum {
   RRC_STATE_N_ITEMS,
 } rrc_state_t;
 static const char rrc_state_text[RRC_STATE_N_ITEMS][100] = {"IDLE",
+                                                            "PLMN SELECTED",
                                                             "PLMN SELECTION",
                                                             "CELL SELECTING",
                                                             "CELL SELECTED",
                                                             "CONNECTING",
                                                             "CONNECTED",
+                                                            "HO PREPARE",
+                                                            "HO PROCESS",
                                                             "LEAVE CONNECTED"};
 
 } // namespace srsue

srsue/src/mac/demux.cc

@@ -165,7 +165,14 @@ void demux::process_sch_pdu(srslte::sch_pdu *pdu_msg)
       // Route logical channel 
       if (route_pdu) {
         Info("Delivering PDU for lcid=%d, %d bytes\n", pdu_msg->get()->get_sdu_lcid(), pdu_msg->get()->get_payload_size());
+        if (pdu_msg->get()->get_payload_size() < MAX_PDU_LEN) {
           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, 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);
+        }
       }
     } else {
       // Process MAC Control Element

srsue/src/mac/mac.cc

@@ -82,6 +82,7 @@ bool mac::init(phy_interface_mac *phy, rlc_interface_mac *rlc, rrc_interface_mac
   started = true;
   start(MAC_MAIN_THREAD_PRIO);
 
+  mactimers.init(&timers, log_h);
 
   return started;
 }
@@ -94,6 +95,7 @@ void mac::stop()
   ttisync.increase();
   pdu_process_thread.stop();
   wait_thread_finish();
+  mactimers.stop();
 }
 
 void mac::start_pcap(srslte::mac_pcap* pcap_)
@@ -148,6 +150,17 @@ void mac::reset()
   bzero(&uernti, sizeof(ue_rnti_t));
 }
 
+void mac::mac_timers::init(srslte::timers *timers, srslte::log *log_h) {
+  this->timers = timers;
+  running = true;
+  this->log_h = log_h;
+  start_periodic(1000);
+}
+
+void mac::mac_timers::run_period() {
+  timers->step_all();
+}
+
 void mac::run_thread() {
   int cnt=0;
 
@@ -165,7 +178,6 @@ void mac::run_thread() {
     tti = ttisync.wait();
 
     log_h->step(tti);
-    timers.step_all();
 
     // Step all procedures
     bsr_procedure.step(tti);

srsue/src/mac/mux.cc

@@ -47,6 +47,7 @@ mux::mux(uint8_t nof_harq_proc_) : pdu_msg(MAX_NOF_SUBHEADERS), pid_has_bsr(nof_
   rlc   = NULL; 
   bsr_procedure = NULL; 
   phr_procedure = NULL;
+  msg3_buff_start_pdu = NULL;
 
   msg3_flush();
 }
@@ -166,7 +167,6 @@ uint8_t* mux::pdu_get(uint8_t *payload, uint32_t pdu_sz, uint32_t tx_tti, uint32
   }
   
   // Logical Channel Procedure
-
   bool is_rar = false;
 
   pdu_msg.init_tx(payload, pdu_sz, true);
@@ -324,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;               
@@ -347,6 +346,7 @@ void mux::msg3_flush()
   msg3_has_been_transmitted = false;
   msg3_pending = false;
   bzero(msg3_buff, sizeof(MSG3_BUFF_SZ));
+  msg3_buff_start_pdu = NULL;
 }
 
 bool mux::msg3_is_transmitted()
@@ -366,19 +366,22 @@ bool mux::msg3_is_pending() {
 uint8_t* mux::msg3_get(uint8_t *payload, uint32_t pdu_sz)
 {
   if (pdu_sz < MSG3_BUFF_SZ - 32) {
-    uint8_t* msg3_buff_start_pdu = pdu_get(msg3_buff, pdu_sz, 0, 0);
+    if (!msg3_buff_start_pdu) {
+      msg3_buff_start_pdu = pdu_get(msg3_buff, pdu_sz, 0, 0);
       if (!msg3_buff_start_pdu) {
         Error("Moving PDU from Mux unit to Msg3 buffer\n");
         return NULL;
       }
-    memcpy(payload, msg3_buff_start_pdu, sizeof(uint8_t)*pdu_sz);
       msg3_has_been_transmitted = true;
       msg3_pending = false;
-    return payload;
+    }
   } else {
     Error("Msg3 size (%d) is longer than internal msg3_buff size=%d, (see mux.h)\n", pdu_sz, MSG3_BUFF_SZ-32);
     return NULL;
   }
+  memcpy(payload, msg3_buff_start_pdu, sizeof(uint8_t)*pdu_sz);
+  msg3_has_been_transmitted = true;
+  return payload;
 }
 
   

srsue/src/main.cc

@@ -82,6 +82,7 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
                                                                                            "UECapabilityInformation message. Default 0xe6041c00")
     ("rrc.ue_category",   bpo::value<string>(&args->ue_category_str)->default_value("4"),  "UE Category (1 to 5)")
 
+    ("nas.apn",   bpo::value<string>(&args->apn)->default_value(""),  "Set Access Point Name (APN) for data services")
 
     ("pcap.enable", bpo::value<bool>(&args->pcap.enable)->default_value(false), "Enable MAC packet captures for wireshark")
     ("pcap.filename", bpo::value<string>(&args->pcap.filename)->default_value("ue.pcap"), "MAC layer capture filename")
@@ -159,11 +160,11 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
      "Pregenerate uplink signals after attach. Improves CPU performance.")
 
     ("expert.rssi_sensor_enabled",
-     bpo::value<bool>(&args->expert.phy.rssi_sensor_enabled)->default_value(true),
+     bpo::value<bool>(&args->expert.phy.rssi_sensor_enabled)->default_value(false),
      "Enable or disable RF frontend RSSI sensor. In some USRP devices can cause segmentation fault")
 
     ("expert.rx_gain_offset",
-     bpo::value<float>(&args->expert.phy.rx_gain_offset)->default_value(10),
+     bpo::value<float>(&args->expert.phy.rx_gain_offset)->default_value(62),
      "RX Gain offset to add to rx_gain to correct RSRP value")
 
       ("expert.prach_gain",
@@ -202,6 +203,11 @@ 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.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.")
@@ -214,12 +220,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)")
@@ -257,10 +257,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.")
@@ -269,6 +265,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")
@@ -418,6 +417,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;
       }
     }
   }
@@ -474,7 +476,8 @@ int main(int argc, char *argv[])
         plot_started = true;
       }
     }
-    sleep(1);
+    ue->print_pool();
+    sleep(10);
   }
   pthread_cancel(input);
   metricshub.stop();

srsue/src/phy/phch_common.cc

@@ -355,13 +355,16 @@ void phch_common::reset() {
 
 void phch_common::reset_ul()
 {
+  /*
   is_first_tx = true;
   is_first_of_burst = true;
+
   for (uint32_t i=0;i<nof_mutex;i++) {
     pthread_mutex_trylock(&tx_mutex[i]);
     pthread_mutex_unlock(&tx_mutex[i]);
   }
   radio_h->tx_end();
+   */
 }
 
 }

srsue/src/phy/phch_recv.cc

@@ -91,7 +91,7 @@ void phch_recv::init(srslte::radio_multi *_radio_handler, mac_interface_phy *_ma
   sfn_p.init(&ue_sync, sf_buffer, log_h);
 
   // Initialize measurement class for the primary cell 
-  measure_p.init(sf_buffer, log_h, radio_h, nof_rx_antennas);
+  measure_p.init(sf_buffer, log_h, nof_rx_antennas);
 
   // Start intra-frequency measurement
   intra_freq_meas.init(worker_com, rrc, log_h);
@@ -132,9 +132,7 @@ void phch_recv::reset()
   next_offset  = 0;
   cell_is_set = false;
   srate_mode = SRATE_NONE;
-  cell_search_in_progress = false;
   current_earfcn = 0;
-  radio_is_resetting = false;
   sfn_p.reset();
   measure_p.reset();
   search_p.reset();
@@ -144,33 +142,16 @@ void phch_recv::reset()
 void phch_recv::radio_error()
 {
   log_h->error("SYNC:  Receiving from radio.\n");
-  phy_state = IDLE;
-  radio_is_resetting=true;
-
-  // Need to find a method to effectively reset radio, reloading the driver does not work
-  //radio_h->reset();
-  radio_h->stop();
-
-  fprintf(stdout, "Error while receiving samples. Restart srsUE\n");
-  exit(-1);
-
   reset();
-  radio_is_resetting=false;
+  phy_state = CELL_SEARCH;
+  // Need to find a method to effectively reset radio, reloading the driver does not work
+  radio_h->reset();
 }
 
 void phch_recv::set_cfo(float cfo) {
   srslte_ue_sync_set_cfo_ref(&ue_sync, cfo);
 }
 
-bool phch_recv::wait_radio_reset() {
-  int cnt=0;
-  while(cnt < 20 && radio_is_resetting) {
-    sleep(1);
-    cnt++;
-  }
-  return radio_is_resetting;
-}
-
 void phch_recv::set_agc_enable(bool enable)
 {
   do_agc = enable;
@@ -239,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);
@@ -265,19 +246,6 @@ bool phch_recv::set_cell() {
   return cell_is_set;
 }
 
-void phch_recv::resync_sfn(bool is_connected, bool now) {
-
-  if (!now) {
-    wait_radio_reset();
-    stop_rx();
-  }
-  start_rx(now);
-  sfn_p.reset();
-  Info("SYNC:  Starting SFN synchronization\n");
-
-  phy_state = is_connected?CELL_RESELECT:CELL_SELECT;
-}
-
 void phch_recv::set_earfcn(std::vector<uint32_t> earfcn) {
   this->earfcn = earfcn;
 }
@@ -287,34 +255,14 @@ void phch_recv::force_freq(float dl_freq, float ul_freq) {
   this->ul_freq = ul_freq;
 }
 
-bool phch_recv::stop_sync() {
-
-  wait_radio_reset();
-
-  if (phy_state == IDLE && is_in_idle) {
-    return true;
-  } else {
-    Info("SYNC:  Going to IDLE\n");
-    phy_state = IDLE;
-    int cnt = 0;
-    while (!is_in_idle && cnt < 100) {
-      usleep(10000);
-      cnt++;
-    }
-    if (!is_in_idle) {
-      Warning("SYNC:  Could not go to IDLE\n");
-    }
-    return is_in_idle;
-  }
-}
-
 void phch_recv::reset_sync() {
 
-  Warning("SYNC:  Resetting sync, cell_search_in_progress=%s\n", cell_search_in_progress?"yes":"no");
+  Info("SYNC:  Reset. Going to Cell Select\n");
-
+  sfn_p.reset();
   search_p.reset();
+  measure_p.reset();
   srslte_ue_sync_reset(&ue_sync);
-  resync_sfn(true, true);
+  phy_state = CELL_SELECT;
 }
 
 void phch_recv::cell_search_inc()
@@ -322,7 +270,9 @@ void phch_recv::cell_search_inc()
   cur_earfcn_index++;
   if (cur_earfcn_index >= 0) {
     if (cur_earfcn_index >= (int) earfcn.size()) {
+      Info("SYNC:  Cell Search finished. Going to IDLE\n");
       cur_earfcn_index = 0;
+      phy_state = IDLE;
       rrc->earfcn_end();
     } else {
       Info("SYNC:  Cell Search idx %d/%d\n", cur_earfcn_index, earfcn.size());
@@ -330,23 +280,16 @@ void phch_recv::cell_search_inc()
         current_earfcn = earfcn[cur_earfcn_index];
         set_frequency();
       }
+      phy_state = CELL_SEARCH;
     }
   }
 }
 
 void phch_recv::cell_search_next(bool reset) {
-  if (cell_search_in_progress || reset) {
-    cell_search_in_progress = false;
-    if (!stop_sync()) {
-      log_h->warning("SYNC:  Couldn't stop PHY\n");
-    }
   if (reset) {
     cur_earfcn_index = -1;
   }
   cell_search_inc();
-    phy_state = CELL_SEARCH;
-    cell_search_in_progress = true;
-  }
 }
 
 void phch_recv::cell_search_start() {
@@ -363,16 +306,13 @@ void phch_recv::cell_search_start() {
   }
 }
 
-void phch_recv::cell_search_stop() {
-  Info("SYNC:  Stopping Cell Search procedure...\n");
-  if (!stop_sync()) {
-    Error("SYNC:  Stopping cell search\n");
-  }
-  cell_search_in_progress = false;
-}
-
 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);
@@ -383,20 +323,21 @@ bool phch_recv::cell_handover(srslte_cell_t cell)
   bool ret = false;
   this->cell = cell;
   Info("Cell HO: Stopping sync with current cell\n");
-  worker_com->reset_ul();
+  phy_state = IDLE;
-  phy_state = IDLE_RX;
   cnt = 0;
-  while(!is_in_idle_rx && cnt<20) {
+  while(!is_in_idle && cnt<20) {
     usleep(1000);
     cnt++;
   }
-  if (is_in_idle_rx) {
+  for(uint32_t i=0;i<workers_pool->get_nof_workers();i++) {
+    ((phch_worker*) workers_pool->get_worker(i))->reset();
+  }
+  worker_com->reset();
+  if (is_in_idle) {
     Info("Cell HO: Reconfiguring cell\n");
     if (set_cell()) {
-      //resync_sfn(true, true);
-      sfn_p.reset();
-      phy_state = CELL_RESELECT;
       Info("Cell HO: Synchronizing with new cell\n");
+      phy_state = CELL_SELECT;
       ret = true;
     } else {
       log_h->error("Cell HO: Configuring cell PCI=%d\n", cell.id);
@@ -409,31 +350,35 @@ bool phch_recv::cell_handover(srslte_cell_t cell)
   return ret;
 }
 
-bool phch_recv::cell_select(uint32_t earfcn, srslte_cell_t cell) {
+/* interface from higher layers to select a new cell */
+void phch_recv::cell_select(uint32_t earfcn, srslte_cell_t cell)
+{
+  Info("SYNC:  Cell Reselect to EARFCN=%d, PCI=%d\n", earfcn, cell.id);
+  new_earfcn = earfcn;
+  new_cell = cell;
+  phy_state = CELL_RESELECT;
+}
+
+/* Perform cell (re)-selection on IDLE or CAMP */
+void phch_recv::cell_reselect()
+{
+  uint32_t earfcn = new_earfcn;
+  srslte_cell_t cell = new_cell;
+
+  reset_sync();
 
-  // Check if we are already camping in this cell
+  // If we are already in the new cell, just resynchronize
   if (earfcn == current_earfcn && this->cell.id == cell.id) {
-    log_h->info("Cell Select: Already in cell EARFCN=%d\n", earfcn);
+    log_h->info("Cell Select: Already in cell EARFCN=%d, PCI=%d\n", earfcn, cell.id);
-    cell_search_in_progress = false;
     if (srate_mode != SRATE_CAMP) {
       set_sampling_rate();
+      log_h->info("Cell Select: Setting Camping sampling rate\n");
     }
-    if (phy_state < CELL_SELECT) {
-      resync_sfn();
-    }
-    return true;
   } else {
 
-    cell_search_in_progress = false;
-
-    if (!stop_sync()) {
-      log_h->warning("Still not in idle\n");
-    }
-
     if (earfcn != current_earfcn) {
       if (set_frequency()) {
         log_h->error("Cell Select: Configuring cell in EARFCN=%d, PCI=%d\n", earfcn, cell.id);
-        return false;
       }
       current_earfcn = earfcn;
     }
@@ -443,13 +388,7 @@ bool phch_recv::cell_select(uint32_t earfcn, srslte_cell_t cell) {
 
     if (set_cell()) {
       log_h->info("Cell Select: Synchronizing on cell...\n");
-
-      resync_sfn();
-
-      usleep(500000); // Time offset we set start_rx to start receiving samples
-      return true;
     }
-    return false;
   }
 }
 
@@ -485,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);
@@ -514,22 +472,6 @@ void phch_recv::set_sampling_rate()
   }
 }
 
-void phch_recv::stop_rx() {
-  if (radio_is_rx) {
-    Info("SYNC:  Stopping RX streaming\n");
-    radio_h->stop_rx();
-  }
-  radio_is_rx = false;
-}
-
-void phch_recv::start_rx(bool now) {
-  if (!radio_is_rx) {
-    Info("SYNC:  Starting RX streaming\n");
-    radio_h->start_rx(now);
-  }
-  radio_is_rx = true;
-}
-
 uint32_t phch_recv::get_current_tti() {
   return tti;
 }
@@ -584,60 +526,62 @@ void phch_recv::run_thread()
   uint32_t sf_idx = 0;
   phy_state  = IDLE;
   is_in_idle = true;
-  is_in_idle_rx = false;
+
+  cf_t *dummy_buffer[SRSLTE_MAX_PORTS];
+
+  for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
+    dummy_buffer[i] = (cf_t*) malloc(sizeof(cf_t)*SRSLTE_SF_LEN_PRB(100));
+  }
 
   while (running)
   {
     if (phy_state != IDLE) {
       is_in_idle = false;
-      Debug("SYNC:  state=%d\n", phy_state);
-    }
-
-    if (phy_state != IDLE_RX) {
-      is_in_idle_rx = false;
     }
+    Debug("SYNC:  state=%d\n", phy_state);
 
     log_h->step(tti);
     log_phy_lib_h->step(tti);
     
     sf_idx = tti%10;
 
+    prev_state = phy_state;
+
     switch (phy_state) {
       case CELL_SEARCH:
-        if (cell_search_in_progress)
-        {
         switch(search_p.run(&cell))
         {
         case search::CELL_FOUND:
           if (!srslte_cell_isvalid(&cell)) {
-              Error("SYNC:  Detected invalid cell\n");
+            Error("SYNC:  Detected invalid cell. Going to IDLE\n");
             phy_state = IDLE;
             break;
           }
           if (set_cell()) {
+            Info("SYNC:  Setting sampling rate and going to Cell Select\n");
             set_sampling_rate();
-              resync_sfn();
+            phy_state = CELL_SELECT;
           }
           break;
         case search::CELL_NOT_FOUND:
-            if (cell_search_in_progress) {
           cell_search_inc();
-            }
           break;
         default:
           radio_error();
           break;
         }
-        }
         break;
       case CELL_RESELECT:
+        cell_reselect();
+        break;
       case CELL_SELECT:
         switch (sfn_p.run_subframe(&cell, &tti))
         {
           case sfn_sync::SFN_FOUND:
-            if (!cell_search_in_progress) {
+            if (prev_state != CELL_SEARCH) {
               log_h->info("Sync OK. Camping on cell PCI=%d...\n", cell.id);
               phy_state = CELL_CAMP;
+              rrc->cell_camping(earfcn[cur_earfcn_index], cell);
             } else {
               log_h->info("Sync OK. Measuring PCI=%d...\n", cell.id);
               measure_p.reset();
@@ -645,13 +589,8 @@ void phch_recv::run_thread()
             }
             break;
           case sfn_sync::TIMEOUT:
-            if (cell_search_in_progress) {
+            log_h->warning("SYNC:  Timeout while synchronizing SFN\n");
-              log_h->warning("SYNC:  Timeout while synchronizing SFN. Going back to cell search\n");
+            rrc->out_of_sync();
-              phy_state = CELL_SEARCH;
-            } else {
-              log_h->warning("SYNC:  Timeout while synchronizing SFN. Reselecting cell\n");
-              resync_sfn(true, true);
-            }
             break;
           case sfn_sync::IDLE:
             break;
@@ -661,12 +600,21 @@ void phch_recv::run_thread()
         }
         break;
       case CELL_MEASURE:
+
         switch(measure_p.run_subframe_sync(&ue_sync, sf_idx))
         {
           case measure::MEASURE_OK:
+
+            // Calibrate measure object since worker not yet calibrated
+            if (worker_com->args->rssi_sensor_enabled) {
+              measure_p.set_rx_gain_offset(measure_p.rssi() - radio_h->get_rssi() + 30);
+            } else {
+              measure_p.set_rx_gain_offset(worker_com->args->rx_gain_offset + radio_h->get_rx_gain());
+            }
+
             log_h->info("SYNC:  Measured OK. Camping on cell PCI=%d...\n", cell.id);
             phy_state = CELL_CAMP;
-            rrc->cell_found(earfcn[cur_earfcn_index], cell, measure_p.rsrp());
+            rrc->cell_camping(earfcn[cur_earfcn_index], cell, measure_p.rsrp());
             break;
           case measure::IDLE:
             break;
@@ -700,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);
@@ -718,16 +664,17 @@ void phch_recv::run_thread()
               tx_mutex_cnt = (tx_mutex_cnt+1) % nof_tx_mutex;
 
               // Reset Uplink TX buffer to avoid mixing packets in TX queue
+              /*
               if (prach_buffer->is_pending()) {
                 Info("SYNC:  PRACH pending: Reset UL\n");
-                worker_com->reset_ul();
+                radio_h->tx_end();
-              }
+              }*/
 
               // Check if we need to TX a PRACH
               if (prach_buffer->is_ready_to_send(tti)) {
                 srslte_timestamp_copy(&tx_time_prach, &rx_time);
                 srslte_timestamp_add(&tx_time_prach, 0, prach::tx_advance_sf * 1e-3);
-                prach_buffer->send(radio_h, ul_dl_factor * metrics.cfo / 15000, worker_com->pathloss, tx_time_prach);
+                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);
@@ -738,7 +685,9 @@ void phch_recv::run_thread()
               if ((tti%5) == 0 && worker_com->args->sic_pss_enabled) {
                 srslte_pss_sic(&ue_sync.strack.pss, &buffer[0][SRSLTE_SF_LEN_PRB(cell.nof_prb)/2-ue_sync.strack.fft_size]);
               }
+              if (srslte_cell_isvalid(&cell)) {
                 intra_freq_meas.write(tti, buffer[0], SRSLTE_SF_LEN_PRB(cell.nof_prb));
+              }
               break;
             case 0:
               Warning("SYNC:  Out-of-sync detected in PSS/SSS\n");
@@ -756,28 +705,18 @@ void phch_recv::run_thread()
         }
         break;
       case IDLE:
-        if (!is_in_idle) {
+        if (radio_h->is_init()) {
-          stop_rx();
+          uint32_t nsamples = 1920;
-        }
+          if (current_srate > 0) {
-        is_in_idle = true;
+            nsamples = current_srate/1000;
-        usleep(1000);
-        break;
-      case IDLE_RX:
-        if (!worker) {
-          worker = (phch_worker *) workers_pool->wait_worker(tti);
-        }
-        is_in_idle_rx = true;
-        if (worker) {
-          for (uint32_t i = 0; i < SRSLTE_MAX_PORTS; i++) {
-            buffer[i] = worker->get_buffer(i);
           }
-          if (!radio_h->rx_now(buffer, SRSLTE_SF_LEN_PRB(cell.nof_prb), NULL)) {
+          if (!radio_h->rx_now(dummy_buffer, nsamples, NULL)) {
-            Error("SYNC:  Receiving from radio while in IDLE_RX\n");
+            printf("SYNC:  Receiving from radio while in IDLE_RX\n");
           }
         } else {
-          // wait_worker() only returns NULL if it's being closed. Quit now to avoid unnecessary loops here
+          usleep(1000);
-          running = false;
         }
+        is_in_idle = true;
         break;
     }
 
@@ -785,26 +724,34 @@ 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() {
-  out_of_sync_cnt = 0;
   in_sync_cnt++;
   // Send RRC in-sync signal after 100 ms consecutive subframes
   if (in_sync_cnt == NOF_IN_SYNC_SF) {
     rrc->in_sync();
     in_sync_cnt = 0;
+    out_of_sync_cnt = 0;
   }
 }
 
 // Out of sync called by worker or phch_recv every 1 or 5 ms
 void phch_recv::out_of_sync() {
-  in_sync_cnt = 0;
   // 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;
   }
 }
 
@@ -897,8 +844,8 @@ phch_recv::search::ret_code phch_recv::search::run(srslte_cell_t *cell)
   if (p->srate_mode != SRATE_FIND) {
     p->srate_mode = SRATE_FIND;
     p->radio_h->set_rx_srate(1.92e6);
+    Info("SYNC:  Setting Cell Search sampling rate\n");
   }
-  p->start_rx();
 
   /* Find a cell in the given N_id_2 or go through the 3 of them to find the strongest */
   uint32_t max_peak_cell = 0;
@@ -918,7 +865,6 @@ phch_recv::search::ret_code phch_recv::search::run(srslte_cell_t *cell)
     Error("SYNC:  Error decoding MIB: Error searching PSS\n");
     return ERROR;
   } else if (ret == 0) {
-    p->stop_rx();
     Info("SYNC:  Could not find any cell in this frequency\n");
     return CELL_NOT_FOUND;
   }
@@ -946,8 +892,6 @@ phch_recv::search::ret_code phch_recv::search::run(srslte_cell_t *cell)
   ret = srslte_ue_mib_sync_decode(&ue_mib_sync,
                                   40,
                                   bch_payload, &cell->nof_ports, &sfn_offset);
-  p->stop_rx();
-
   if (ret == 1) {
     srslte_pbch_mib_unpack(bch_payload, cell, NULL);
 
@@ -1022,7 +966,7 @@ phch_recv::sfn_sync::ret_code phch_recv::sfn_sync::run_subframe(srslte_cell_t *c
   srslte_ue_sync_decode_sss_on_track(ue_sync, true);
   int ret = srslte_ue_sync_zerocopy_multi(ue_sync, buffer);
   if (ret < 0) {
-    Error("SYNC:  Error calling ue_sync_get_buffer");
+    Error("SYNC:  Error calling ue_sync_get_buffer.\n");
     return ERROR;
   }
 
@@ -1080,10 +1024,9 @@ phch_recv::sfn_sync::ret_code phch_recv::sfn_sync::run_subframe(srslte_cell_t *c
 /*********
  * Measurement class 
  */
-void phch_recv::measure::init(cf_t *buffer[SRSLTE_MAX_PORTS], srslte::log *log_h, srslte::radio *radio_h, uint32_t nof_rx_antennas, uint32_t nof_subframes)
+void phch_recv::measure::init(cf_t *buffer[SRSLTE_MAX_PORTS], srslte::log *log_h, uint32_t nof_rx_antennas, uint32_t nof_subframes)
 
 {
-  this->radio_h       = radio_h;
   this->log_h         = log_h;
   this->nof_subframes = nof_subframes;
   for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
@@ -1094,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();
 }
 
@@ -1118,16 +1062,20 @@ void phch_recv::measure::set_cell(srslte_cell_t cell)
   reset();
 }
 
+float phch_recv::measure::rssi() {
+  return 10*log10(mean_rssi);
+}
+
 float phch_recv::measure::rsrp() {
-  return mean_rsrp;
+  return 10*log10(mean_rsrp) + 30 - rx_gain_offset;
 }
 
 float phch_recv::measure::rsrq() {
-  return mean_rsrq;
+  return 10*log10(mean_rsrq);
 }
 
 float phch_recv::measure::snr() {
-  return mean_snr;
+  return 10*log10(mean_snr);
 }
 
 uint32_t phch_recv::measure::frame_st_idx() {
@@ -1153,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)
 {
@@ -1162,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;
@@ -1196,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;
 
@@ -1228,10 +1180,10 @@ phch_recv::measure::ret_code phch_recv::measure::run_subframe(uint32_t sf_idx)
     return ERROR;
   }
 
-  float rsrp   = 10*log10(srslte_chest_dl_get_rsrp(&ue_dl.chest)) + 30 - rx_gain_offset;
+  float rsrp   = srslte_chest_dl_get_rsrp_neighbour(&ue_dl.chest);
-  float rsrq   = 10*log10(srslte_chest_dl_get_rsrq(&ue_dl.chest));
+  float rsrq   = srslte_chest_dl_get_rsrq(&ue_dl.chest);
-  float snr    = 10*log10(srslte_chest_dl_get_snr(&ue_dl.chest));
+  float snr    = srslte_chest_dl_get_snr(&ue_dl.chest);
-  float rssi   = 10*log10(srslte_vec_avg_power_cf(buffer[0], SRSLTE_SF_LEN_PRB(current_prb))) + 30;
+  float rssi   = srslte_vec_avg_power_cf(buffer[0], SRSLTE_SF_LEN_PRB(current_prb));
 
   if (cnt == 0) {
     mean_rsrp   = rsrp;
@@ -1246,21 +1198,10 @@ 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) {
-
-    // Calibrate RSRP if no gain offset measurements
-    if (fabsf(rx_gain_offset) < 1.0 && radio_h) {
-      float temporal_offset = 0;
-      if (radio_h->has_rssi()) {
-        temporal_offset = mean_rssi - radio_h->get_rssi() + 30;
-      } else {
-        temporal_offset = radio_h->get_rx_gain();
-      }
-      mean_rsrp -= temporal_offset;
-    }
     return MEASURE_OK;
   } else {
     return IDLE;
@@ -1287,15 +1228,15 @@ 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, NULL, 1, max_sf_window);
+  measure_p.init(sf_buffer, log_h, 1, max_sf_window);
 
   //do this different we don't need all this search window.
   if(srslte_sync_init(&sync_find, max_sf_window*max_sf_size, 5*max_sf_size, max_fft_sz)) {
     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);
@@ -1309,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();
 }
@@ -1321,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);
@@ -1365,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) {
@@ -1395,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++;
 
@@ -1516,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);
@@ -1594,6 +1539,7 @@ 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));
       int found_cells = scell.find_cells(search_buffer, common->rx_gain_offset, primary_cell, INTRA_FREQ_MEAS_LEN_MS, info);

srsue/src/phy/phch_worker.cc

@@ -137,32 +137,39 @@ 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;
 
+  pthread_mutex_init(&mutex, NULL);
   return true;
 }
 
 bool phch_worker::set_cell(srslte_cell_t cell_)
 {
+  bool ret = false;
+  pthread_mutex_lock(&mutex);
   if (cell.id != cell_.id || !cell_initiated) {
     memcpy(&cell, &cell_, sizeof(srslte_cell_t));
 
     if (srslte_ue_dl_set_cell(&ue_dl, cell)) {
       Error("Initiating UE DL\n");
-      return false;
+      goto unlock;
     }
 
     if (srslte_ue_ul_set_cell(&ue_ul, cell)) {
       Error("Initiating UE UL\n");
-      return false;
+      goto unlock;
     }
     srslte_ue_ul_set_normalization(&ue_ul, true);
     srslte_ue_ul_set_cfo_enable(&ue_ul, true);
 
     cell_initiated = true;
   }
-  return true;
+  ret = true;
+unlock:
+  pthread_mutex_unlock(&mutex);
+  return ret;
 }
 
 cf_t* phch_worker::get_buffer(uint32_t antenna_idx)
@@ -183,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);
@@ -224,26 +223,14 @@ 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) {
     return; 
   }
 
+  pthread_mutex_lock(&mutex);
+
   Debug("TTI %d running\n", tti);
 
 #ifdef LOG_EXECTIME
@@ -359,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; 
@@ -389,7 +376,9 @@ void phch_worker::work_imp()
 
   if (!dl_action.generate_ack_callback) {
     if (dl_mac_grant.rnti_type == SRSLTE_RNTI_PCH && dl_action.decode_enabled[0]) {
+      if (dl_ack[0]) {
         phy->mac->pch_decoded_ok(dl_mac_grant.n_bytes[0]);
+      }
     } else if (!rar_delivered) {
       for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
         if (dl_action.decode_enabled[tb]) {
@@ -402,17 +391,19 @@ void phch_worker::work_imp()
   update_measurements();
 
   if (chest_ok) {
-    if (phy->avg_rsrp_dbm > -130.0 && phy->avg_snr_db > -30.0) {
+    if (phy->avg_rsrp_dbm > -130.0 && phy->avg_snr_db > -10.0) {
       log_h->debug("SNR=%.1f dB, RSRP=%.1f dBm sync=in-sync from channel estimator\n",
-                   10*log10(srslte_chest_dl_get_snr(&ue_dl.chest)), phy->avg_rsrp_dbm);
+                   phy->avg_snr_db, phy->avg_rsrp_dbm);
       chest_loop->in_sync();
     } else {
       log_h->warning("SNR=%.1f dB RSRP=%.1f dBm, sync=out-of-sync from channel estimator\n",
-                   10*log10(srslte_chest_dl_get_snr(&ue_dl.chest)), phy->avg_rsrp_dbm);
+                     phy->avg_snr_db, phy->avg_rsrp_dbm);
       chest_loop->out_of_sync();
     }
   }
 
+  pthread_mutex_unlock(&mutex);
+
   /* Tell the plotting thread to draw the plots */
 #ifdef ENABLE_GUI
   if ((int) get_id() == plot_worker_id) {
@@ -510,10 +501,17 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
     srslte_dci_msg_t dci_msg; 
     srslte_ra_dl_dci_t dci_unpacked;
 
-    Debug("Looking for RNTI=0x%x\n", dl_rnti);
+    if (type == SRSLTE_RNTI_RAR) {
+      Info("Looking for RNTI=0x%x\n", dl_rnti);
+    }
 
     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, 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;
     }
     
@@ -554,10 +552,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);
@@ -792,10 +790,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", 
@@ -1211,6 +1209,7 @@ void phch_worker::enable_pregen_signals(bool enabled)
 
 void phch_worker::set_ul_params(bool pregen_disabled)
 {
+
   phy_interface_rrc::phy_cfg_common_t         *common    = &phy->config->common;
   LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *dedicated = &phy->config->dedicated;
   
@@ -1380,7 +1379,7 @@ void phch_worker::update_measurements()
 
     /* Only worker 0 reads the RSSI sensor every ~1-nof_cores s */
     if (get_id() == 0) {
-      if (rssi_read_cnt) {
+      if (!rssi_read_cnt) {
         if (phy->get_radio()->has_rssi() && phy->args->rssi_sensor_enabled) {
           phy->last_radio_rssi = phy->get_radio()->get_rssi();
           phy->rx_gain_offset = phy->avg_rssi_dbm - phy->last_radio_rssi + 30;

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);
 
@@ -256,11 +255,6 @@ void phy::cell_search_start()
   sf_recv.cell_search_start();
 }
 
-void phy::cell_search_stop()
-{
-  sf_recv.cell_search_stop();
-}
-
 void phy::cell_search_next()
 {
   sf_recv.cell_search_next();
@@ -282,9 +276,9 @@ int phy::meas_stop(uint32_t earfcn, int pci) {
   return sf_recv.meas_stop(earfcn, pci);
 }
 
-bool phy::cell_select(uint32_t earfcn, srslte_cell_t phy_cell)
+void phy::cell_select(uint32_t earfcn, srslte_cell_t phy_cell)
 {
-  return sf_recv.cell_select(earfcn, phy_cell);
+  sf_recv.cell_select(earfcn, phy_cell);
 }
 
 bool phy::cell_handover(srslte_cell_t cell) {
@@ -366,8 +360,6 @@ void phy::reset()
     workers[i].reset();
   }
   workers_common.reset();
-  usleep(4000);
-  workers_common.reset_ul();
 }
 
 uint32_t phy::get_current_tti()

srsue/src/ue.cc

@@ -40,12 +40,13 @@ namespace srsue{
 ue::ue()
     :started(false)
 {
-  pool = byte_buffer_pool::get_instance();
 }
 
 ue::~ue()
 {
-  byte_buffer_pool::cleanup();
+  for (uint32_t i = 0; i < phy_log.size(); i++) {
+    delete(phy_log[i]);
+  }
 }
 
 bool ue::init(all_args_t *args_)
@@ -192,7 +193,8 @@ bool ue::init(all_args_t *args_)
   pdcp.init(&rlc, &rrc, &gw, &pdcp_log, 0 /* RB_ID_SRB0 */, SECURITY_DIRECTION_UPLINK);
 
   usim.init(&args->usim, &usim_log);
-  nas.init(&usim, &rrc, &gw, &nas_log, 1 /* RB_ID_SRB1 */);
+  srslte_nas_config_t nas_cfg(1, args->apn); /* RB_ID_SRB1 */
+  nas.init(&usim, &rrc, &gw, &nas_log, nas_cfg);
   gw.init(&pdcp, &nas, &gw_log, 3 /* RB_ID_DRB1 */);
 
   gw.set_netmask(args->expert.ip_netmask);
@@ -273,13 +275,17 @@ void ue::stop()
 
 bool ue::is_attached()
 {
-  return (RRC_STATE_CONNECTED == rrc.get_state());
+  return rrc.is_connected();
 }
 
 void ue::start_plot() {
   phy.start_plot();
 }
 
+void ue::print_pool() {
+  byte_buffer_pool::get_instance()->print_all_buffers();
+}
+
 bool ue::get_metrics(ue_metrics_t &m)
 {
   m.rf = rf_metrics;
@@ -308,6 +314,11 @@ void ue::rf_msg(srslte_rf_error_t error)
   ue->handle_rf_msg(error);
   if (error.type == srslte_rf_error_t::SRSLTE_RF_ERROR_OVERFLOW) {
     ue->radio_overflow();
+  } else
+  if (error.type == srslte_rf_error_t::SRSLTE_RF_ERROR_RX) {
+    ue->stop();
+    ue->cleanup();
+    exit(-1);
   }
 }
 

srsue/src/ue_base.cc

@@ -64,6 +64,12 @@ ue_base::ue_base() {
 
   // load FFTW wisdom
   srslte_dft_load();
+
+  pool = byte_buffer_pool::get_instance();
+}
+
+ue_base::~ue_base() {
+  byte_buffer_pool::cleanup();
 }
 
 void ue_base::cleanup(void)

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,15 +148,17 @@ 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
 void nas::plmn_search_end() {
   if (known_plmns.size() > 0) {
+    if (home_plmn.mcc != known_plmns[0].mcc && home_plmn.mnc != known_plmns[0].mnc) {
       nas_log->info("Could not find Home PLMN Id=%s, trying to connect to PLMN Id=%s\n",
                     plmn_id_to_string(home_plmn).c_str(),
                     plmn_id_to_string(known_plmns[0]).c_str());
@@ -168,10 +166,13 @@ void nas::plmn_search_end() {
       nas_log->console("Could not find Home PLMN Id=%s, trying to connect to PLMN Id=%s\n",
                        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->debug("Finished searching PLMN in current EARFCN set but no networks were found.\n");
+    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) {
+      rrc->plmn_search();
+    }
   }
 }
 
@@ -218,7 +219,7 @@ void nas::write_pdu(uint32_t lcid, byte_buffer_t *pdu) {
     default:
       nas_log->error("Not handling NAS message with SEC_HDR_TYPE=%02X\n", sec_hdr_type);
       pool->deallocate(pdu);
-      break;
+      return;
   }
 
   // Write NAS pcap
@@ -262,7 +263,7 @@ void nas::write_pdu(uint32_t lcid, byte_buffer_t *pdu) {
     default:
       nas_log->error("Not handling NAS message with MSG_TYPE=%02X\n", msg_type);
       pool->deallocate(pdu);
-      break;
+      return;
   }
 }
 
@@ -534,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++;
 
@@ -543,6 +545,8 @@ void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu) {
     act_def_eps_bearer_context_accept.protocol_cnfg_opts_present = false;
     liblte_mme_pack_activate_default_eps_bearer_context_accept_msg(&act_def_eps_bearer_context_accept,
                                                                    &attach_complete.esm_msg);
+
+    pdu->reset();
     liblte_mme_pack_attach_complete_msg(&attach_complete,
                                         LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY_AND_CIPHERED,
                                         ctxt.tx_count,
@@ -758,36 +762,36 @@ void nas::parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu)
   }
 
   // Send response
-  byte_buffer_t *sdu = pool_allocate;
+  pdu->reset();
   liblte_mme_pack_security_mode_complete_msg(&sec_mode_comp,
                                              LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY_AND_CIPHERED_WITH_NEW_EPS_SECURITY_CONTEXT,
                                              ctxt.tx_count,
-                                             (LIBLTE_BYTE_MSG_STRUCT *) sdu);
+                                             (LIBLTE_BYTE_MSG_STRUCT *) pdu);
   if(pcap != NULL) {
-    pcap->write_nas(sdu->msg, sdu->N_bytes);
+    pcap->write_nas(pdu->msg, pdu->N_bytes);
   }
-  cipher_encrypt(sdu);
+  cipher_encrypt(pdu);
   integrity_generate(&k_nas_int[16],
                      ctxt.tx_count,
                      SECURITY_DIRECTION_UPLINK,
-                     &sdu->msg[5],
+                     &pdu->msg[5],
-                     sdu->N_bytes - 5,
+                     pdu->N_bytes - 5,
-                     &sdu->msg[1]);
+                     &pdu->msg[1]);
   nas_log->info("Sending Security Mode Complete nas_current_ctxt.tx_count=%d, RB=%s\n",
                 ctxt.tx_count,
                 rrc->get_rb_name(lcid).c_str());
-  rrc->write_sdu(lcid, sdu);
+  rrc->write_sdu(lcid, pdu);
   ctxt.tx_count++;
-  pool->deallocate(pdu);
 }
 
 void nas::parse_service_reject(uint32_t lcid, byte_buffer_t *pdu) {
   nas_log->error("TODO:parse_service_reject\n");
+  pool->deallocate(pdu);
 }
 
 void nas::parse_esm_information_request(uint32_t lcid, byte_buffer_t *pdu) {
   nas_log->error("TODO:parse_esm_information_request\n");
-
+  pool->deallocate(pdu);
 }
 
 void nas::parse_emm_information(uint32_t lcid, byte_buffer_t *pdu) {
@@ -796,6 +800,7 @@ void nas::parse_emm_information(uint32_t lcid, byte_buffer_t *pdu) {
   nas_log->info("Received EMM Information: %s\n", str.c_str());
   nas_log->console("%s\n", str.c_str());
   ctxt.rx_count++;
+  pool->deallocate(pdu);
 }
 
 /*******************************************************************************
@@ -805,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;
@@ -889,7 +899,14 @@ void nas::gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg) {
 
   // Set the optional flags
   pdn_con_req.esm_info_transfer_flag_present = false; //FIXME: Check if this is needed
+  if (cfg.apn == "") {
     pdn_con_req.apn_present = false;
+  } else {
+    pdn_con_req.apn_present = true;
+    LIBLTE_MME_ACCESS_POINT_NAME_STRUCT apn;
+    apn.apn = cfg.apn;
+    pdn_con_req.apn = apn;
+  }
   pdn_con_req.protocol_cnfg_opts_present = false;
   pdn_con_req.device_properties_present = false;
 
@@ -899,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;
@@ -914,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);