Merge branch 'next'

CMakeLists.txt

@@ -371,7 +371,7 @@ if(RF_FOUND)
     message(STATUS "Building with srsENB")
     add_subdirectory(srsenb)
   else(ENABLE_SRSENB)
-    message(STATUS "srsUE build disabled")
+    message(STATUS "srsENB build disabled")
   endif(ENABLE_SRSENB)
 else(RF_FOUND)
   message(STATUS "srsUE and srsENB builds disabled due to missing RF driver")

lib/examples/pdsch_enodeb.c

@@ -353,11 +353,6 @@ void base_init() {
     exit(-1);
   }
 
-  if (srslte_regs_set_cfi(&regs, cfi)) {
-    fprintf(stderr, "Error setting CFI\n");
-    exit(-1);
-  }
-
   if (srslte_pdcch_init_enb(&pdcch, cell.nof_prb)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);

lib/examples/pdsch_ue.c

@@ -162,8 +162,8 @@ void usage(prog_args_t *args, char *prog) {
   printf("\t-r RNTI in Hex [Default 0x%x]\n",args->rnti);
   printf("\t-l Force N_id_2 [Default best]\n");
   printf("\t-C Disable CFO correction [Default %s]\n", args->disable_cfo?"Disabled":"Enabled");
-  printf("\t-F Enable RS-based CFO correction [Default %s]\n", args->enable_cfo_ref?"Disabled":"Enabled");
+  printf("\t-F Enable RS-based CFO correction [Default %s]\n", !args->enable_cfo_ref?"Disabled":"Enabled");
-  printf("\t-R Average channel estimates on 1 ms [Default %s]\n", args->average_subframe?"Disabled":"Enabled");
+  printf("\t-R Average channel estimates on 1 ms [Default %s]\n", !args->average_subframe?"Disabled":"Enabled");
   printf("\t-t Add time offset [Default %d]\n", args->time_offset);
 #ifndef DISABLE_GRAPHICS
   printf("\t-d disable plots [Default enabled]\n");
@@ -1029,7 +1029,7 @@ void *plot_thread_run(void *arg) {
         
       }
       
-      plot_scatter_setNewData(&pscatequal_pdcch, ue_dl.pdcch.d, 36*ue_dl.pdcch.nof_cce);
+      plot_scatter_setNewData(&pscatequal_pdcch, ue_dl.pdcch.d, 36*ue_dl.pdcch.nof_cce[0]);
     }
     
     plot_scatter_setNewData(&pscatequal, ue_dl.pdsch.d[0], nof_symbols);

lib/include/srslte/common/buffer_pool.h

@@ -99,7 +99,6 @@ public:
       
       if (is_almost_empty()) {
         printf("Warning buffer pool capacity is %f %%\n", (float) 100*available.size()/capacity);
-        print_all_buffers();
       }
 #ifdef SRSLTE_BUFFER_POOL_LOG_ENABLED
     if (debug_name) {

lib/include/srslte/common/log.h

@@ -70,6 +70,7 @@ public:
     level = LOG_LEVEL_NONE;
     hex_limit = 0;
     show_layer_en = true;
+    add_string_en = false;
     level_text_short = true;
   }
 
@@ -79,6 +80,7 @@ public:
     level = LOG_LEVEL_NONE;
     hex_limit = 0;
     show_layer_en = true;
+    add_string_en = false;
     level_text_short = true;
   }
 

lib/include/srslte/interfaces/ue_interfaces.h

@@ -161,7 +161,7 @@ public:
   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 new_phy_meas(float rsrp, float rsrq, uint32_t tti, uint32_t earfcn = 0, uint32_t pci = 0) = 0;
+  virtual void new_phy_meas(float rsrp, float rsrq, uint32_t tti, int earfcn = -1, int pci = -1) = 0;
 };
 
 // RRC interface for NAS

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

@@ -60,8 +60,8 @@ typedef enum SRSLTE_API {
 /* PDCCH object */
 typedef struct SRSLTE_API {
   srslte_cell_t cell;
-  uint32_t nof_regs;
+  uint32_t nof_regs[3];
-  uint32_t nof_cce;
+  uint32_t nof_cce[3];
   uint32_t max_bits;
   uint32_t nof_rx_antennas;
   bool     is_ue;
@@ -99,10 +99,7 @@ SRSLTE_API int srslte_pdcch_set_cell(srslte_pdcch_t *q,
 SRSLTE_API void srslte_pdcch_free(srslte_pdcch_t *q);
 
 
-SRSLTE_API void srslte_pdcch_set_cfi(srslte_pdcch_t *q, 
+SRSLTE_API float srslte_pdcch_coderate(uint32_t nof_bits,
-                                     uint32_t cfi); 
-
-SRSLTE_API float srslte_pdcch_coderate(uint32_t nof_bits, 
                                        uint32_t l); 
 
 /* Encoding function */
@@ -134,6 +131,7 @@ SRSLTE_API int srslte_pdcch_decode_msg(srslte_pdcch_t *q,
                                        srslte_dci_msg_t *msg, 
                                        srslte_dci_location_t *location,
                                        srslte_dci_format_t format,
+                                       uint32_t cfi,
                                        uint16_t *crc_rem);
 
 SRSLTE_API int srslte_pdcch_dci_decode(srslte_pdcch_t *q, 

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

@@ -63,8 +63,6 @@ typedef struct SRSLTE_API {
 typedef struct SRSLTE_API {
   srslte_cell_t cell;
   uint32_t max_ctrl_symbols;
-  uint32_t cfi;
-  bool cfi_initiated;
   uint32_t ngroups_phich;
   
   srslte_phich_resources_t phich_res;
@@ -83,27 +81,30 @@ SRSLTE_API int srslte_regs_init(srslte_regs_t *h,
                                 srslte_cell_t cell);
 
 SRSLTE_API void srslte_regs_free(srslte_regs_t *h);
-SRSLTE_API int srslte_regs_set_cfi(srslte_regs_t *h, 
-                                   uint32_t nof_ctrl_symbols);
 
 
-SRSLTE_API uint32_t srslte_regs_pcfich_nregs(srslte_regs_t *h);
+SRSLTE_API int srslte_regs_pdcch_nregs(srslte_regs_t *h,
-SRSLTE_API int srslte_regs_pcfich_put(srslte_regs_t *h, 
+                                       uint32_t cfi);
-                               cf_t symbols[REGS_PCFICH_NSYM], 
+
-                               cf_t *slot_symbols);
+SRSLTE_API int srslte_regs_pdcch_ncce(srslte_regs_t *h,
+                                      uint32_t cfi);
+
+SRSLTE_API int srslte_regs_pcfich_put(srslte_regs_t *h,
+                                      cf_t symbols[REGS_PCFICH_NSYM],
+                                      cf_t *slot_symbols);
 
 SRSLTE_API int srslte_regs_pcfich_get(srslte_regs_t *h,
-                                      cf_t *slot_symbols, 
+                                      cf_t *slot_symbols,
                                       cf_t symbols[REGS_PCFICH_NSYM]);
 
 SRSLTE_API uint32_t srslte_regs_phich_nregs(srslte_regs_t *h);
-SRSLTE_API int srslte_regs_phich_add(srslte_regs_t *h, 
+SRSLTE_API int srslte_regs_phich_add(srslte_regs_t *h,
-                                     cf_t symbols[REGS_PHICH_NSYM], 
+                                     cf_t symbols[REGS_PHICH_NSYM],
                                      uint32_t ngroup, 
                                      cf_t *slot_symbols);
 
-SRSLTE_API int srslte_regs_phich_get(srslte_regs_t *h, 
+SRSLTE_API int srslte_regs_phich_get(srslte_regs_t *h,
-                                     cf_t *slot_symbols, 
+                                     cf_t *slot_symbols,
                                      cf_t symbols[REGS_PHICH_NSYM], 
                                      uint32_t ngroup);
 
@@ -111,28 +112,26 @@ SRSLTE_API uint32_t srslte_regs_phich_ngroups(srslte_regs_t *h);
 SRSLTE_API int srslte_regs_phich_reset(srslte_regs_t *h, 
                                        cf_t *slot_symbols);
 
-SRSLTE_API int srslte_regs_pdcch_nregs(srslte_regs_t *h, 
+SRSLTE_API int srslte_regs_pdcch_put(srslte_regs_t *h,
-                                       uint32_t cfi);
+                                     uint32_t cfi,
-
+                                     cf_t *d,
-SRSLTE_API int srslte_regs_pdcch_ncce(srslte_regs_t *h, 
-                                      uint32_t cfi); 
-
-SRSLTE_API int srslte_regs_pdcch_put(srslte_regs_t *h, 
-                                     cf_t *d, 
                                      cf_t *slot_symbols);
 
-SRSLTE_API int srslte_regs_pdcch_put_offset(srslte_regs_t *h, 
+SRSLTE_API int srslte_regs_pdcch_put_offset(srslte_regs_t *h,
-                                     cf_t *d, 
+                                            uint32_t cfi,
-                                     cf_t *slot_symbols, 
+                                            cf_t *d,
-                                     uint32_t start_reg, 
+                                            cf_t *slot_symbols,
-                                     uint32_t nof_regs);
+                                            uint32_t start_reg,
+                                            uint32_t nof_regs);
 
-SRSLTE_API int srslte_regs_pdcch_get(srslte_regs_t *h, 
+SRSLTE_API int srslte_regs_pdcch_get(srslte_regs_t *h,
-                                     cf_t *slot_symbols, 
+                                     uint32_t cfi,
+                                     cf_t *slot_symbols,
                                      cf_t *d);
 
-SRSLTE_API int srslte_regs_pdcch_get_offset(srslte_regs_t *h, 
+SRSLTE_API int srslte_regs_pdcch_get_offset(srslte_regs_t *h,
-                                            cf_t *slot_symbols, 
+                                            uint32_t cfi,
+                                            cf_t *slot_symbols,
                                             cf_t *d, 
                                             uint32_t start_reg, 
                                             uint32_t nof_regs);

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

@@ -5,9 +5,11 @@
 #include "srslte/config.h"
 #include <pthread.h>
 #include <stdint.h>
+#include <stdbool.h>
 
 typedef struct {
-  uint8_t *buffer; 
+  uint8_t *buffer;
+  bool active;
   int capacity; 
   int count; 
   int wpm; 
@@ -34,6 +36,7 @@ SRSLTE_API int  srslte_ringbuffer_read(srslte_ringbuffer_t *q,
                                        void *ptr,
                                        int nof_bytes);
 
+SRSLTE_API void srslte_ringbuffer_stop(srslte_ringbuffer_t *q);
 
 #endif
 

lib/include/srslte/radio/radio.h

@@ -101,6 +101,7 @@ namespace srslte {
       void set_tx_freq(double freq);
       void set_rx_freq(double freq);
 
+      double get_freq_offset();
       double get_tx_freq();
       double get_rx_freq();
 

lib/include/srslte/upper/rlc.h

@@ -55,9 +55,9 @@ public:
   void init(srsue::pdcp_interface_rlc *pdcp_,
             srsue::rrc_interface_rlc  *rrc_,
             srsue::ue_interface       *ue_,
-            log        *rlc_log_, 
+            log                       *rlc_log_,
-            mac_interface_timers *mac_timers_,
+            mac_interface_timers      *mac_timers_,
-            uint32_t                  lcid_);
+            uint32_t                   lcid_);
   void stop();
 
   void get_metrics(rlc_metrics_t &m);

lib/include/srslte/upper/rlc_am.h

@@ -40,7 +40,7 @@
 
 namespace srslte {
 
-
+#undef RLC_AM_BUFFER_DEBUG
 
 struct rlc_amd_rx_pdu_t{
   rlc_amd_pdu_header_t  header;
@@ -189,6 +189,7 @@ private:
   bool inside_tx_window(uint16_t sn);
   bool inside_rx_window(uint16_t sn);
   void debug_state();
+  void print_rx_segments();
 
   bool add_segment_and_check(rlc_amd_rx_pdu_segments_t *pdu, rlc_amd_rx_pdu_t *segment);
   int  required_buffer_size(rlc_amd_retx_t retx);

lib/src/common/log_filter.cc

@@ -126,7 +126,7 @@ void log_filter::all_log(srslte::LOG_LEVEL_ENUM level,
       ss << std::endl; 
     }
     
-    if (hex_limit > 0) {
+    if (hex_limit > 0 && hex && size > 0) {
       ss << hex_string(hex, size);
     } 
     str_ptr s_ptr(new std::string(ss.str()));

lib/src/common/logger_file.cc

@@ -36,6 +36,8 @@ namespace srslte{
 logger_file::logger_file()
   :inited(false)
   ,not_done(true)
+  ,cur_length(0)
+  ,max_length(0)
 {}
 
 logger_file::~logger_file() {
@@ -48,11 +50,11 @@ logger_file::~logger_file() {
   }
 }
 
-void logger_file::init(std::string file, int max_length) {
+void logger_file::init(std::string file, int max_length_) {
   pthread_mutex_init(&mutex, NULL); 
   pthread_cond_init(&not_empty, NULL);
   pthread_cond_init(&not_full, NULL);
-  this->max_length = max_length*1024;
+  max_length = max_length_*1024;
   name_idx = 0;
   filename = file;
   logfile = fopen(filename.c_str(), "w");

lib/src/phy/ch_estimation/chest_dl.c

@@ -282,10 +282,14 @@ static float estimate_noise_pilots(srslte_chest_dl_t *q, uint32_t port_id, srslt
   
   /* Compute average power. Normalized for filter len 3 using matlab */
   float norm  = 1;
-  if (q->smooth_filter_len == 3) {
+  if (q->average_subframe) {
-    float a = q->smooth_filter[0];
+    norm = 32;
-    float norm3 = 6.143*a*a+0.04859*a-0.002774;
+  } else {
-    norm /= norm3; 
+    if (q->smooth_filter_len == 3) {
+      float a = q->smooth_filter[0];
+      float norm3 = 6.143*a*a+0.04859*a-0.002774;
+      norm /= norm3;
+    }
   }
   float power = norm*q->cell.nof_ports*srslte_vec_avg_power_cf(q->tmp_noise, nref);
   return power; 
@@ -539,8 +543,7 @@ 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);
-  float energy = cabsf(srslte_vec_acc_cc(q->pilot_estimates, npilots)/npilots);
+  q->rsrp[rxant_id][port_id] = __real__ srslte_vec_dot_prod_conj_ccc(q->pilot_estimates, q->pilot_estimates, npilots) / npilots;
-  q->rsrp[rxant_id][port_id] = energy*energy;
   q->rssi[rxant_id][port_id] = srslte_chest_dl_rssi(q, input, port_id);
 }
 

lib/src/phy/enb/enb_dl.c

@@ -142,7 +142,6 @@ int srslte_enb_dl_set_cell(srslte_enb_dl_t *q, srslte_cell_t cell)
   if (q                 != NULL &&
       srslte_cell_isvalid(&cell))
   {
-    srslte_enb_dl_set_cfi(q, 3);
     q->tx_amp = SRSLTE_ENB_RF_AMP;
 
     if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
@@ -207,10 +206,9 @@ void srslte_enb_dl_set_amp(srslte_enb_dl_t *q, float amp)
   q->tx_amp = amp; 
 }
 
-void srslte_enb_dl_set_cfi(srslte_enb_dl_t *q, uint32_t cfi) 
+void srslte_enb_dl_set_cfi(srslte_enb_dl_t *q, uint32_t cfi)
 {
-  q->cfi = cfi; 
+  q->cfi = cfi;
-  srslte_regs_set_cfi(&q->regs, cfi);
 }
 
 void srslte_enb_dl_set_power_allocation(srslte_enb_dl_t *q, float rho_a, float rho_b)

lib/src/phy/fec/viterbi37_neon.c

@@ -176,7 +176,7 @@ void print_uint8x16_t(char *s, uint8x16_t val) {
   printf("\n");
 }
 
-int movemask_neon(uint8x16_t movemask_low_in)
+static inline int movemask_neon(uint8x16_t movemask_low_in)
 {
     uint8x8_t lo = vget_low_u8(movemask_low_in);
     uint8x8_t hi = vget_high_u8(movemask_low_in);

lib/src/phy/phch/dci.c

@@ -426,7 +426,8 @@ uint32_t srslte_dci_dl_info(char *info_str, uint32_t len, srslte_ra_dl_dci_t *dc
     n += snprintf(&info_str[n], len - n, "%d}, ", dci_msg->ndi_1);
   }
 
-  if (format == SRSLTE_DCI_FORMAT1 || format == SRSLTE_DCI_FORMAT1A || format == SRSLTE_DCI_FORMAT1B) {
+  if (format == SRSLTE_DCI_FORMAT1 || format == SRSLTE_DCI_FORMAT1A || format == SRSLTE_DCI_FORMAT1B ||
+      format == SRSLTE_DCI_FORMAT2 || format == SRSLTE_DCI_FORMAT2A || format == SRSLTE_DCI_FORMAT2B) {
     n += snprintf(&info_str[n], len-n, "tpc_pucch=%d, ", dci_msg->tpc_pucch);
   }
   if (format == SRSLTE_DCI_FORMAT2 || format == SRSLTE_DCI_FORMAT2A || format == SRSLTE_DCI_FORMAT2B) {

lib/src/phy/phch/pdcch.c

@@ -45,16 +45,8 @@
 #define PDCCH_FORMAT_NOF_REGS(i)        ((1<<i)*9)
 #define PDCCH_FORMAT_NOF_BITS(i)        ((1<<i)*72)
 
-static void set_cfi(srslte_pdcch_t *q, uint32_t cfi) {
+#define NOF_CCE(cfi)  ((cfi>0&&cfi<4)?q->nof_cce[cfi-1]:0)
-  if (cfi > 0 && cfi < 4) {
+#define NOF_REGS(cfi) ((cfi>0&&cfi<4)?q->nof_regs[cfi-1]:0)
-    q->nof_regs = (srslte_regs_pdcch_nregs(q->regs, cfi) / 9) * 9;
-    q->nof_cce = q->nof_regs / 9;
-  } 
-}
-
-void srslte_pdcch_set_cfi(srslte_pdcch_t *q, uint32_t cfi) {
-  set_cfi(q, cfi);
-}
 
 float srslte_pdcch_coderate(uint32_t nof_bits, uint32_t l) {
   return (float) (nof_bits+16)/(4*PDCCH_FORMAT_NOF_REGS(l));
@@ -73,7 +65,7 @@ static int pdcch_init(srslte_pdcch_t *q, uint32_t max_prb, uint32_t nof_rx_anten
     q->is_ue           = is_ue;
     /* Allocate memory for the maximum number of PDCCH bits (CFI=3) */
     q->max_bits = max_prb*3*12*2;
-
+    
     INFO("Init PDCCH: Max bits: %d\n", q->max_bits);
 
     if (srslte_modem_table_lte(&q->mod, SRSLTE_MOD_QPSK)) {
@@ -188,8 +180,13 @@ int srslte_pdcch_set_cell(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t
   {
     q->regs = regs;
 
+    for (int cfi=0;cfi<3;cfi++) {
+      q->nof_regs[cfi] = (srslte_regs_pdcch_nregs(q->regs, cfi+1) / 9) * 9;
+      q->nof_cce[cfi]  = q->nof_regs[cfi]/ 9;
+    }
+
     /* Allocate memory for the maximum number of PDCCH bits (CFI=3) */
-    q->max_bits = (srslte_regs_pdcch_nregs(q->regs, 3) / 9) * 72;
+    q->max_bits = (NOF_REGS(3)/ 9) * 72;
 
     INFO("PDCCH: Cell config PCI=%d, %d ports.\n",
          q->cell.id, q->cell.nof_ports);
@@ -214,8 +211,7 @@ int srslte_pdcch_set_cell(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t
 uint32_t srslte_pdcch_ue_locations(srslte_pdcch_t *q, srslte_dci_location_t *c, uint32_t max_candidates,
                         uint32_t nsubframe, uint32_t cfi, uint16_t rnti) 
 {
-  set_cfi(q, cfi);
+  return srslte_pdcch_ue_locations_ncce(NOF_CCE(cfi), c, max_candidates, nsubframe, rnti);
-  return srslte_pdcch_ue_locations_ncce(q->nof_cce, c, max_candidates, nsubframe, rnti);
 }
 
 
@@ -286,8 +282,7 @@ uint32_t srslte_pdcch_ue_locations_ncce_L(uint32_t nof_cce, srslte_dci_location_
 uint32_t srslte_pdcch_common_locations(srslte_pdcch_t *q, srslte_dci_location_t *c, uint32_t max_candidates, 
                                 uint32_t cfi) 
 {
-  set_cfi(q, cfi);
+  return srslte_pdcch_common_locations_ncce(NOF_CCE(cfi), c, max_candidates); 
-  return srslte_pdcch_common_locations_ncce(q->nof_cce, c, max_candidates); 
 }
 
 uint32_t srslte_pdcch_common_locations_ncce(uint32_t nof_cce, srslte_dci_location_t *c, uint32_t max_candidates) 
@@ -298,9 +293,10 @@ uint32_t srslte_pdcch_common_locations_ncce(uint32_t nof_cce, srslte_dci_locatio
   for (l = 3; l > 1; l--) {
     L = (1 << l);
     for (i = 0; i < SRSLTE_MIN(nof_cce, 16) / (L); i++) {
-      if (k < max_candidates) {
+      uint32_t ncce = (L) * (i % (nof_cce / (L)));
-        c[k].L = l;
+      if (k < max_candidates && ncce + L <= nof_cce) {
-        c[k].ncce = (L) * (i % (nof_cce / (L)));
+        c[k].L    = l;
+        c[k].ncce = ncce;
         DEBUG("Common SS Candidate %d: nCCE: %d, L: %d\n",
             k, c[k].ncce, c[k].L);
         k++;
@@ -370,7 +366,8 @@ int srslte_pdcch_dci_decode(srslte_pdcch_t *q, float *e, uint8_t *data, uint32_t
 int srslte_pdcch_decode_msg(srslte_pdcch_t *q, 
                             srslte_dci_msg_t *msg, 
                             srslte_dci_location_t *location, 
-                            srslte_dci_format_t format, 
+                            srslte_dci_format_t format,
+                            uint32_t cfi,
                             uint16_t *crc_rem) 
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
@@ -379,9 +376,9 @@ int srslte_pdcch_decode_msg(srslte_pdcch_t *q,
       srslte_dci_location_isvalid(location))
   {
     if (location->ncce * 72 + PDCCH_FORMAT_NOF_BITS(location->L) > 
-      q->nof_cce*72) {
+      NOF_CCE(cfi)*72) {
       fprintf(stderr, "Invalid location: nCCE: %d, L: %d, NofCCE: %d\n", 
-        location->ncce, location->L, q->nof_cce);
+        location->ncce, location->L, NOF_CCE(cfi));
     } else {
       ret = SRSLTE_SUCCESS;
       
@@ -457,9 +454,8 @@ int srslte_pdcch_extract_llr_multi(srslte_pdcch_t *q, cf_t *sf_symbols[SRSLTE_MA
       cfi               >  0    &&
       cfi               <  4)
   {
-    set_cfi(q, cfi);
     
-    uint32_t e_bits = 72*q->nof_cce;
+    uint32_t e_bits = 72*NOF_CCE(cfi);
     nof_symbols = e_bits/2;
     ret = SRSLTE_ERROR;
     bzero(q->llr, sizeof(float) * q->max_bits);
@@ -475,7 +471,7 @@ int srslte_pdcch_extract_llr_multi(srslte_pdcch_t *q, cf_t *sf_symbols[SRSLTE_MA
           
     /* extract symbols */
     for (int j=0;j<q->nof_rx_antennas;j++) {
-      int n = srslte_regs_pdcch_get(q->regs, sf_symbols[j], q->symbols[j]);
+      int n = srslte_regs_pdcch_get(q->regs, cfi, sf_symbols[j], q->symbols[j]);
       if (nof_symbols != n) {
         fprintf(stderr, "Expected %d PDCCH symbols but got %d symbols\n", nof_symbols, n);
         return ret;
@@ -483,7 +479,7 @@ int srslte_pdcch_extract_llr_multi(srslte_pdcch_t *q, cf_t *sf_symbols[SRSLTE_MA
 
       /* extract channel estimates */
       for (i = 0; i < q->cell.nof_ports; i++) {
-        n = srslte_regs_pdcch_get(q->regs, ce[i][j], q->ce[i][j]);
+        n = srslte_regs_pdcch_get(q->regs, cfi, ce[i][j], q->ce[i][j]);
         if (nof_symbols != n) {
           fprintf(stderr, "Expected %d PDCCH symbols but got %d symbols\n", nof_symbols, n);
           return ret;
@@ -595,13 +591,11 @@ int srslte_pdcch_encode(srslte_pdcch_t *q, srslte_dci_msg_t *msg, srslte_dci_loc
       srslte_dci_location_isvalid(&location))
   {
 
-    set_cfi(q, cfi);
-
     uint32_t e_bits = PDCCH_FORMAT_NOF_BITS(location.L);
     nof_symbols = e_bits/2;
     ret = SRSLTE_ERROR;
     
-    if (location.ncce + PDCCH_FORMAT_NOF_CCE(location.L) <= q->nof_cce && 
+    if (location.ncce + PDCCH_FORMAT_NOF_CCE(location.L) <= NOF_CCE(cfi) && 
         msg->nof_bits < SRSLTE_DCI_MAX_BITS - 16) 
     {      
       DEBUG("Encoding DCI: Nbits: %d, E: %d, nCCE: %d, L: %d, RNTI: 0x%x\n",
@@ -634,14 +628,14 @@ int srslte_pdcch_encode(srslte_pdcch_t *q, srslte_dci_msg_t *msg, srslte_dci_loc
       
       /* mapping to resource elements */
       for (i = 0; i < q->cell.nof_ports; i++) {
-        srslte_regs_pdcch_put_offset(q->regs, q->symbols[i], sf_symbols[i], 
+        srslte_regs_pdcch_put_offset(q->regs, cfi, q->symbols[i], sf_symbols[i],
-                              location.ncce * 9, PDCCH_FORMAT_NOF_REGS(location.L));
+                                     location.ncce * 9, PDCCH_FORMAT_NOF_REGS(location.L));
       }
       
       ret = SRSLTE_SUCCESS;
       
     } else {
-        fprintf(stderr, "Illegal DCI message nCCE: %d, L: %d, nof_cce: %d, nof_bits=%d\n", location.ncce, location.L, q->nof_cce, msg->nof_bits);
+        fprintf(stderr, "Illegal DCI message nCCE: %d, L: %d, nof_cce: %d, nof_bits=%d\n", location.ncce, location.L, NOF_CCE(cfi), msg->nof_bits);
     }
   } else {
     fprintf(stderr, "Invalid parameters: cfi=%d, L=%d, nCCE=%d\n", cfi, location.L, location.ncce);

lib/src/phy/phch/ra.c

@@ -150,7 +150,8 @@ int srslte_ra_ul_dci_to_grant_prb_allocation(srslte_ra_ul_dci_t *dci, srslte_ra_
     // starting prb idx for slot 0 is as given by resource grant
     grant->n_prb[0] = n_prb_1;
     if (n_prb_1 < n_rb_ho/2) {
-      fprintf(stderr, "Invalid Frequency Hopping parameters. Offset: %d, n_prb_1: %d\n", n_rb_ho, n_prb_1);
+      INFO("Invalid Frequency Hopping parameters. Offset: %d, n_prb_1: %d\n", n_rb_ho, n_prb_1);
+      return SRSLTE_ERROR;
     }
     uint32_t n_prb_1_tilde = n_prb_1;
 

lib/src/phy/phch/regs.c

@@ -111,7 +111,7 @@ int regs_pdcch_init(srslte_regs_t *h) {
     }
 
     h->pdcch[cfi].nof_regs = m;
-    
+
     h->pdcch[cfi].regs = malloc(sizeof(srslte_regs_reg_t*) * h->pdcch[cfi].nof_regs);
     if (!h->pdcch[cfi].regs) {
       perror("malloc");
@@ -133,7 +133,7 @@ int regs_pdcch_init(srslte_regs_t *h) {
           if (k < h->cell.id) {
             kp = (h->pdcch[cfi].nof_regs + k-(h->cell.id%h->pdcch[cfi].nof_regs))%h->pdcch[cfi].nof_regs;
           } else {
-            kp = (k-h->cell.id)%h->pdcch[cfi].nof_regs;            
+            kp = (k-h->cell.id)%h->pdcch[cfi].nof_regs;
           }
           h->pdcch[cfi].regs[m] = tmp[kp];
           k++;
@@ -141,7 +141,7 @@ int regs_pdcch_init(srslte_regs_t *h) {
       }
     }
     h->pdcch[cfi].nof_regs = (h->pdcch[cfi].nof_regs/9)*9;
-    INFO("Init PDCCH REG space CFI %d. %d useful REGs (%d CCEs)\n",cfi+1, 
+    INFO("Init PDCCH REG space CFI %d. %d useful REGs (%d CCEs)\n",cfi+1,
          h->pdcch[cfi].nof_regs, h->pdcch[cfi].nof_regs/9);
     free(tmp);
     tmp = NULL;
@@ -168,11 +168,11 @@ int srslte_regs_pdcch_nregs(srslte_regs_t *h, uint32_t cfi) {
 }
 
 int srslte_regs_pdcch_ncce(srslte_regs_t *h, uint32_t cfi) {
-  int nregs = srslte_regs_pdcch_nregs(h, cfi); 
+  int nregs = srslte_regs_pdcch_nregs(h, cfi);
   if (nregs > 0) {
     return (uint32_t) (nregs / 9);
   } else {
-    return SRSLTE_ERROR; 
+    return SRSLTE_ERROR;
   }
 }
 
@@ -180,53 +180,59 @@ int srslte_regs_pdcch_ncce(srslte_regs_t *h, uint32_t cfi) {
  * second part of 6.8.5 in 36.211
  */
 
-int srslte_regs_pdcch_put_offset(srslte_regs_t *h, cf_t *d, cf_t *slot_symbols, uint32_t start_reg, uint32_t nof_regs) {
+int srslte_regs_pdcch_put_offset(srslte_regs_t *h, uint32_t cfi, cf_t *d, cf_t *slot_symbols, uint32_t start_reg, uint32_t nof_regs) {
-  if (h->cfi_initiated) {
+  if (cfi < 1 || cfi > 3) {
-    if (start_reg + nof_regs <= h->pdcch[h->cfi].nof_regs) {
+    fprintf(stderr, "Invalid CFI=%d\n", cfi);
-      uint32_t i, k;
-      k = 0;
-      for (i=start_reg;i<start_reg+nof_regs;i++) {
-        regs_put_reg(h->pdcch[h->cfi].regs[i], &d[k], slot_symbols, h->cell.nof_prb);
-        k += 4;
-      }
-      return k;      
-    } else {
-      fprintf(stderr, "Out of range: start_reg + nof_reg must be lower than %d\n", h->pdcch[h->cfi].nof_regs);
-      return SRSLTE_ERROR;      
-    }       
-  } else {
-    fprintf(stderr, "Must call srslte_regs_set_cfi() first\n");
     return SRSLTE_ERROR;
   }
-}
+  if (start_reg + nof_regs <= h->pdcch[cfi-1].nof_regs) {
-
+    uint32_t i, k;
-int srslte_regs_pdcch_put(srslte_regs_t *h, cf_t *d, cf_t *slot_symbols) {
+    k = 0;
-  return srslte_regs_pdcch_put_offset(h, d, slot_symbols, 0, h->pdcch[h->cfi].nof_regs);
+    for (i=start_reg;i<start_reg+nof_regs;i++) {
-}
+      regs_put_reg(h->pdcch[cfi-1].regs[i], &d[k], slot_symbols, h->cell.nof_prb);
-
+      k += 4;
-int srslte_regs_pdcch_get_offset(srslte_regs_t *h, cf_t *slot_symbols, cf_t *d, uint32_t start_reg, uint32_t nof_regs) {
-  if (h->cfi_initiated) {
-    if (start_reg + nof_regs <= h->pdcch[h->cfi].nof_regs) {
-      uint32_t i, k;  
-      k = 0;
-      for (i=start_reg;i<start_reg + nof_regs;i++) {
-        regs_get_reg(h->pdcch[h->cfi].regs[i], slot_symbols, &d[k], h->cell.nof_prb);
-        k += 4;
-      }
-      return k;    
-    } else {
-      fprintf(stderr, "Out of range: start_reg + nof_reg must be lower than %d\n", h->pdcch[h->cfi].nof_regs);
-      return SRSLTE_ERROR;
     }
+    return k;
   } else {
-    fprintf(stderr, "Must call srslte_regs_set_cfi() first\n");
+    fprintf(stderr, "Out of range: start_reg + nof_reg must be lower than %d\n", h->pdcch[cfi-1].nof_regs);
+    return SRSLTE_ERROR;
+  }
+}
+
+int srslte_regs_pdcch_put(srslte_regs_t *h, uint32_t cfi, cf_t *d, cf_t *slot_symbols) {
+  if (cfi < 1 || cfi > 3) {
+    fprintf(stderr, "Invalid CFI=%d\n", cfi);
+    return SRSLTE_ERROR;
+  }
+  return srslte_regs_pdcch_put_offset(h, cfi, d, slot_symbols, 0, h->pdcch[cfi-1].nof_regs);
+}
+
+int srslte_regs_pdcch_get_offset(srslte_regs_t *h, uint32_t cfi, cf_t *slot_symbols, cf_t *d, uint32_t start_reg, uint32_t nof_regs) {
+  if (cfi < 1 || cfi > 3) {
+    fprintf(stderr, "Invalid CFI=%d\n", cfi);
+    return SRSLTE_ERROR;
+  }
+  if (start_reg + nof_regs <= h->pdcch[cfi-1].nof_regs) {
+    uint32_t i, k;
+    k = 0;
+    for (i=start_reg;i<start_reg + nof_regs;i++) {
+      regs_get_reg(h->pdcch[cfi-1].regs[i], slot_symbols, &d[k], h->cell.nof_prb);
+      k += 4;
+    }
+    return k;
+  } else {
+    fprintf(stderr, "Out of range: start_reg + nof_reg must be lower than %d\n", h->pdcch[cfi-1].nof_regs);
     return SRSLTE_ERROR;
   }
 }
 
 
-int srslte_regs_pdcch_get(srslte_regs_t *h, cf_t *slot_symbols, cf_t *d) {
+int srslte_regs_pdcch_get(srslte_regs_t *h, uint32_t cfi, cf_t *slot_symbols, cf_t *d) {
- return srslte_regs_pdcch_get_offset(h, slot_symbols, d, 0, h->pdcch[h->cfi].nof_regs);
+  if (cfi < 1 || cfi > 3) {
+    fprintf(stderr, "Invalid CFI=%d\n", cfi);
+    return SRSLTE_ERROR;
+  }
+  return srslte_regs_pdcch_get_offset(h, cfi, slot_symbols, d, 0, h->pdcch[cfi-1].nof_regs);
 }
 
 
@@ -668,25 +674,6 @@ void srslte_regs_free(srslte_regs_t *h) {
   bzero(h, sizeof(srslte_regs_t));
 }
 
-/** Sets the CFI value for this subframe (CFI must be in the range 1..3).
- */
-int srslte_regs_set_cfi(srslte_regs_t *h, uint32_t cfi) {  
-  if (cfi > 0 && cfi <= 3) {
-    if (h->phich_len == SRSLTE_PHICH_EXT &&
-        ((h->cell.nof_prb <= 10 && cfi < 2) || (h->cell.nof_prb >= 10 && cfi < 3))) {
-      fprintf(stderr, "PHICH length is extended. The number of control symbols should be at least 3.\n");
-      return SRSLTE_ERROR_INVALID_INPUTS;
-    } else {
-      h->cfi_initiated = true;
-      h->cfi = cfi - 1;
-      return SRSLTE_SUCCESS;
-    }
-  } else {
-    fprintf(stderr, "Invalid CFI %d\n", cfi);
-    return SRSLTE_ERROR_INVALID_INPUTS;
-  }
-}
-
 /**
  * Initializes REGs structure.
  * Sets all REG indices and initializes PCFICH, PHICH and PDCCH REGs
@@ -709,7 +696,6 @@ int srslte_regs_init(srslte_regs_t *h, srslte_cell_t cell) {
     vo = cell.id % 3;
     h->cell = cell;
     h->max_ctrl_symbols = max_ctrl_symbols;
-    h->cfi_initiated = false;
     h->phich_res = cell.phich_resources;
     h->phich_len = cell.phich_length;
 

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

@@ -167,10 +167,6 @@ int base_init() {
     return -1;
   }
 
-  if (srslte_regs_set_cfi(&regs, cfi)) {
-    fprintf(stderr, "Error setting CFI %d\n", cfi);
-    return -1;
-  }
   if (srslte_pdcch_init_ue(&pdcch, cell.nof_prb, 1)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);
@@ -252,7 +248,7 @@ int main(int argc, char **argv) {
     }
 
     for (i=0;i<nof_locations && crc_rem != rnti;i++) {
-      if (srslte_pdcch_decode_msg(&pdcch, &dci_msg, &locations[i], dci_format, &crc_rem)) {
+      if (srslte_pdcch_decode_msg(&pdcch, &dci_msg, &locations[i], dci_format, cfi, &crc_rem)) {
         fprintf(stderr, "Error decoding DCI msg\n");
         return -1;
       }

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

@@ -199,11 +199,6 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-  if (srslte_regs_set_cfi(&regs, cfi)) {
-    fprintf(stderr, "Error setting CFI\n");
-    exit(-1);
-  }
-
   if (srslte_pdcch_init_enb(&pdcch_tx, cell.nof_prb)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);
@@ -297,7 +292,7 @@ int main(int argc, char **argv) {
   /* Decode DCIs */
   for (i=0;i<nof_dcis;i++) {
     uint16_t crc_rem;
-    if (srslte_pdcch_decode_msg(&pdcch_rx, &testcases[i].dci_rx, &testcases[i].dci_location, testcases[i].dci_format, &crc_rem)) {
+    if (srslte_pdcch_decode_msg(&pdcch_rx, &testcases[i].dci_rx, &testcases[i].dci_location, testcases[i].dci_format, cfi, &crc_rem)) {
       fprintf(stderr, "Error decoding DCI message\n");
       goto quit;
     }

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

@@ -175,6 +175,10 @@ int main(int argc, char **argv) {
   }
 
   uint8_t *data[] = {malloc(100000)};
+  if (!data[0]) {
+    perror("malloc");
+    exit(-1);
+  }
 
   ret = -1;
   nof_frames = 0;
@@ -195,7 +199,8 @@ int main(int argc, char **argv) {
   } while (nof_frames <= max_frames && ret == 0);
 
   base_free();
-  free(data[0]);
+  if (data[0])
+    free(data[0]);
   if (ret > 0) {
     exit(0);
   } else {

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

@@ -181,13 +181,17 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-  uint8_t *data[] = {malloc(100000)};
+  uint8_t *data = malloc(100000);
+  if (!data) {
+    perror("malloc");
+    exit(-1);
+  }
 
   ret = -1;
 
   srslte_filesource_read(&fsrc, input_buffer[0], flen);
   INFO("Reading %d samples sub-frame %d\n", flen, sf_idx);
-  ret = srslte_ue_dl_decode_mbsfn(&ue_dl, data[0], sf_idx);
+  ret = srslte_ue_dl_decode_mbsfn(&ue_dl, data, sf_idx);
   if(ret > 0) {
     printf("PMCH Decoded OK!\n");       
   }  else if (ret < 0) {
@@ -195,7 +199,9 @@ int main(int argc, char **argv) {
   }
 
   base_free();
-  free(data[0]);
+  if (data != NULL) {
+    free(data);
+  }
   if (ret > 0) {
     exit(0);
   } else {

lib/src/phy/sync/sync.c

@@ -71,7 +71,7 @@ int srslte_sync_init_decim(srslte_sync_t *q, uint32_t frame_size, uint32_t max_o
     q->N_id_1 = 1000;
 
     q->cfo_ema_alpha = CFO_EMA_ALPHA;
-    q->sss_alg = SSS_FULL;
+    q->sss_alg = SSS_PARTIAL_3;
 
     q->detect_cp       = true;
     q->sss_en          = true;

lib/src/phy/ue/ue_dl.c

@@ -441,12 +441,7 @@ int srslte_ue_dl_decode_estimate_mbsfn(srslte_ue_dl_t *q, uint32_t sf_idx, uint3
 
     INFO("Decoded CFI=%d with correlation %.2f, sf_idx=%d\n", *cfi, cfi_corr, sf_idx);
 
-    if (srslte_regs_set_cfi(&q->regs, *cfi)) {
+    return SRSLTE_SUCCESS;
-      fprintf(stderr, "Error setting CFI\n");
-      return SRSLTE_ERROR;
-    }
-    
-    return SRSLTE_SUCCESS; 
   } else {
     return SRSLTE_ERROR_INVALID_INPUTS; 
   }
@@ -780,7 +775,7 @@ uint32_t srslte_ue_dl_get_ncce(srslte_ue_dl_t *q) {
   return q->last_location.ncce; 
 }
 
-static int dci_blind_search(srslte_ue_dl_t *q, dci_blind_search_t *search_space, uint16_t rnti, srslte_dci_msg_t *dci_msg) 
+static int dci_blind_search(srslte_ue_dl_t *q, dci_blind_search_t *search_space, uint16_t rnti, uint32_t cfi, srslte_dci_msg_t *dci_msg)
 {
   int ret = SRSLTE_ERROR; 
   uint16_t crc_rem = 0; 
@@ -792,7 +787,7 @@ static int dci_blind_search(srslte_ue_dl_t *q, dci_blind_search_t *search_space,
              srslte_dci_format_string(search_space->format), search_space->loc[i].ncce, search_space->loc[i].L, 
              i, search_space->nof_locations);
       
-      if (srslte_pdcch_decode_msg(&q->pdcch, dci_msg, &search_space->loc[i], search_space->format, &crc_rem)) {
+      if (srslte_pdcch_decode_msg(&q->pdcch, dci_msg, &search_space->loc[i], search_space->format, cfi, &crc_rem)) {
         fprintf(stderr, "Error decoding DCI msg\n");
         return SRSLTE_ERROR;
       }
@@ -834,7 +829,8 @@ int srslte_ue_dl_find_ul_dci(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_idx, u
     }
     
     // Configure and run DCI blind search 
-    dci_blind_search_t search_space; 
+    dci_blind_search_t search_space;
+    search_space.nof_locations = 0;
     dci_blind_search_t *current_ss = &search_space;
     if (q->current_rnti == rnti) {
       current_ss = &q->current_ss_ue[cfi-1][sf_idx];
@@ -843,11 +839,9 @@ int srslte_ue_dl_find_ul_dci(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_idx, u
       current_ss->nof_locations = srslte_pdcch_ue_locations(&q->pdcch, current_ss->loc, MAX_CANDIDATES_UE, sf_idx, cfi, rnti);        
     }
     
-    srslte_pdcch_set_cfi(&q->pdcch, cfi);
+   current_ss->format = SRSLTE_DCI_FORMAT0;
-    
-    current_ss->format = SRSLTE_DCI_FORMAT0; 
     INFO("Searching UL C-RNTI in %d ue locations\n", search_space.nof_locations);
-    return dci_blind_search(q, current_ss, rnti, dci_msg);
+    return dci_blind_search(q, current_ss, rnti, cfi, dci_msg);
   } else {
     return 0; 
   }
@@ -880,7 +874,7 @@ static int find_dl_dci_type_siprarnti(srslte_ue_dl_t *q, uint32_t cfi, uint16_t
   if (search_space.nof_locations > 0) {    
     for (int f=0;f<nof_common_formats;f++) {
       search_space.format = common_formats[f];   
-      if ((ret = dci_blind_search(q, &search_space, rnti, dci_msg))) {
+      if ((ret = dci_blind_search(q, &search_space, rnti, cfi, dci_msg))) {
         return ret; 
       }
     }
@@ -907,8 +901,6 @@ static int find_dl_dci_type_crnti(srslte_ue_dl_t *q, uint32_t tm, uint32_t cfi,
     // If locations are not pre-generated, generate them now
     current_ss->nof_locations = srslte_pdcch_ue_locations(&q->pdcch, current_ss->loc, MAX_CANDIDATES_UE, sf_idx, cfi, rnti);        
   }
-  
-  srslte_pdcch_set_cfi(&q->pdcch, cfi);
 
   for (int f = 0; f < 2; f++) {
     srslte_dci_format_t format = ue_dci_formats[tm][f];
@@ -917,8 +909,8 @@ static int find_dl_dci_type_crnti(srslte_ue_dl_t *q, uint32_t tm, uint32_t cfi,
          current_ss->nof_locations);
 
     current_ss->format = format;
-    if ((ret = dci_blind_search(q, current_ss, rnti, dci_msg))) {
+    if ((ret = dci_blind_search(q, current_ss, rnti, cfi, dci_msg))) {
-      return ret; 
+      return ret;
     }
   }
 
@@ -930,13 +922,11 @@ static int find_dl_dci_type_crnti(srslte_ue_dl_t *q, uint32_t tm, uint32_t cfi,
     current_ss->nof_locations = srslte_pdcch_common_locations(&q->pdcch, current_ss->loc, MAX_CANDIDATES_COM, cfi);
   }
   
-  srslte_pdcch_set_cfi(&q->pdcch, cfi);
+  // Search for RNTI only if there is room for the common search space
-  
-  // Search for RNTI only if there is room for the common search space 
   if (current_ss->nof_locations > 0) {    
     current_ss->format = SRSLTE_DCI_FORMAT1A; 
     INFO("Searching DL C-RNTI in %d ue locations, format 1A\n", current_ss->nof_locations);
-    return dci_blind_search(q, current_ss, rnti, dci_msg);   
+    return dci_blind_search(q, current_ss, rnti, cfi, dci_msg);
   }
   return SRSLTE_SUCCESS; 
 }
@@ -988,11 +978,11 @@ void srslte_ue_dl_save_signal(srslte_ue_dl_t *q, srslte_softbuffer_rx_t *softbuf
   srslte_vec_save_file("pcfich_eq_symbols", q->pcfich.d, q->pcfich.nof_symbols*sizeof(cf_t));
   srslte_vec_save_file("pcfich_llr", q->pcfich.data_f, PCFICH_CFI_LEN*sizeof(float));
   
-  srslte_vec_save_file("pdcch_ce0", q->pdcch.ce[0], q->pdcch.nof_cce*36*sizeof(cf_t));
+  srslte_vec_save_file("pdcch_ce0", q->pdcch.ce[0], q->pdcch.nof_cce[cfi-1]*36*sizeof(cf_t));
-  srslte_vec_save_file("pdcch_ce1", q->pdcch.ce[1], q->pdcch.nof_cce*36*sizeof(cf_t));
+  srslte_vec_save_file("pdcch_ce1", q->pdcch.ce[1], q->pdcch.nof_cce[cfi-1]*36*sizeof(cf_t));
-  srslte_vec_save_file("pdcch_symbols", q->pdcch.symbols[0], q->pdcch.nof_cce*36*sizeof(cf_t));
+  srslte_vec_save_file("pdcch_symbols", q->pdcch.symbols[0], q->pdcch.nof_cce[cfi-1]*36*sizeof(cf_t));
-  srslte_vec_save_file("pdcch_eq_symbols", q->pdcch.d, q->pdcch.nof_cce*36*sizeof(cf_t));
+  srslte_vec_save_file("pdcch_eq_symbols", q->pdcch.d, q->pdcch.nof_cce[cfi-1]*36*sizeof(cf_t));
-  srslte_vec_save_file("pdcch_llr", q->pdcch.llr, q->pdcch.nof_cce*72*sizeof(float));
+  srslte_vec_save_file("pdcch_llr", q->pdcch.llr, q->pdcch.nof_cce[cfi-1]*72*sizeof(float));
 
   
   srslte_vec_save_file("pdsch_symbols", q->pdsch.d[0], q->pdsch_cfg.nbits[0].nof_re*sizeof(cf_t));

lib/src/phy/ue/ue_ul.c

@@ -626,13 +626,12 @@ int srslte_ue_ul_sr_send_tti(uint32_t I_sr, uint32_t current_tti) {
   } else {
     return SRSLTE_ERROR;
   }
-  uint32_t sfn = current_tti/10;
+  if (current_tti >= sr_N_offset) {
-  uint32_t subf = current_tti%10; 
+    if ((current_tti - sr_N_offset) % sr_periodicity == 0) {
-  if ((10*sfn+subf-sr_N_offset)%sr_periodicity==0) {
+      return 1;
-    return 1; 
+    }
-  } else {
-    return SRSLTE_SUCCESS;
   }
+  return SRSLTE_SUCCESS;
 }
 
 

lib/src/phy/utils/ringbuffer.c

@@ -11,7 +11,7 @@ int srslte_ringbuffer_init(srslte_ringbuffer_t *q, int capacity)
   if (!q->buffer) {
     return -1; 
   }
-
+  q->active     = true;
   q->capacity   = capacity;
   srslte_ringbuffer_reset(q);
   
@@ -24,6 +24,7 @@ int srslte_ringbuffer_init(srslte_ringbuffer_t *q, int capacity)
 void srslte_ringbuffer_free(srslte_ringbuffer_t *q)
 {
   if (q) {
+    srslte_ringbuffer_stop(q);
     if (q->buffer) {
       free(q->buffer);
       q->buffer = NULL; 
@@ -52,6 +53,9 @@ int srslte_ringbuffer_write(srslte_ringbuffer_t *q, void *p, int nof_bytes)
   uint8_t *ptr = (uint8_t*) p;
   int w_bytes = nof_bytes;
   pthread_mutex_lock(&q->mutex);
+  if (!q->active) {
+    return 0;
+  }
   if (q->count + w_bytes > q->capacity) {
     w_bytes = q->capacity - q->count; 
     fprintf(stderr, "Buffer overrun: lost %d bytes\n", nof_bytes - w_bytes);
@@ -77,9 +81,12 @@ int srslte_ringbuffer_read(srslte_ringbuffer_t *q, void *p, int nof_bytes)
 {
   uint8_t *ptr = (uint8_t*) p;
   pthread_mutex_lock(&q->mutex);
-  while(q->count < nof_bytes) {
+  while(q->count < nof_bytes && q->active) {
     pthread_cond_wait(&q->cvar, &q->mutex);
   }
+  if (!q->active) {
+    return 0;
+  }
   if (nof_bytes + q->rpm > q->capacity) {
     int x = q->capacity - q->rpm; 
     memcpy(ptr, &q->buffer[q->rpm], x);
@@ -96,5 +103,9 @@ int srslte_ringbuffer_read(srslte_ringbuffer_t *q, void *p, int nof_bytes)
   return nof_bytes; 
 }
 
-
+void srslte_ringbuffer_stop(srslte_ringbuffer_t *q) {
+  pthread_mutex_lock(&q->mutex);
+  pthread_cond_broadcast(&q->cvar);
+  pthread_mutex_unlock(&q->mutex);
+}
 

lib/src/radio/radio.cc

@@ -313,6 +313,11 @@ double radio::get_rx_freq()
   return rx_freq;
 }
 
+double radio::get_freq_offset()
+{
+  return freq_offset;
+}
+
 double radio::get_tx_freq()
 {
   return tx_freq; 

lib/src/upper/rlc_am.cc

@@ -31,8 +31,8 @@
 #include <sstream>
 
 #define MOD 1024
-#define RX_MOD_BASE(x) (x-vr_r)%1024
+#define RX_MOD_BASE(x) ((x-vr_r)%1024)
-#define TX_MOD_BASE(x) (x-vt_a)%1024
+#define TX_MOD_BASE(x) ((x-vt_a)%1024)
 
 namespace srslte {
 
@@ -312,11 +312,29 @@ int rlc_am::read_pdu(uint8_t *payload, uint32_t nof_bytes)
     pthread_mutex_unlock(&mutex);
     return build_status_pdu(payload, nof_bytes);
   }
+
+  // if tx_window is full and retx_queue empty, retransmit next PDU to be ack'ed
+  if (tx_window.size() >= RLC_AM_WINDOW_SIZE && retx_queue.size() == 0) {
+    if (tx_window[vt_a].buf != NULL) {
+      log->warning("Full Tx window, ReTx'ing first outstanding PDU\n");
+      rlc_amd_retx_t retx;
+      retx.is_segment = false;
+      retx.so_start   = 0;
+      retx.so_end     = tx_window[vt_a].buf->N_bytes;
+      retx.sn         = vt_a;
+      retx_queue.push_back(retx);
+    } else {
+      log->error("Found invalid PDU in tx_window.\n");
+    }
+  }
+
   // RETX if required
   if(retx_queue.size() > 0) {
     int ret = build_retx_pdu(payload, nof_bytes);
-    pthread_mutex_unlock(&mutex);
+    if (ret > 0) {
-    return ret; 
+      pthread_mutex_unlock(&mutex);
+      return ret;
+    }
   }
 
   // Build a PDU from SDUs
@@ -471,8 +489,8 @@ int  rlc_am::build_retx_pdu(uint8_t *payload, uint32_t nof_bytes)
     if (!retx_queue.empty()) {
       retx = retx_queue.front();
     } else {
-      log->error("In build_retx_pdu(): retx_queue is empty during sanity check\n");
+      log->info("In build_retx_pdu(): retx_queue is empty during sanity check, sn=%d\n", retx.sn);
-      return -1;
+      return 0;
     }
   }
 
@@ -491,6 +509,12 @@ int  rlc_am::build_retx_pdu(uint8_t *payload, uint32_t nof_bytes)
   // Update & write header
   rlc_amd_pdu_header_t new_header = tx_window[retx.sn].header;
   new_header.p = 0;
+
+  // Set poll bit
+  pdu_without_poll++;
+  byte_without_poll += (tx_window[retx.sn].buf->N_bytes + rlc_am_packed_length(&new_header));
+  log->info("%s pdu_without_poll: %d\n", rrc->get_rb_name(lcid).c_str(), pdu_without_poll);
+  log->info("%s byte_without_poll: %d\n", rrc->get_rb_name(lcid).c_str(), byte_without_poll);
   if(poll_required())
   {
     new_header.p      = 1;
@@ -530,14 +554,28 @@ int rlc_am::build_segment(uint8_t *payload, uint32_t nof_bytes, rlc_amd_retx_t r
   rlc_amd_pdu_header_t new_header;
   rlc_amd_pdu_header_t old_header = tx_window[retx.sn].header;
 
+  pdu_without_poll++;
+  byte_without_poll += (tx_window[retx.sn].buf->N_bytes + rlc_am_packed_length(&new_header));
+  log->info("%s pdu_without_poll: %d\n", rrc->get_rb_name(lcid).c_str(), pdu_without_poll);
+  log->info("%s byte_without_poll: %d\n", rrc->get_rb_name(lcid).c_str(), byte_without_poll);
+
   new_header.dc   = RLC_DC_FIELD_DATA_PDU;
   new_header.rf   = 1;
-  new_header.p    = 0;
   new_header.fi   = RLC_FI_FIELD_NOT_START_OR_END_ALIGNED;
   new_header.sn   = old_header.sn;
   new_header.lsf  = 0;
   new_header.so   = retx.so_start;
   new_header.N_li = 0;
+  new_header.p    = 0;
+  if(poll_required())
+  {
+    log->debug("%s setting poll bit to request status\n", rrc->get_rb_name(lcid).c_str());
+    new_header.p      = 1;
+    poll_sn           = vt_s;
+    pdu_without_poll  = 0;
+    byte_without_poll = 0;
+    poll_retx_timeout.start(cfg.t_poll_retx);
+  }
 
   uint32_t head_len  = 0;
   uint32_t pdu_space = 0;
@@ -549,7 +587,7 @@ int rlc_am::build_segment(uint8_t *payload, uint32_t nof_bytes, rlc_amd_retx_t r
                  rrc->get_rb_name(lcid).c_str(), nof_bytes, head_len);
     return 0;
   }
-  pdu_space = nof_bytes-head_len;
+  pdu_space = nof_bytes-head_len-2;
   if(pdu_space < (retx.so_end-retx.so_start))
     retx.so_end = retx.so_start+pdu_space;
 
@@ -568,7 +606,7 @@ int rlc_am::build_segment(uint8_t *payload, uint32_t nof_bytes, rlc_amd_retx_t r
     upper += old_header.li[i];
 
     head_len    = rlc_am_packed_length(&new_header);
-    pdu_space   = nof_bytes-head_len;
+    pdu_space   = nof_bytes-head_len-2;
     if(pdu_space < (retx.so_end-retx.so_start))
       retx.so_end = retx.so_start+pdu_space;
 
@@ -634,8 +672,15 @@ int  rlc_am::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
     return 0;
   }
 
+  // do not build any more PDU if window is already full
+  if (!tx_sdu && tx_window.size() >= RLC_AM_WINDOW_SIZE) {
+    log->info("Tx window full.\n");
+    return 0;
+  }
+
   byte_buffer_t *pdu = pool_allocate;
   if (!pdu) {
+#ifdef RLC_AM_BUFFER_DEBUG
     log->console("Fatal Error: Could not allocate PDU in build_data_pdu()\n");
     log->console("tx_window size: %d PDUs\n", tx_window.size());
     log->console("vt_a = %d, vt_ms = %d, vt_s = %d, poll_sn = %d "
@@ -648,6 +693,10 @@ int  rlc_am::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
       log->console("tx_window - SN: %d\n", txit->first);
     }
     exit(-1);
+#else
+    log->error("Fatal Error: Couldn't allocate PDU in build_data_pdu().\n");
+    return 0;
+#endif
   }
   rlc_amd_pdu_header_t header;
   header.dc   = RLC_DC_FIELD_DATA_PDU;
@@ -806,8 +855,13 @@ void rlc_am::handle_data_pdu(uint8_t *payload, uint32_t nof_bytes, rlc_amd_pdu_h
   rlc_amd_rx_pdu_t pdu;
   pdu.buf = pool_allocate;
   if (!pdu.buf) {
-    log->console("Fatal Error: Could not allocate PDU in handle_data_pdu()\n");
+#ifdef RLC_AM_BUFFER_DEBUG
+    log->console("Fatal Error: Couldn't allocate PDU in handle_data_pdu().\n");
     exit(-1);
+#else
+    log->error("Fatal Error: Couldn't allocate PDU in handle_data_pdu().\n");
+    return;
+#endif
   }
 
   memcpy(pdu.buf->msg, payload, nof_bytes);
@@ -849,12 +903,7 @@ void rlc_am::handle_data_pdu(uint8_t *payload, uint32_t nof_bytes, rlc_amd_pdu_h
   // Update reordering variables and timers (36.322 v10.0.0 Section 5.1.3.2.3)
   if(reordering_timeout.is_running())
   {
-    if(
+    if(vr_x == vr_r || (!inside_rx_window(vr_x) && vr_x != vr_mr))
-       vr_x == vr_r ||
-       (RX_MOD_BASE(vr_x) < RX_MOD_BASE(vr_r)  ||
-        (RX_MOD_BASE(vr_x) > RX_MOD_BASE(vr_mr) &&
-        vr_x != vr_mr))
-       )
     {
       reordering_timeout.reset();
     }
@@ -892,9 +941,15 @@ void rlc_am::handle_data_pdu_segment(uint8_t *payload, uint32_t nof_bytes, rlc_a
   rlc_amd_rx_pdu_t segment;
   segment.buf = pool_allocate;
   if (!segment.buf) {
-    log->console("Fatal Error: Could not allocate PDU in handle_data_pdu_segment()\n");
+#ifdef RLC_AM_BUFFER_DEBUG
+    log->console("Fatal Error: Couldn't allocate PDU in handle_data_pdu_segment().\n");
     exit(-1);
+#else
+    log->error("Fatal Error: Couldn't allocate PDU in handle_data_pdu_segment().\n");
+    return;
+#endif
   }
+
   memcpy(segment.buf->msg, payload, nof_bytes);
   segment.buf->N_bytes = nof_bytes;
   memcpy(&segment.header, &header, sizeof(rlc_amd_pdu_header_t));
@@ -946,7 +1001,9 @@ void rlc_am::handle_data_pdu_segment(uint8_t *payload, uint32_t nof_bytes, rlc_a
       // else delay for reordering timer
     }
   }
-
+#ifdef RLC_AM_BUFFER_DEBUG
+  print_rx_segments();
+#endif
   debug_state();
 }
 
@@ -961,6 +1018,11 @@ void rlc_am::handle_control_pdu(uint8_t *payload, uint32_t nof_bytes)
 
   poll_retx_timeout.reset();
 
+  // flush retx queue to avoid unordered SNs, we expect the Rx to request lost PDUs again
+  if (status.N_nack > 0) {
+    retx_queue.clear();
+  }
+
   // Handle ACKs and NACKs
   std::map<uint32_t, rlc_amd_tx_pdu_t>::iterator it;
   bool update_vt_a = true;
@@ -984,15 +1046,26 @@ void rlc_am::handle_control_pdu(uint8_t *payload, uint32_t nof_bytes)
             retx.so_end     = it->second.buf->N_bytes;
 
             if(status.nacks[j].has_so) {
+              // sanity check
+              if (status.nacks[j].so_start >= it->second.buf->N_bytes) {
+                // print error but try to send original PDU again
+                log->error("SO_start is larger than original PDU (%d >= %d)\n",
+                           status.nacks[j].so_start,
+                           it->second.buf->N_bytes);
+                status.nacks[j].so_start = 0;
+              }
+
+              // check for special SO_end value
+              if(status.nacks[j].so_end == 0x7FFF) {
+                status.nacks[j].so_end = it->second.buf->N_bytes;
+              }else{
+                retx.so_end = status.nacks[j].so_end + 1;
+              }
+
               if(status.nacks[j].so_start <  it->second.buf->N_bytes &&
                  status.nacks[j].so_end   <= it->second.buf->N_bytes) {
                   retx.is_segment = true;
                   retx.so_start = status.nacks[j].so_start;
-                  if(status.nacks[j].so_end == 0x7FFF) {
-                    retx.so_end = it->second.buf->N_bytes;
-                  }else{
-                    retx.so_end   = status.nacks[j].so_end + 1;
-                  }
               } else {
                 log->warning("%s invalid segment NACK received for SN %d. so_start: %d, so_end: %d, N_bytes: %d\n",
                              rrc->get_rb_name(lcid).c_str(), i, status.nacks[j].so_start, status.nacks[j].so_end, it->second.buf->N_bytes);
@@ -1010,16 +1083,16 @@ void rlc_am::handle_control_pdu(uint8_t *payload, uint32_t nof_bytes)
       //ACKed SNs get marked and removed from tx_window if possible
       if(tx_window.count(i) > 0) {
         it = tx_window.find(i);
-        it->second.is_acked = true;
+        if (it != tx_window.end()) {
-        if(it->second.buf) {
+          if(update_vt_a) {
-          pool->deallocate(it->second.buf);
+            tx_window.erase(it);
-          it->second.buf = 0;
+            if(it->second.buf) {
-        }
+              pool->deallocate(it->second.buf);
-        if(update_vt_a)
+              it->second.buf = 0;
-        {
+            }
-          tx_window.erase(it);
+            vt_a = (vt_a + 1)%MOD;
-          vt_a = (vt_a + 1)%MOD;
+            vt_ms = (vt_ms + 1)%MOD;
-          vt_ms = (vt_ms + 1)%MOD;
+          }
         }
       }
     }
@@ -1034,8 +1107,13 @@ void rlc_am::reassemble_rx_sdus()
   if(!rx_sdu) {
     rx_sdu = pool_allocate;
     if (!rx_sdu) {
+#ifdef RLC_AM_BUFFER_DEBUG
       log->console("Fatal Error: Could not allocate PDU in reassemble_rx_sdus() (1)\n");
       exit(-1);
+#else
+      log->error("Fatal Error: Could not allocate PDU in reassemble_rx_sdus() (1)\n");
+      return;
+#endif
     }
   }
   // Iterate through rx_window, assembling and delivering SDUs
@@ -1054,10 +1132,14 @@ void rlc_am::reassemble_rx_sdus()
       pdcp->write_pdu(lcid, rx_sdu);
       rx_sdu = pool_allocate;
       if (!rx_sdu) {
+#ifdef RLC_AM_BUFFER_DEBUG
         log->console("Fatal Error: Could not allocate PDU in reassemble_rx_sdus() (2)\n");
-      exit(-1);
+        exit(-1);
+#else
+        log->error("Fatal Error: Could not allocate PDU in reassemble_rx_sdus() (2)\n");
+        return;
+#endif
       }
-
     }
 
     // Handle last segment
@@ -1070,8 +1152,13 @@ void rlc_am::reassemble_rx_sdus()
       pdcp->write_pdu(lcid, rx_sdu);
       rx_sdu = pool_allocate;
       if (!rx_sdu) {
+#ifdef RLC_AM_BUFFER_DEBUG
         log->console("Fatal Error: Could not allocate PDU in reassemble_rx_sdus() (3)\n");
-      exit(-1);
+        exit(-1);
+#else
+        log->error("Fatal Error: Could not allocate PDU in reassemble_rx_sdus() (3)\n");
+        return;
+#endif
       }
     }
 
@@ -1114,6 +1201,20 @@ void rlc_am::debug_state()
 
 }
 
+void rlc_am::print_rx_segments()
+{
+  std::map<uint32_t, rlc_amd_rx_pdu_segments_t>::iterator it;
+  std::stringstream ss;
+  ss << "rx_segments:" << std::endl;
+  for(it=rx_segments.begin();it!=rx_segments.end();it++) {
+    std::list<rlc_amd_rx_pdu_t>::iterator segit;
+    for(segit = it->second.segments.begin(); segit != it->second.segments.end(); segit++) {
+      ss << "    SN:" << segit->header.sn << " SO:" << segit->header.so << " N:" << segit->buf->N_bytes << std::endl;
+    }
+  }
+  log->debug("%s\n", ss.str().c_str());
+}
+
 bool rlc_am::add_segment_and_check(rlc_amd_rx_pdu_segments_t *pdu, rlc_amd_rx_pdu_t *segment)
 {
   // Ordered insert
@@ -1172,8 +1273,13 @@ bool rlc_am::add_segment_and_check(rlc_amd_rx_pdu_segments_t *pdu, rlc_amd_rx_pd
   // Copy data
   byte_buffer_t *full_pdu = pool_allocate;
   if (!full_pdu) {
+#ifdef RLC_AM_BUFFER_DEBUG
     log->console("Fatal Error: Could not allocate PDU in add_segment_and_check()\n");
     exit(-1);
+#else
+    log->error("Fatal Error: Could not allocate PDU in add_segment_and_check()\n");
+    return false;
+#endif
   }
   for(it = pdu->segments.begin(); it != pdu->segments.end(); it++) {
     memcpy(&full_pdu->msg[full_pdu->N_bytes], it->buf->msg, it->buf->N_bytes);

lib/test/common/logger_test.cc

@@ -82,6 +82,7 @@ bool read(std::string filename) {
         written[thread][msg] = true;
       } else {
         perror("Wrong thread and/or msg");
+        fclose(f);
         return false;
       }
     }

lib/test/upper/CMakeLists.txt

@@ -30,6 +30,9 @@ add_executable(rlc_am_test rlc_am_test.cc)
 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)
+
 add_executable(rlc_um_data_test rlc_um_data_test.cc)
 target_link_libraries(rlc_um_data_test srslte_upper srslte_phy srslte_common)
 add_test(rlc_um_data_test rlc_um_data_test)

lib/test/upper/rlc_am_stress_test.cc

@@ -0,0 +1,250 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsUE library.
+ *
+ * srsUE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsUE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <iostream>
+#include <stdlib.h>
+#include <pthread.h>
+#include "srslte/common/log_filter.h"
+#include "srslte/common/logger_stdout.h"
+#include "srslte/common/threads.h"
+#include "srslte/upper/rlc.h"
+#include <assert.h>
+#define NBUFS 5
+
+using namespace srsue;
+using namespace srslte;
+
+class mac_reader
+    :public thread
+{
+public:
+  mac_reader(rlc_interface_mac *rlc1_, rlc_interface_mac *rlc2_, float fail_rate_)
+  {
+    rlc1 = rlc1_;
+    rlc2 = rlc2_;
+    fail_rate = fail_rate_;
+    run_enable = true;
+  }
+
+  void stop()
+  {
+    run_enable = false;
+    int cnt=0;
+    while(running && cnt<100) {
+      usleep(10000);
+      cnt++;
+    }
+    if(running) {
+      thread_cancel();
+    }
+    wait_thread_finish();
+  }
+
+private:
+  void run_thread()
+  {
+    running = true;
+    byte_buffer_t *pdu = byte_buffer_pool::get_instance()->allocate("mac_reader::run_thread");
+    if (!pdu) {
+      printf("Fatal Error: Could not allocate PDU in mac_reader::run_thread\n");
+      exit(-1);
+    }
+
+    while(run_enable) {
+      float r = (float)rand()/RAND_MAX;
+      int opp_size = r*1500;
+      rlc1->get_buffer_state(1);
+      int read = rlc1->read_pdu(1, pdu->msg, opp_size);
+      if(((float)rand()/RAND_MAX > fail_rate) && read>0) {
+        rlc2->write_pdu(1, pdu->msg, opp_size);
+      }
+      usleep(1000);
+    }
+    running = false;
+  }
+
+  rlc_interface_mac *rlc1;
+  rlc_interface_mac *rlc2;
+  float fail_rate;
+
+  bool run_enable;
+  bool running;
+};
+
+class mac_dummy
+    :public srslte::mac_interface_timers
+{
+public:
+  mac_dummy(rlc_interface_mac *rlc1_, rlc_interface_mac *rlc2_, float fail_rate_)
+    :r1(rlc1_, rlc2_, fail_rate_)
+    ,r2(rlc2_, rlc1_, fail_rate_)
+  {
+  }
+
+  void start()
+  {
+    r1.start(7);
+    r2.start(7);
+  }
+
+  void stop()
+  {
+    r1.stop();
+    r2.stop();
+  }
+
+  srslte::timers::timer* timer_get(uint32_t timer_id)
+  {
+    return &t;
+  }
+  uint32_t timer_get_unique_id(){return 0;}
+  void timer_release_id(uint32_t id){}
+
+private:
+  srslte::timers::timer t;
+
+  mac_reader r1;
+  mac_reader r2;
+};
+
+
+
+class rlc_am_tester
+    :public pdcp_interface_rlc
+    ,public rrc_interface_rlc
+    ,public thread
+{
+public:
+  rlc_am_tester(rlc_interface_pdcp *rlc_){
+    rlc = rlc_;
+    run_enable = true;
+  }
+
+  void stop()
+  {
+    run_enable = false;
+    int cnt=0;
+    while(running && cnt<100) {
+      usleep(10000);
+      cnt++;
+    }
+    if(running) {
+      thread_cancel();
+    }
+    wait_thread_finish();
+  }
+
+  // PDCP interface
+  void write_pdu(uint32_t lcid, byte_buffer_t *sdu)
+  {
+    assert(lcid == 1);
+    byte_buffer_pool::get_instance()->deallocate(sdu);
+  }
+  void write_pdu_bcch_bch(byte_buffer_t *sdu) {}
+  void write_pdu_bcch_dlsch(byte_buffer_t *sdu) {}
+  void write_pdu_pcch(byte_buffer_t *sdu) {}
+  
+  // RRC interface
+  void max_retx_attempted(){}
+  std::string get_rb_name(uint32_t lcid) { return std::string(""); }
+
+private:
+  void run_thread()
+  {
+    uint8_t sn = 0;
+    running = true;
+    while(run_enable) {
+      byte_buffer_t *pdu = byte_buffer_pool::get_instance()->allocate("rlc_am_tester::run_thread");
+      if (!pdu) {
+        printf("Fatal Error: Could not allocate PDU in rlc_am_tester::run_thread\n");
+        exit(-1);
+      }
+      pdu->N_bytes = 1500;
+      pdu->msg[0]   = sn++;
+      rlc->write_sdu(1, pdu);
+      usleep(1000);
+    }
+    running = false;
+  }
+
+  bool run_enable;
+  bool running;
+
+  rlc_interface_pdcp *rlc;
+};
+
+void stress_test()
+{
+  srslte::log_filter log1("RLC_AM_1");
+  srslte::log_filter log2("RLC_AM_2");
+  log1.set_level(srslte::LOG_LEVEL_DEBUG);
+  log2.set_level(srslte::LOG_LEVEL_DEBUG);
+  log1.set_hex_limit(-1);
+  log2.set_hex_limit(-1);
+
+  float fail_rate = 0.1;
+
+  rlc rlc1;
+  rlc rlc2;
+
+  rlc_am_tester tester1(&rlc1);
+  rlc_am_tester tester2(&rlc2);
+  mac_dummy     mac(&rlc1, &rlc2, fail_rate);
+  ue_interface  ue;
+
+  rlc1.init(&tester1, &tester1, &ue, &log1, &mac, 0);
+  rlc2.init(&tester1, &tester1, &ue, &log2, &mac, 0);
+
+  LIBLTE_RRC_RLC_CONFIG_STRUCT cnfg;
+  cnfg.rlc_mode = LIBLTE_RRC_RLC_MODE_AM;
+  cnfg.dl_am_rlc.t_reordering = LIBLTE_RRC_T_REORDERING_MS5;
+  cnfg.dl_am_rlc.t_status_prohibit = LIBLTE_RRC_T_STATUS_PROHIBIT_MS5;
+  cnfg.ul_am_rlc.max_retx_thresh = LIBLTE_RRC_MAX_RETX_THRESHOLD_T4;
+  cnfg.ul_am_rlc.poll_byte = LIBLTE_RRC_POLL_BYTE_KB25;
+  cnfg.ul_am_rlc.poll_pdu = LIBLTE_RRC_POLL_PDU_P4;
+  cnfg.ul_am_rlc.t_poll_retx = LIBLTE_RRC_T_POLL_RETRANSMIT_MS5;
+
+  srslte_rlc_config_t cnfg_(&cnfg);
+
+  rlc1.add_bearer(1, cnfg_);
+  rlc2.add_bearer(1, cnfg_);
+
+  tester1.start(7);
+  //tester2.start(7);
+  mac.start();
+
+  usleep(100e6);
+
+  tester1.stop();
+  tester2.stop();
+  mac.stop();
+}
+
+
+int main(int argc, char **argv) {
+  stress_test();
+  byte_buffer_pool::get_instance()->cleanup();
+}

lib/test/upper/rlc_am_test.cc

@@ -482,7 +482,7 @@ void resegment_test_1()
 
   // Read the retx PDU from RLC1 and force resegmentation
   byte_buffer_t retx1;
-  len = rlc1.read_pdu(retx1.msg, 9); // 4 byte header + 5 data
+  len = rlc1.read_pdu(retx1.msg, 11); // 4 byte header + 5 data
   retx1.N_bytes = len;
 
   // Write the retx PDU to RLC2
@@ -492,7 +492,7 @@ void resegment_test_1()
 
   // Read the remaining segment
   byte_buffer_t retx2;
-  len = rlc1.read_pdu(retx2.msg, 9); // 4 byte header + 5 data
+  len = rlc1.read_pdu(retx2.msg, 11); // 4 byte header + 5 data
   retx2.N_bytes = len;
 
   // Write the retx PDU to RLC2
@@ -591,7 +591,7 @@ void resegment_test_2()
 
   // Read the retx PDU from RLC1 and force resegmentation
   byte_buffer_t retx1;
-  retx1.N_bytes = rlc1.read_pdu(retx1.msg, 16); // 6 byte header + 10 data
+  retx1.N_bytes = rlc1.read_pdu(retx1.msg, 18); // 6 byte header + 10 data
 
   // Write the retx PDU to RLC2
   rlc2.write_pdu(retx1.msg, retx1.N_bytes);
@@ -600,7 +600,7 @@ void resegment_test_2()
 
   // Read the remaining segment
   byte_buffer_t retx2;
-  retx2.N_bytes = rlc1.read_pdu(retx2.msg, 16); // 6 byte header + 10 data
+  retx2.N_bytes = rlc1.read_pdu(retx2.msg, 18); // 6 byte header + 10 data
 
   // Write the retx PDU to RLC2
   rlc2.write_pdu(retx2.msg, retx2.N_bytes);
@@ -696,14 +696,14 @@ void resegment_test_3()
 
   // Read the retx PDU from RLC1 and force resegmentation
   byte_buffer_t retx1;
-  retx1.N_bytes = rlc1.read_pdu(retx1.msg, 14); // 4 byte header + 10 data
+  retx1.N_bytes = rlc1.read_pdu(retx1.msg, 16); // 4 byte header + 10 data
 
   // Write the retx PDU to RLC2
   rlc2.write_pdu(retx1.msg, retx1.N_bytes);
 
   // Read the remaining segment
   byte_buffer_t retx2;
-  retx2.N_bytes = rlc1.read_pdu(retx2.msg, 14); // 4 byte header + 10 data
+  retx2.N_bytes = rlc1.read_pdu(retx2.msg, 16); // 4 byte header + 10 data
 
   // Write the retx PDU to RLC2
   rlc2.write_pdu(retx2.msg, retx2.N_bytes);
@@ -799,14 +799,14 @@ void resegment_test_4()
 
   // Read the retx PDU from RLC1 and force resegmentation
   byte_buffer_t retx1;
-  retx1.N_bytes = rlc1.read_pdu(retx1.msg, 21); // 6 byte header + 15 data
+  retx1.N_bytes = rlc1.read_pdu(retx1.msg, 23); // 6 byte header + 15 data
 
   // Write the retx PDU to RLC2
   rlc2.write_pdu(retx1.msg, retx1.N_bytes);
 
   // Read the remaining segment
   byte_buffer_t retx2;
-  retx2.N_bytes = rlc1.read_pdu(retx2.msg, 21); // 6 byte header + 15 data
+  retx2.N_bytes = rlc1.read_pdu(retx2.msg, 23); // 6 byte header + 15 data
 
   // Write the retx PDU to RLC2
   rlc2.write_pdu(retx2.msg, retx2.N_bytes);
@@ -902,14 +902,14 @@ void resegment_test_5()
 
   // Read the retx PDU from RLC1 and force resegmentation
   byte_buffer_t retx1;
-  retx1.N_bytes = rlc1.read_pdu(retx1.msg, 27); // 7 byte header + 20 data
+  retx1.N_bytes = rlc1.read_pdu(retx1.msg, 29); // 7 byte header + 20 data
 
   // Write the retx PDU to RLC2
   rlc2.write_pdu(retx1.msg, retx1.N_bytes);
 
   // Read the remaining segment
   byte_buffer_t retx2;
-  retx2.N_bytes = rlc1.read_pdu(retx2.msg, 27); // 7 byte header + 20 data
+  retx2.N_bytes = rlc1.read_pdu(retx2.msg, 29); // 7 byte header + 20 data
 
   // Write the retx PDU to RLC2
   rlc2.write_pdu(retx2.msg, retx2.N_bytes);
@@ -1017,7 +1017,7 @@ void resegment_test_6()
 
   // Read the retx PDU from RLC1 and force resegmentation
   byte_buffer_t retx1;
-  len = rlc1.read_pdu(retx1.msg, 127);
+  len = rlc1.read_pdu(retx1.msg, 129);
   retx1.N_bytes = len;
 
   // Write the retx PDU to RLC2
@@ -1027,7 +1027,7 @@ void resegment_test_6()
 
   // Read the remaining segment
   byte_buffer_t retx2;
-  len = rlc1.read_pdu(retx2.msg, 157);
+  len = rlc1.read_pdu(retx2.msg, 159);
   retx2.N_bytes = len;
 
   // Write the retx PDU to RLC2

srsenb/src/mac/scheduler.cc

@@ -697,7 +697,7 @@ int sched::dl_sched_data(dl_sched_data_t data[MAX_DATA_LIST])
         for(uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
           h->reset(tb);
         }
-        Warning("SCHED: Could not schedule DL DCI for rnti=0x%x, pid=%d\n", rnti, h->get_id());              
+        Warning("SCHED: Could not schedule DL DCI for rnti=0x%x, pid=%d, cfi=%d\n", rnti, h->get_id(), current_cfi);
       }      
     }    
   } 
@@ -857,13 +857,14 @@ int sched::ul_sched(uint32_t tti, srsenb::sched_interface::ul_sched_res_t* sched
       if (needs_pdcch) {
         uint32_t aggr_level = user->get_aggr_level(srslte_dci_format_sizeof(SRSLTE_DCI_FORMAT0, cfg.cell.nof_prb, cfg.cell.nof_ports));
         if (!generate_dci(&sched_result->pusch[nof_dci_elems].dci_location, 
-            user->get_locations(current_cfi, sf_idx), 
+            user->get_locations(current_cfi, sf_idx),
             aggr_level)) 
         {
           h->reset(0);
-          log_h->warning("SCHED: Could not schedule UL DCI rnti=0x%x, pid=%d, L=%d\n", 
+          printf("SCHED: Could not schedule UL DCI rnti=0x%x, pid=%d, L=%d, sf_idx=%d\n",
-                          rnti, h->get_id(), aggr_level);          
+                 rnti, h->get_id(), aggr_level, sf_idx);
-          sched_result->pusch[nof_dci_elems].needs_pdcch = false; 
+
+          sched_result->pusch[nof_dci_elems].needs_pdcch = false;
         } else {
           sched_result->pusch[nof_dci_elems].needs_pdcch = true; 
         }
@@ -939,7 +940,7 @@ void sched::generate_cce_location(srslte_regs_t *regs_, sched_ue::sched_dci_cce_
     nloc = srslte_pdcch_ue_locations_ncce(srslte_regs_pdcch_ncce(regs_, cfi), 
                                    loc, 64, sf_idx, rnti);           
   }
-  
+
   for (uint32_t l=0;l<=3;l++) {
     int n=0;
     for (uint32_t i=0;i<nloc;i++) {
@@ -958,10 +959,12 @@ void sched::generate_cce_location(srslte_regs_t *regs_, sched_ue::sched_dci_cce_
 
 bool sched::generate_dci(srslte_dci_location_t *sched_location, sched_ue::sched_dci_cce_t *locations, uint32_t aggr_level, sched_ue *user) 
 {
-  uint32_t ncand=0;
+  uint32_t nof_cand  = 0;
-  bool allocated=false; 
+  uint32_t test_cand = rand()%locations->nof_loc[aggr_level];
-  while(ncand<locations->nof_loc[aggr_level] && !allocated) {
+  bool allocated=false;
-    uint32_t ncce = locations->cce_start[aggr_level][ncand];
+
+  while(nof_cand<locations->nof_loc[aggr_level] && !allocated) {
+    uint32_t ncce = locations->cce_start[aggr_level][test_cand];
     bool used = false;
     if (user) {
       used = user->pucch_sr_collision(current_tti, ncce);
@@ -972,7 +975,11 @@ bool sched::generate_dci(srslte_dci_location_t *sched_location, sched_ue::sched_
       }
     }
     if (used) {
-      ncand++;
+      test_cand++;
+      if (test_cand==locations->nof_loc[aggr_level]) {
+        test_cand = 0;
+      }
+      nof_cand++;
     } else {
       for (int j=0;j<NCCE(aggr_level) && !used;j++) {
         used_cce[ncce+j] = true; 
@@ -984,7 +991,7 @@ bool sched::generate_dci(srslte_dci_location_t *sched_location, sched_ue::sched_
   
   if (allocated && sched_location) {
     sched_location->L = aggr_level; 
-    sched_location->ncce = locations->cce_start[aggr_level][ncand]; 
+    sched_location->ncce = locations->cce_start[aggr_level][test_cand];
   }
     
   return allocated; 

srsenb/test/upper/ip_test.cc

@@ -591,7 +591,7 @@ int main(int argc, char *argv[])
 int setup_if_addr(char *ip_addr)
 {
   char *dev = (char*) "tun_srsenb";
-  int sock = 0;
+  int sock = -1;
 
   // Construct the TUN device
   int tun_fd = open("/dev/net/tun", O_RDWR);
@@ -642,10 +642,16 @@ int setup_if_addr(char *ip_addr)
     perror("ioctl");
     goto clean_exit;
   }
+  shutdown(sock, SHUT_RDWR);
 
   return(tun_fd);
 
 clean_exit:
-  close(tun_fd);
+  if (sock != -1) {
+    shutdown(sock, SHUT_RDWR);
+  }
+  if (tun_fd != -1) {
+    close(tun_fd);
+  }
   return SRSLTE_ERROR;
 }

srsepc/user_db.csv.example

@@ -10,4 +10,4 @@
 #
 # Note: Lines starting by '#' are ignored
 ue1,001010123456789,00112233445566778899aabbccddeeff,63BFA50EE6523365FF14C1F45F88737D,9001
-ue2,001010123456780,00112233445566778899aabbccddeeaa,63BFA50EE6523365FF14C1F45F88737D,2000
+ue2,001010123456780,00112233445566778899aabbccddeeaa,63BFA50EE6523365FF14C1F45F88737D,8000

srsue/hdr/mac/mac.h

@@ -113,7 +113,7 @@ private:
   void run_thread(); 
   
   static const int MAC_MAIN_THREAD_PRIO = -1; // Use default high-priority below UHD
-  static const int MAC_PDU_THREAD_PRIO  = -1;
+  static const int MAC_PDU_THREAD_PRIO  = DEFAULT_PRIORITY-5;
   static const int MAC_NOF_HARQ_PROC    = 2*HARQ_DELAY_MS;
 
   // Interaction with PHY 

srsue/hdr/mac/ul_harq.h

@@ -145,8 +145,13 @@ public:
 private:
   class ul_harq_process {
   public:
-    ul_harq_process()
+    ul_harq_process() {
-    {
+      pid = 0;
+      harq_feedback = false;
+      log_h = NULL;
+      bzero(&softbuffer, sizeof(srslte_softbuffer_tx_t));
+      is_msg3 = false;
+      pdu_ptr = NULL;
       current_tx_nb = 0;
       current_irv = 0;
       is_initiated = false;
@@ -201,7 +206,7 @@ private:
     {
       if (ack) {
         if (grant) {
-          if (grant->ndi[0] == get_ndi()) {
+          if (grant->ndi[0] == get_ndi() && grant->phy_grant.ul.mcs.tbs != 0) {
             *ack = false;
           }
         }
@@ -210,7 +215,7 @@ private:
 
       // Reset HARQ process if TB has changed
       if (harq_feedback && has_grant() && grant) {
-        if (grant->n_bytes[0] != cur_grant.n_bytes[0] && cur_grant.n_bytes[0] > 0) {
+        if (grant->n_bytes[0] != cur_grant.n_bytes[0] && cur_grant.n_bytes[0] > 0 && grant->n_bytes[0] > 0) {
           Debug("UL %d: Reset due to change of grant size last_grant=%d, new_grant=%d\n",
                pid, cur_grant.n_bytes[0], grant->n_bytes[0]);
           reset();
@@ -329,7 +334,7 @@ private:
 
       // HARQ entity requests an adaptive transmission
       if (grant) {
-        if (grant->rv) {
+        if (grant->rv[0]) {
           current_irv = irv_of_rv[grant->rv[0]%4];
         }
 

srsue/hdr/phy/phch_recv.h

@@ -166,7 +166,7 @@ private:
     typedef enum {IDLE, MEASURE_OK, ERROR} ret_code;
 
     ~measure();
-    void      init(cf_t *buffer[SRSLTE_MAX_PORTS], srslte::log *log_h,
+    void      init(cf_t *buffer[SRSLTE_MAX_PORTS], srslte::log *log_h, srslte::radio *radio_h,
                    uint32_t nof_rx_antennas, uint32_t nof_subframes = RSRP_MEASURE_NOF_FRAMES);
     void      reset();
     void      set_cell(srslte_cell_t cell);
@@ -182,11 +182,12 @@ 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;
     float rx_gain_offset;
-    float mean_rsrp, mean_rsrq, mean_snr;
+    float mean_rsrp, mean_rsrq, mean_snr, mean_rssi;
     uint32_t final_offset;
     const static int RSRP_MEASURE_NOF_FRAMES = 5;
   };
@@ -201,13 +202,11 @@ private:
       float    rsrq;
       uint32_t offset;
     } cell_info_t;
-    void init(srslte::log *log_h, bool sic_pss_enabled);
+    void init(srslte::log *log_h, bool sic_pss_enabled, uint32_t max_sf_window);
     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:
 
-    const static int DEFAULT_MEASUREMENT_LEN = 10;
-
     cf_t               *input_cfo_corrected;
     cf_t               *sf_buffer[SRSLTE_MAX_PORTS];
     srslte::log        *log_h;
@@ -235,8 +234,10 @@ private:
     void write(uint32_t tti, cf_t *data, uint32_t nsamples);
   private:
     void run_thread();
-    const static int CAPTURE_LEN_SF = 15;
+    const static int INTRA_FREQ_MEAS_LEN_MS    = 20;
     const static int INTRA_FREQ_MEAS_PERIOD_MS = 200;
+    const static int INTRA_FREQ_MEAS_PRIO      = DEFAULT_PRIORITY + 5;
+
     scell_recv         scell;
     rrc_interface_phy  *rrc;
     srslte::log        *log_h;
@@ -260,6 +261,8 @@ private:
     srslte_ringbuffer_t ring_buffer;
   };
 
+  // 36.133 9.1.2.1 for band 7
+  const static float ABSOLUTE_RSRP_THRESHOLD_DBM = -125;
 
 
   // Objects for internal use
@@ -291,6 +294,13 @@ private:
   // Sync metrics
   sync_metrics_t        metrics;
 
+  // in-sync / out-of-sync counters
+  uint32_t out_of_sync_cnt;
+  uint32_t in_sync_cnt;
+
+  const static uint32_t NOF_OUT_OF_SYNC_SF = 200;
+  const static uint32_t NOF_IN_SYNC_SF     = 100;
+
   // State for primary cell
   enum {
     IDLE = 0,

srsue/hdr/phy/phch_worker.h

@@ -70,6 +70,8 @@ public:
   void start_plot();
 
   float get_ref_cfo();
+  float get_cfo();
+  float get_ul_cfo();
 
 private:
   /* Inherited from thread_pool::worker. Function called every subframe to run the DL/UL processing */

srsue/hdr/phy/phy.h

@@ -78,6 +78,7 @@ public:
   void set_earfcn(std::vector<uint32_t> earfcns);
   void force_freq(float dl_freq, float ul_freq);
 
+  void radio_overflow();
 
   /********** RRC INTERFACE ********************/
   void    reset();

srsue/hdr/ue.h

@@ -73,13 +73,14 @@ public:
   void start_plot();
 
   static void rf_msg(srslte_rf_error_t error);
-  void handle_rf_msg(srslte_rf_error_t error);
 
   // UE metrics interface
   bool get_metrics(ue_metrics_t &m);
 
   void pregenerate_signals(bool enable);
 
+  void radio_overflow();
+
 private:
   virtual ~ue();
 

srsue/hdr/ue_base.h

@@ -156,6 +156,8 @@ public:
   virtual void stop() = 0;
   virtual bool is_attached() = 0;
   virtual void start_plot() = 0;
+
+  virtual void radio_overflow() = 0;
   
   void handle_rf_msg(srslte_rf_error_t error);
 

srsue/hdr/upper/rrc.h

@@ -51,6 +51,58 @@ using srslte::byte_buffer_t;
 
 namespace srsue {
 
+
+class cell_t
+{
+ public:
+  bool is_valid() {
+    return earfcn != 0 && srslte_cell_isvalid(&phy_cell);
+  }
+  bool equals(cell_t *x) {
+    return equals(x->earfcn, x->phy_cell.id);
+  }
+  bool equals(uint32_t earfcn, uint32_t pci) {
+    return earfcn == this->earfcn && pci == phy_cell.id;
+  }
+  bool greater(cell_t *x) {
+    return x->rsrp > rsrp;
+  }
+  bool plmn_equals(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {
+    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() {
+    this->has_valid_sib1 = false;
+    this->has_valid_sib2 = false;
+    this->has_valid_sib3 = false;
+  }
+  cell_t(srslte_cell_t phy_cell, uint32_t earfcn, float rsrp) {
+    this->has_valid_sib1 = false;
+    this->has_valid_sib2 = false;
+    this->has_valid_sib3 = false;
+    this->phy_cell = phy_cell;
+    this->rsrp = rsrp;
+    this->earfcn = earfcn;
+  }
+
+  uint32_t earfcn;
+  srslte_cell_t phy_cell;
+  float    rsrp;
+  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;
+  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13_STRUCT sib13;
+};
+
 class rrc
   :public rrc_interface_nas
   ,public rrc_interface_phy
@@ -100,7 +152,7 @@ public:
   void out_of_sync();
   void earfcn_end();
   void cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp);
-  void new_phy_meas(float rsrp, float rsrq, uint32_t tti, uint32_t earfcn, uint32_t pci);
+  void new_phy_meas(float rsrp, float rsrq, uint32_t tti, int earfcn, int pci);
 
   // MAC interface
   void ho_ra_completed(bool ra_successful);
@@ -154,7 +206,7 @@ private:
   bool first_stimsi_attempt;
 
   uint16_t ho_src_rnti;
-  int      ho_src_cell_idx;
+  cell_t   ho_src_cell;
   phy_interface_rrc::phy_cfg_t ho_src_phy_cfg;
   mac_interface_rrc::mac_cfg_t ho_src_mac_cfg;
   bool pending_mob_reconf;
@@ -215,28 +267,18 @@ private:
     }
   }
 
-  typedef struct {
+  // List of strongest neighbour cell
-    uint32_t earfcn;
+  const static int NOF_NEIGHBOUR_CELLS = 8;
-    srslte_cell_t phy_cell;
+  std::vector<cell_t*> neighbour_cells;
-    float    rsrp;
+  cell_t *serving_cell;
-    bool     has_valid_sib1;
+  void set_serving_cell(uint32_t cell_idx);
-    bool     has_valid_sib2;
+  void set_serving_cell(uint32_t earfcn, uint32_t pci);
-    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;
-    LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13_STRUCT sib13;
-  } cell_t;
 
-  const static int MAX_KNOWN_CELLS = 64;
+  int  find_neighbour_cell(uint32_t earfcn, uint32_t pci);
-  cell_t known_cells[MAX_KNOWN_CELLS];
+  bool add_neighbour_cell(uint32_t earfcn, uint32_t pci, float rsrp);
-  cell_t *current_cell;
+  bool add_neighbour_cell(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp);
-
+  bool add_neighbour_cell(cell_t *cell);
-  int        find_cell_idx(uint32_t earfcn, uint32_t pci);
+  void sort_neighbour_cells();
-  cell_t*    add_new_cell(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp);
-  uint32_t   find_best_cell(uint32_t earfcn, srslte_cell_t *cell);
 
   typedef enum {
     SI_ACQUIRE_IDLE = 0,
@@ -253,7 +295,6 @@ private:
 
   void select_next_cell_in_plmn();
   LIBLTE_RRC_PLMN_IDENTITY_STRUCT selected_plmn_id;
-  int last_selected_cell;
 
   bool thread_running;
   void run_thread();
@@ -395,10 +436,10 @@ private:
   // Helpers
   void          ho_failed();
   bool          ho_prepare();
-  void          add_neighbour_cell(uint32_t earfcn, uint32_t pci, float rsrp);
   void          rrc_connection_release();
   void          con_restablish_cell_reselected();
   void          radio_link_failure();
+  void          leave_connected();
 
   static void*  start_sib_thread(void *rrc_);
   void          sib_search();

srsue/hdr/upper/usim.h

@@ -43,7 +43,6 @@ typedef enum{
 typedef struct{
   std::string algo;
   std::string op;
-  std::string amf;
   std::string imsi;
   std::string imei;
   std::string k;

srsue/src/mac/proc_ra.cc

@@ -222,7 +222,7 @@ void ra_proc::step_resource_selection() {
   if (preambleIndex > 0) {
     // Preamble is chosen by Higher layers (ie Network)
     sel_maskIndex = maskIndex;
-    sel_preamble = (uint32_t) preambleIndex%nof_preambles;
+    sel_preamble = (uint32_t) preambleIndex;
   } else {
     // Preamble is chosen by MAC UE
     if (!msg3_transmitted) {
@@ -361,7 +361,7 @@ void ra_proc::tb_decoded_ok() {
           
           // If we have a C-RNTI, tell Mux unit to append C-RNTI CE if no CCCH SDU transmission
           if (transmitted_crnti) {
-            rDebug("Appending C-RNTI MAC CE in next transmission\n");
+            rInfo("Appending C-RNTI MAC CE 0x%x in next transmission\n", transmitted_crnti);
             mux_unit->append_crnti_ce_next_tx(transmitted_crnti);
             phy_h->pdcch_ul_search(SRSLTE_RNTI_USER, transmitted_crnti);
             phy_h->pdcch_dl_search(SRSLTE_RNTI_USER, transmitted_crnti);
@@ -375,7 +375,7 @@ void ra_proc::tb_decoded_ok() {
         contention_resolution_timer->run();
       }  
     } else {
-      rDebug("Found RAR for preamble %d\n", rar_pdu_msg.get()->get_rapid());
+      rInfo("Found RAR for preamble %d\n", rar_pdu_msg.get()->get_rapid());
     }
   }
 }

srsue/src/main.cc

@@ -124,7 +124,6 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
 
     ("usim.algo", bpo::value<string>(&args->usim.algo), "USIM authentication algorithm")
     ("usim.op", bpo::value<string>(&args->usim.op), "USIM operator variant")
-    ("usim.amf", bpo::value<string>(&args->usim.amf), "USIM authentication management field")
     ("usim.imsi", bpo::value<string>(&args->usim.imsi), "USIM IMSI")
     ("usim.imei", bpo::value<string>(&args->usim.imei), "USIM IMEI")
     ("usim.k", bpo::value<string>(&args->usim.k), "USIM K")
@@ -243,11 +242,11 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
      "After the PSS estimation is below cfo_loop_pss_tol for cfo_loop_pss_timeout times consecutively, RS adjustments are allowed.")
 
     ("expert.sic_pss_enabled",
-     bpo::value<bool>(&args->expert.phy.sic_pss_enabled)->default_value(true),
+     bpo::value<bool>(&args->expert.phy.sic_pss_enabled)->default_value(false),
      "Applies Successive Interference Cancellation to PSS signals when searching for neighbour cells. Must be disabled if cells have identical channel and timing.")
 
     ("expert.average_subframe_enabled",
-     bpo::value<bool>(&args->expert.phy.average_subframe_enabled)->default_value(false),
+     bpo::value<bool>(&args->expert.phy.average_subframe_enabled)->default_value(true),
      "Averages in the time domain the channel estimates within 1 subframe. Needs accurate CFO correction.")
 
     ("expert.time_correct_period",

srsue/src/phy/phch_common.cc

@@ -46,7 +46,8 @@ phch_common::phch_common(uint32_t max_mutex_) : tx_mutex(max_mutex_)
   radio_h   = NULL; 
   mac       = NULL; 
   max_mutex = max_mutex_;
-  nof_mutex = 0; 
+  nof_mutex = 0;
+  rx_gain_offset = 0;
 
   bzero(&dl_metrics, sizeof(dl_metrics_t));
   dl_metrics_read = true;
@@ -336,7 +337,6 @@ void phch_common::reset() {
   cur_pusch_power = 0;
   p0_preamble = 0;
   cur_radio_power = 0;
-  rx_gain_offset = 0;
   sr_last_tx_tti = -1;
   cur_pusch_power = 0;
   avg_rsrp = 0;

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, nof_rx_antennas);
+  measure_p.init(sf_buffer, log_h, radio_h, nof_rx_antennas);
 
   // Start intra-frequency measurement
   intra_freq_meas.init(worker_com, rrc, log_h);
@@ -124,6 +124,8 @@ void phch_recv::stop()
 
 void phch_recv::reset()
 {
+  in_sync_cnt = 0;
+  out_of_sync_cnt = 0;
   tx_mutex_cnt = 0;
   phy_state = IDLE;
   time_adv_sec = 0;
@@ -294,15 +296,17 @@ bool phch_recv::stop_sync() {
       usleep(10000);
       cnt++;
     }
+    if (!is_in_idle) {
+      Warning("SYNC:  Could not go to IDLE\n");
+    }
     return is_in_idle;
   }
 }
 
 void phch_recv::reset_sync() {
 
-  wait_radio_reset();
-
   Warning("SYNC:  Resetting sync, cell_search_in_progress=%s\n", cell_search_in_progress?"yes":"no");
+
   search_p.reset();
   srslte_ue_sync_reset(&ue_sync);
   resync_sfn(true, true);
@@ -613,6 +617,7 @@ void phch_recv::run_thread()
               log_h->info("Sync OK. Camping on cell PCI=%d...\n", cell.id);
               phy_state = CELL_CAMP;
             } else {
+              log_h->info("Sync OK. Measuring PCI=%d...\n", cell.id);
               measure_p.reset();
               phy_state = CELL_MEASURE;
             }
@@ -713,6 +718,7 @@ void phch_recv::run_thread()
               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");
               out_of_sync();
               worker->release();
               worker_com->reset_ul();
@@ -742,11 +748,24 @@ void phch_recv::run_thread()
 }
 
 void phch_recv::in_sync() {
-  rrc->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 called by worker or phch_recv every 1 or 5 ms
 void phch_recv::out_of_sync() {
-  rrc->out_of_sync();
+  in_sync_cnt = 0;
+  // Send RRC out-of-sync signal after 200 ms consecutive subframes
+  out_of_sync_cnt++;
+  if (out_of_sync_cnt >= NOF_OUT_OF_SYNC_SF) {
+    rrc->out_of_sync();
+    out_of_sync_cnt = 0;
+  }
 }
 
 
@@ -1019,8 +1038,10 @@ 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, uint32_t nof_rx_antennas, uint32_t nof_subframes)
+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)
+
 {
+  this->radio_h       = radio_h;
   this->log_h         = log_h; 
   this->nof_subframes = nof_subframes;
   for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
@@ -1043,6 +1064,7 @@ void phch_recv::measure::reset() {
   mean_rsrp = 0;
   mean_rsrq = 0;
   mean_snr  = 0;
+  mean_rssi = 0;
 }
 
 void phch_recv::measure::set_cell(srslte_cell_t cell) 
@@ -1071,7 +1093,7 @@ uint32_t phch_recv::measure::frame_st_idx() {
 }
 
 void phch_recv::measure::set_rx_gain_offset(float rx_gain_offset) {
-  this->rx_gain_offset = rx_gain_offset;
+  this->rx_gain_offset  = rx_gain_offset;
 }
 
 phch_recv::measure::ret_code phch_recv::measure::run_subframe_sync(srslte_ue_sync_t *ue_sync, uint32_t sf_idx)
@@ -1103,7 +1125,7 @@ phch_recv::measure::ret_code phch_recv::measure::run_multiple_subframes(cf_t *in
     sf_idx ++;
   }
 
-  float max_rsrp = -99;
+  float max_rsrp = -200;
   int best_test_offset = 0;
   int test_offset = 0;
   bool found_best = false;
@@ -1164,16 +1186,18 @@ phch_recv::measure::ret_code phch_recv::measure::run_subframe(uint32_t sf_idx)
   float rsrp   = 10*log10(srslte_chest_dl_get_rsrp(&ue_dl.chest)) + 30 - rx_gain_offset;
   float rsrq   = 10*log10(srslte_chest_dl_get_rsrq(&ue_dl.chest));
   float snr    = 10*log10(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;
 
   if (cnt == 0) {
     mean_rsrp  = rsrp;
     mean_rsrq  = rsrq;
     mean_snr   = snr;
+    mean_rssi  = rssi;
   } else {
     mean_rsrp = SRSLTE_VEC_CMA(rsrp, mean_rsrp, cnt);
     mean_rsrq = SRSLTE_VEC_CMA(rsrq, mean_rsrq, cnt);
-    mean_snr  = SRSLTE_VEC_CMA(snr,  mean_snr, cnt);
+    mean_snr  = SRSLTE_VEC_CMA(snr,  mean_snr,  cnt);
-
+    mean_rssi = SRSLTE_VEC_CMA(rssi, mean_rssi, cnt);
   }
   cnt++;
 
@@ -1181,6 +1205,20 @@ phch_recv::measure::ret_code phch_recv::measure::run_subframe(uint32_t sf_idx)
               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;
+    }
+  }
+
+  if (cnt > 2) {
     return MEASURE_OK;
   } else {
     return IDLE;
@@ -1196,7 +1234,7 @@ phch_recv::measure::ret_code phch_recv::measure::run_subframe(uint32_t sf_idx)
  * Secondary cell receiver
  */
 
-void phch_recv::scell_recv::init(srslte::log *log_h, bool sic_pss_enabled)
+void phch_recv::scell_recv::init(srslte::log *log_h, bool sic_pss_enabled, uint32_t max_sf_window)
 {
   this->log_h           = log_h;
   this->sic_pss_enabled = sic_pss_enabled;
@@ -1209,23 +1247,24 @@ void phch_recv::scell_recv::init(srslte::log *log_h, bool sic_pss_enabled)
   sf_buffer[0]        = (cf_t*) srslte_vec_malloc(sizeof(cf_t)*max_sf_size);
   input_cfo_corrected = (cf_t*) srslte_vec_malloc(sizeof(cf_t)*15*max_sf_size);
 
-  measure_p.init(sf_buffer, log_h, 1, DEFAULT_MEASUREMENT_LEN);
+  measure_p.init(sf_buffer, log_h, NULL, 1, max_sf_window);
 
   //do this different we don't need all this search window.
-  if(srslte_sync_init(&sync_find, 50*max_sf_size, 5*max_sf_size, max_fft_sz)) {
+  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_cp_en(&sync_find, false);
-  srslte_sync_set_threshold(&sync_find, 1.2);
+  srslte_sync_set_cfo_pss_enable(&sync_find, true);
-  srslte_sync_set_em_alpha(&sync_find, 0.0);
+  srslte_sync_set_threshold(&sync_find, 1.7);
+  srslte_sync_set_em_alpha(&sync_find, 0.3);
 
   // Configure FIND object behaviour (this configuration is always the same)
   srslte_sync_set_cfo_ema_alpha(&sync_find,    1.0);
   srslte_sync_set_cfo_i_enable(&sync_find,     false);
   srslte_sync_set_cfo_pss_enable(&sync_find,   true);
   srslte_sync_set_pss_filt_enable(&sync_find,  true);
-  srslte_sync_set_sss_eq_enable(&sync_find,    true);
+  srslte_sync_set_sss_eq_enable(&sync_find,    false);
 
   sync_find.pss.chest_on_filter = true;
 
@@ -1262,39 +1301,55 @@ int phch_recv::scell_recv::find_cells(cf_t *input_buffer, float rx_gain_offset,
 
   srslte_cell_t found_cell;
   memcpy(&found_cell, &cell, sizeof(srslte_cell_t));
-  found_cell.id = 10000;
 
   measure_p.set_rx_gain_offset(rx_gain_offset);
 
   for (uint32_t n_id_2=0;n_id_2<3;n_id_2++) {
 
+    found_cell.id = 10000;
+
     if (n_id_2 != (cell.id%3) || sic_pss_enabled) {
       srslte_sync_set_N_id_2(&sync_find, n_id_2);
 
-      srslte_sync_find_ret_t sync_res;
+      srslte_sync_find_ret_t sync_res, best_sync_res;
 
       do {
         srslte_sync_reset(&sync_find);
         srslte_sync_cfo_reset(&sync_find);
 
-        int sf5_cnt=-1;
+        best_sync_res = SRSLTE_SYNC_NOFOUND;
-        do {
+        sync_res = SRSLTE_SYNC_NOFOUND;
-          sf5_cnt++;
+        cell_id          = 0;
-          sync_res = srslte_sync_find(&sync_find, input_buffer, sf5_cnt*5*sf_len, &peak_idx);
+        float max_peak   = -1;
-        } while(sync_res != SRSLTE_SYNC_FOUND && (uint32_t) sf5_cnt + 1 < nof_sf/5);
+        uint32_t max_sf5 = 0;
+        uint32_t max_sf_idx = 0;
 
-        switch(sync_res) {
+        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",
+                 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) {
+            best_sync_res = sync_res;
+            max_sf5    = sf5_cnt;
+            max_sf_idx = srslte_sync_get_sf_idx(&sync_find);
+            cell_id    = srslte_sync_get_cell_id(&sync_find);
+          }
+        }
+
+        switch(best_sync_res) {
           case SRSLTE_SYNC_ERROR:
             return SRSLTE_ERROR;
             fprintf(stderr, "Error finding correlation peak\n");
             return SRSLTE_ERROR;
           case SRSLTE_SYNC_FOUND:
-            sf_idx  = srslte_sync_get_sf_idx(&sync_find);
+
-            cell_id = srslte_sync_get_cell_id(&sync_find);
+            sf_idx = (10-max_sf_idx - 5*(max_sf5%2))%10;
 
             if (cell_id >= 0) {
               // We found the same cell as before, look another N_id_2
               if ((uint32_t) cell_id == found_cell.id || (uint32_t) cell_id == cell.id) {
+                Info("n_id_2=%d, PCI=%d, found_cell.id=%d, cell.id=%d\n", n_id_2, cell_id, found_cell.id, cell.id);
                 sync_res = SRSLTE_SYNC_NOFOUND;
               } else {
                 // We found a new cell ID
@@ -1303,34 +1358,42 @@ int phch_recv::scell_recv::find_cells(cf_t *input_buffer, float rx_gain_offset,
                 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))
-                switch(measure_p.run_multiple_subframes(input_cfo_corrected, peak_idx+sf5_cnt*5*sf_len, sf_idx, nof_sf)) {
+                {
                   case measure::MEASURE_OK:
-                    cells[nof_cells].pci    = found_cell.id;
+                    // Consider a cell to be detectable 8.1.2.2.1.1 from 36.133. Currently only using first condition
-                    cells[nof_cells].rsrp   = measure_p.rsrp();
+                    if (measure_p.rsrp() > ABSOLUTE_RSRP_THRESHOLD_DBM) {
-                    cells[nof_cells].rsrq   = measure_p.rsrq();
+                      cells[nof_cells].pci = found_cell.id;
-                    cells[nof_cells].offset = measure_p.frame_st_idx();
+                      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, sf5_cnt=%d, n_id_2=%d, CFO=%6.1f Hz\n",
+                      Info(
-                         nof_cells, cell_id, measure_p.rsrp(), measure_p.frame_st_idx(), sync_find.peak_value, sf5_cnt, n_id_2, 15000*srslte_sync_get_cfo(&sync_find));
+                          "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",
+                          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));
 
-                    nof_cells++;
+                      nof_cells++;
 
-                    if (sic_pss_enabled) {
+                      /*
-                      srslte_pss_sic(&sync_find.pss, &input_buffer[sf5_cnt*5*sf_len+sf_len/2-fft_sz]);
+                      if (sic_pss_enabled) {
+                        srslte_pss_sic(&sync_find.pss, &input_buffer[sf5_cnt * 5 * sf_len + sf_len / 2 - fft_sz]);
+                      }*/
                     }
-
+                    break;
+                  default:
+                    Info("INTRA: Not enough samples to measure PCI=%d\n", cell_id);
                     break;
                   case measure::ERROR:
                     Error("Measuring neighbour cell\n");
                     return SRSLTE_ERROR;
-                  default:
-                    break;
                 }
               }
             } else {
@@ -1396,20 +1459,21 @@ void phch_recv::intra_measure::init(phch_common *common, rrc_interface_phy *rrc,
   receive_enabled = false;
 
   // Start scell
-  scell.init(log_h, common->args->sic_pss_enabled);
+  scell.init(log_h, common->args->sic_pss_enabled, INTRA_FREQ_MEAS_LEN_MS);
 
-  search_buffer = (cf_t*) srslte_vec_malloc(CAPTURE_LEN_SF*SRSLTE_SF_LEN_PRB(SRSLTE_MAX_PRB)*sizeof(cf_t));
+  search_buffer = (cf_t*) srslte_vec_malloc(INTRA_FREQ_MEAS_LEN_MS*SRSLTE_SF_LEN_PRB(SRSLTE_MAX_PRB)*sizeof(cf_t));
 
-  if (srslte_ringbuffer_init(&ring_buffer, sizeof(cf_t)*100*SRSLTE_SF_LEN_PRB(SRSLTE_MAX_PRB))) {
+  if (srslte_ringbuffer_init(&ring_buffer, sizeof(cf_t)*INTRA_FREQ_MEAS_LEN_MS*2*SRSLTE_SF_LEN_PRB(SRSLTE_MAX_PRB))) {
     return;
   }
 
   running = true;
-  start();
+  start(INTRA_FREQ_MEAS_PRIO);
 }
 
 void phch_recv::intra_measure::stop() {
   running = false;
+  srslte_ringbuffer_stop(&ring_buffer);
   tti_sync.increase();
   wait_thread_finish();
 }
@@ -1475,7 +1539,7 @@ void phch_recv::intra_measure::write(uint32_t tti, cf_t *data, uint32_t nsamples
         receiving = false;
       } else {
         receive_cnt++;
-        if (receive_cnt == CAPTURE_LEN_SF) {
+        if (receive_cnt == INTRA_FREQ_MEAS_LEN_MS) {
           tti_sync.increase();
           receiving = false; 
         }
@@ -1492,9 +1556,9 @@ void phch_recv::intra_measure::run_thread()
     }
 
     if (running) {
-      // Read 15 ms data from buffer
+      // Read data from buffer and find cells in it
-      srslte_ringbuffer_read(&ring_buffer, search_buffer, CAPTURE_LEN_SF*current_sflen*sizeof(cf_t));
+      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, CAPTURE_LEN_SF, info);
+      int found_cells = scell.find_cells(search_buffer, common->rx_gain_offset, primary_cell, INTRA_FREQ_MEAS_LEN_MS, info);
       receiving = false;
 
       for (int i=0;i<found_cells;i++) {

srsue/src/phy/phch_worker.cc

@@ -203,6 +203,25 @@ float phch_worker::get_ref_cfo()
   return srslte_chest_dl_get_cfo(&ue_dl.chest);
 }
 
+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) {
@@ -241,15 +260,12 @@ void phch_worker::work_imp()
   /* Do FFT and extract PDCCH LLR, or quit if no actions are required in this subframe */
   bool chest_ok = extract_fft_and_pdcch_llr();
 
-  bool snr_th_err = 10*log10(srslte_chest_dl_get_snr(&ue_dl.chest))<-20.0;
-  bool snr_th_ok  = 10*log10(srslte_chest_dl_get_snr(&ue_dl.chest))>-15.0;
-
   // Call feedback loop for chest
   if (chest_loop && ((1<<(tti%10)) & phy->args->cfo_ref_mask)) {
     chest_loop->set_cfo(srslte_chest_dl_get_cfo(&ue_dl.chest));
   }
 
-  if (chest_ok && !snr_th_err) {
+  if (chest_ok) {
 
     /***** Downlink Processing *******/
 
@@ -327,7 +343,7 @@ void phch_worker::work_imp()
   }
 
   /* Set UL CFO before transmission */  
-  srslte_ue_ul_set_cfo(&ue_ul, cfo);
+  srslte_ue_ul_set_cfo(&ue_ul, get_ul_cfo());
 
   /* Transmit PUSCH, PUCCH or SRS */
   bool signal_ready = false; 
@@ -370,12 +386,13 @@ void phch_worker::work_imp()
   update_measurements();
 
   if (chest_ok) {
-    if (snr_th_ok) {
+    if (phy->avg_rsrp_dbm > -130.0 && phy->avg_snr_db > -30.0) {
-      log_h->debug("SNR=%.1f dB sync=in-sync from channel estimator\n", 10*log10(srslte_chest_dl_get_snr(&ue_dl.chest)));
+      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);
       chest_loop->in_sync();
-    } else if (snr_th_err) {
+    } else {
-      log_h->info("SNR=%.1f dB sync=out-of-sync from channel estimator\n",
+      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)));
+                   10*log10(srslte_chest_dl_get_snr(&ue_dl.chest)), phy->avg_rsrp_dbm);
       chest_loop->out_of_sync();
     }
   }
@@ -478,10 +495,10 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
     srslte_ra_dl_dci_t dci_unpacked;
     
     Debug("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) {
-      return false; 
+      return false;
     }
     
     if (srslte_dci_msg_to_dl_grant(&dci_msg, dl_rnti, cell.nof_prb, cell.nof_ports, &dci_unpacked, &grant->phy_grant.dl)) {
@@ -745,7 +762,7 @@ bool phch_worker::decode_pdcch_ul(mac_interface_phy::mac_grant_t* grant)
     ul_rnti = phy->get_ul_rnti(tti);
     if (ul_rnti) {
       if (srslte_ue_dl_find_ul_dci(&ue_dl, cfi, tti%10, ul_rnti, &dci_msg) != 1) {
-        return false; 
+        return false;
       }
       
       if (srslte_dci_msg_to_ul_grant(&dci_msg, cell.nof_prb, pusch_hopping.hopping_offset, 
@@ -933,7 +950,7 @@ void phch_worker::set_uci_aperiodic_cqi()
 
           int cqi_len = srslte_cqi_value_pack(&cqi_report, uci_data.uci_cqi);
           if (cqi_len < 0) {
-            Error("Error packing CQI value (Aperiodic reporting mode RM31).");
+            Error("Error packing CQI value (Aperiodic reporting mode RM30).");
             return;
           }
           uci_data.uci_cqi_len = (uint32_t) cqi_len;
@@ -942,11 +959,16 @@ void phch_worker::set_uci_aperiodic_cqi()
           srslte_cqi_to_str(uci_data.uci_cqi, uci_data.uci_cqi_len, cqi_str, SRSLTE_CQI_STR_MAX_CHAR);
 
           /* Set RI = 1 */
-          uci_data.uci_ri = ri;
+          if (phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_3 ||
-          uci_data.uci_ri_len = 1;
+              phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_4) {
+            uci_data.uci_ri = ri;
+            uci_data.uci_ri_len = 1;
+          } else {
+            uci_data.uci_ri_len = 0;
+          }
 
-          Info("PUSCH: Aperiodic RM30 ri%s, CQI=%s, SNR=%.1f dB, for %d subbands\n",
+          Info("PUSCH: Aperiodic RM30 CQI=%s, %sSNR=%.1f dB, for %d subbands\n",
-               (uci_data.uci_ri == 0)?"=1":"~1", cqi_str, phy->avg_snr_db, cqi_report.subband_hl.N);
+               cqi_str, (uci_data.uci_ri_len)?((uci_data.uci_ri == 0)?"ri=0, ":"ri=1, "):"", phy->avg_snr_db, cqi_report.subband_hl.N);
         }
         break;
       case LIBLTE_RRC_CQI_REPORT_MODE_APERIODIC_RM31:
@@ -1473,6 +1495,13 @@ plot_scatter_t pconst;
 float tmp_plot[SCATTER_PDSCH_BUFFER_LEN];
 cf_t  tmp_plot2[SRSLTE_SF_LEN_RE(SRSLTE_MAX_PRB, SRSLTE_CP_NORM)];
 
+#define CFO_PLOT_LEN 0 /* Set to non zero for enabling CFO plot */
+#if CFO_PLOT_LEN > 0
+static plot_real_t    pcfo;
+static uint32_t icfo = 0;
+static float cfo_buffer[CFO_PLOT_LEN];
+#endif /* CFO_PLOT_LEN > 0 */
+
 void *plot_thread_run(void *arg) {
   srsue::phch_worker *worker = (srsue::phch_worker*) arg; 
 
@@ -1497,10 +1526,14 @@ void *plot_thread_run(void *arg) {
 
   plot_scatter_addToWindowGrid(&pconst, (char*)"srsue", 0, worker->get_rx_nof_antennas());
 
+#if CFO_PLOT_LEN > 0
+  plot_real_init(&pcfo);
+  plot_real_setTitle(&pcfo, (char*) "CFO (Hz)");
+  plot_real_setLabels(&pcfo, (char *) "Time", (char *) "Hz");
+  plot_real_setYAxisScale(&pcfo, -4000, 4000);
 
-
+  plot_scatter_addToWindowGrid(&pcfo, (char*)"srsue", 1, worker->get_rx_nof_antennas());
-
+#endif /* CFO_PLOT_LEN > 0 */
-
 
   int n; 
   int readed_pdsch_re=0; 
@@ -1524,7 +1557,14 @@ void *plot_thread_run(void *arg) {
       }
       readed_pdsch_re = 0; 
     }
-  }  
+
+#if CFO_PLOT_LEN > 0
+    cfo_buffer[icfo] = worker->get_cfo() * 15000.0f;
+    icfo = (icfo + 1)%CFO_PLOT_LEN;
+    plot_real_setNewData(&pcfo, cfo_buffer, CFO_PLOT_LEN);
+#endif /* CFO_PLOT_LEN > 0 */
+
+  }
   return NULL;
 }
 

srsue/src/phy/phy.cc

@@ -352,6 +352,11 @@ int phy::prach_tx_tti()
   return prach_buffer.tx_tti();
 }
 
+// Handle the case of a radio overflow. Resynchronise inmediatly
+void phy::radio_overflow() {
+  sf_recv.reset_sync();
+}
+
 void phy::reset()
 {
   Info("Resetting PHY\n");

srsue/src/ue.cc

@@ -297,10 +297,17 @@ bool ue::get_metrics(ue_metrics_t &m)
   return false;
 }
 
+void ue::radio_overflow() {
+  phy.radio_overflow();
+}
+
 void ue::rf_msg(srslte_rf_error_t error)
 {
   ue_base *ue = ue_base::get_instance(LTE);
   ue->handle_rf_msg(error);
+  if(error.type == srslte_rf_error_t::SRSLTE_RF_ERROR_OVERFLOW) {
+    ue->radio_overflow();
+  }
 }
 
 } // namespace srsue

srsue/src/upper/rrc.cc

@@ -52,6 +52,7 @@ rrc::rrc()
 {
   n310_cnt       = 0;
   n311_cnt       = 0;
+  serving_cell = new cell_t();
 }
 
 static void liblte_rrc_handler(void *ctx, char *str) {
@@ -89,8 +90,6 @@ void rrc::init(phy_interface_rrc *phy_,
   state = RRC_STATE_IDLE;
   si_acquire_state = SI_ACQUIRE_IDLE;
 
-  bzero(known_cells, MAX_KNOWN_CELLS*sizeof(cell_t));
-
   thread_running = true;
   start();
 
@@ -210,13 +209,13 @@ void rrc::run_thread() {
          * Cell is selected when all SIBs downloaded or applied.
          */
         if (phy->sync_status()) {
-          if (!current_cell->has_valid_sib1) {
+          if (!serving_cell->has_valid_sib1) {
             si_acquire_state = SI_ACQUIRE_SIB1;
             sysinfo_index = 0;
-          } else if (!current_cell->has_valid_sib2) {
+          } else if (!serving_cell->has_valid_sib2) {
             si_acquire_state = SI_ACQUIRE_SIB2;
           } else {
-            apply_sib2_configs(&current_cell->sib2);
+            apply_sib2_configs(&serving_cell->sib2);
             si_acquire_state = SI_ACQUIRE_IDLE;
             state = RRC_STATE_CELL_SELECTED;
           }
@@ -228,6 +227,7 @@ void rrc::run_thread() {
             rrc_log->info("RRC Cell Selecting: timeout expired. Starting Cell Search...\n");
             plmn_select_timeout = 0;
             select_cell_timeout = 0;
+            serving_cell->in_sync = false;
             phy->cell_search_start();
           }
         }
@@ -254,7 +254,7 @@ void rrc::run_thread() {
         } else {
           rrc_log->info("RRC Cell Selected: Starting paging and going to IDLE...\n");
           mac->pcch_start_rx();
-          state = RRC_STATE_IDLE;
+          state = RRC_STATE_LEAVE_CONNECTED;
         }
         break;
       case RRC_STATE_CONNECTING:
@@ -287,25 +287,7 @@ void rrc::run_thread() {
         break;
       case RRC_STATE_LEAVE_CONNECTED:
         usleep(60000);
-        rrc_log->console("RRC IDLE\n");
+        leave_connected();
-        rrc_log->info("Leaving RRC_CONNECTED state\n");
-        drb_up = false;
-        measurements.reset();
-        pdcp->reset();
-        rlc->reset();
-        phy->reset();
-        mac->reset();
-        set_phy_default();
-        set_mac_default();
-        mac_timers->timer_get(t310)->stop();
-        mac_timers->timer_get(t311)->stop();
-        if (phy->sync_status()) {
-          // Instruct MAC to look for P-RNTI
-          mac->pcch_start_rx();
-          // Instruct PHY to measure serving cell for cell reselection
-          phy->meas_start(phy->get_current_earfcn(), phy->get_current_pci());
-        }
-
         // Move to RRC_IDLE
         state = RRC_STATE_IDLE;
         break;
@@ -370,19 +352,19 @@ void rrc::run_si_acquisition_procedure()
       break;
     case SI_ACQUIRE_SIB2:
       // Instruct MAC to look for next SIB
-      if(sysinfo_index < current_cell->sib1.N_sched_info) {
+      if(sysinfo_index < serving_cell->sib1.N_sched_info) {
-        si_win_len   = liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length];
+        si_win_len   = liblte_rrc_si_window_length_num[serving_cell->sib1.si_window_length];
         x            = sysinfo_index*si_win_len;
         sf           = x%10;
         offset       = x/10;
 
         tti          = mac->get_current_tti();
-        period       = liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[sysinfo_index].si_periodicity];
+        period       = liblte_rrc_si_periodicity_num[serving_cell->sib1.sched_info[sysinfo_index].si_periodicity];
         si_win_start = sib_start_tti(tti, period, offset, sf);
 
         if (tti > last_win_start + 10) {
           last_win_start = si_win_start;
-          si_win_len = liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length];
+          si_win_len = liblte_rrc_si_window_length_num[serving_cell->sib1.si_window_length];
 
           mac->bcch_start_rx(si_win_start, si_win_len);
           rrc_log->debug("Instructed MAC to search for system info, win_start=%d, win_len=%d\n",
@@ -421,19 +403,15 @@ void rrc::run_si_acquisition_procedure()
 *******************************************************************************/
 
 uint16_t rrc::get_mcc() {
-  if (current_cell) {
+  if (serving_cell->sib1.N_plmn_ids > 0) {
-    if (current_cell->sib1.N_plmn_ids > 0) {
+    return serving_cell->sib1.plmn_id[0].id.mcc;
-      return current_cell->sib1.plmn_id[0].id.mcc;
-    }
   }
   return 0;
 }
 
 uint16_t rrc::get_mnc() {
-  if (current_cell) {
+  if (serving_cell->sib1.N_plmn_ids > 0) {
-    if (current_cell->sib1.N_plmn_ids > 0) {
+    return serving_cell->sib1.plmn_id[0].id.mnc;
-      return current_cell->sib1.plmn_id[0].id.mnc;
-    }
   }
   return 0;
 }
@@ -468,188 +446,261 @@ void rrc::plmn_select_rrc(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {
       // Sort cells according to RSRP
 
       selected_plmn_id = plmn_id;
-      last_selected_cell = -1;
       select_cell_timeout = 0;
 
       state = RRC_STATE_CELL_SELECTING;
-      select_next_cell_in_plmn();
     }
   } else {
     rrc_log->warning("Requested PLMN select in incorrect state %s\n", rrc_state_text[state]);
   }
 }
 
-void rrc::select_next_cell_in_plmn() {
+void rrc::set_serving_cell(uint32_t earfcn, uint32_t pci) {
-  for (uint32_t i = last_selected_cell + 1; i < MAX_KNOWN_CELLS && known_cells[i].earfcn; i++) {
+  int cell_idx = find_neighbour_cell(earfcn, pci);
-    for (uint32_t j = 0; j < known_cells[i].sib1.N_plmn_ids; j++) {
+  if (cell_idx >= 0) {
-      if (known_cells[i].sib1.plmn_id[j].id.mcc == selected_plmn_id.mcc ||
+    set_serving_cell(cell_idx);
-          known_cells[i].sib1.plmn_id[j].id.mnc == selected_plmn_id.mnc) {
+  } else {
-        rrc_log->info("Selecting cell PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
+    rrc_log->error("Setting serving cell: Unkonwn cell with earfcn=%d, PCI=%d\n", earfcn, pci);
-                      known_cells[i].phy_cell.id, known_cells[i].earfcn,
+  }
-                      known_cells[i].sib1.cell_id);
+}
-        rrc_log->console("Select cell: PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
+
-                      known_cells[i].phy_cell.id, known_cells[i].earfcn,
+void rrc::set_serving_cell(uint32_t cell_idx) {
-                      known_cells[i].sib1.cell_id);
+  if (cell_idx < neighbour_cells.size())
-        // Check that cell satisfies S criteria
+  {
-        if (known_cells[i].in_sync) { // %% rsrp > S dbm
+    // Remove future serving cell from neighbours to make space for current serving cell
-          // Try to select Cell
+    cell_t *new_serving_cell = neighbour_cells[cell_idx];
-          if (phy->cell_select(known_cells[i].earfcn, known_cells[i].phy_cell))
+    if (!new_serving_cell) {
-          {
+      rrc_log->error("Setting serving cell. Index %d is empty\n", cell_idx);
-            last_selected_cell = i;
+      return;
-            current_cell = &known_cells[i];
+    }
-            rrc_log->info("Selected cell PCI=%d, EARFCN=%d, Cell ID=0x%x, addr=0x%x\n",
+    neighbour_cells.erase(std::remove(neighbour_cells.begin(), neighbour_cells.end(), neighbour_cells[cell_idx]), neighbour_cells.end());
-                          current_cell->phy_cell.id, current_cell->earfcn,
+
-                          current_cell->sib1.cell_id, current_cell);
+    // Move serving cell to neighbours list
-            return;
+    if (serving_cell->is_valid()) {
-          } else {
+      // Make sure it does not exist already
-            rrc_log->warning("Selecting cell EARFCN=%d, Cell ID=0x%x.\n",
+      int serving_idx = find_neighbour_cell(serving_cell->earfcn, serving_cell->phy_cell.id);
-                           known_cells[i].earfcn, known_cells[i].sib1.cell_id);
+      if (serving_idx >= 0 && (uint32_t) serving_idx < neighbour_cells.size()) {
-          }
+        printf("Error serving cell is already in the neighbour list. Removing it\n");
-        }
+        neighbour_cells.erase(std::remove(neighbour_cells.begin(), neighbour_cells.end(), neighbour_cells[serving_idx]), neighbour_cells.end());
       }
+      // If not in the list, add it to the list of neighbours (sorted inside the function)
+      if (!add_neighbour_cell(serving_cell)) {
+        rrc_log->info("Serving cell not added to list of neighbours. Worse than current neighbours\n");
+      }
+    }
+
+    // Set new serving cell
+    serving_cell = new_serving_cell;
+
+    rrc_log->info("Setting serving cell idx=%d, earfcn=%d, PCI=%d, nof_neighbours=%d\n",
+                  cell_idx, serving_cell->earfcn, serving_cell->phy_cell.id, neighbour_cells.size());
+
+  } else {
+    rrc_log->error("Setting invalid serving cell idx %d\n", cell_idx);
+  }
+}
+
+void rrc::select_next_cell_in_plmn() {
+  // Neighbour cells are sorted in descending order of RSRP
+  for (uint32_t i = 0; i < neighbour_cells.size(); i++) {
+    if (neighbour_cells[i]->plmn_equals(selected_plmn_id) &&
+        neighbour_cells[i]->in_sync) // matches S criteria
+    {
+      // Try to select Cell
+      if (phy->cell_select(neighbour_cells[i]->earfcn, neighbour_cells[i]->phy_cell)) {
+        set_serving_cell(i);
+        rrc_log->info("Selected cell PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
+                      serving_cell->phy_cell.id, serving_cell->earfcn,
+                      serving_cell->sib1.cell_id);
+        rrc_log->console("Selected cell PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
+                         serving_cell->phy_cell.id, serving_cell->earfcn,
+                         serving_cell->sib1.cell_id);
+      } else {
+        // Set to out-of-sync if can't synchronize
+        neighbour_cells[i]->in_sync = false;
+        rrc_log->warning("Selecting cell EARFCN=%d, Cell ID=0x%x.\n",
+                         neighbour_cells[i]->earfcn, neighbour_cells[i]->sib1.cell_id);
+      }
+      return;
     }
   }
   rrc_log->info("No more known cells. Starting again\n");
-  last_selected_cell = -1;
 }
 
-void rrc::new_phy_meas(float rsrp, float rsrq, uint32_t tti, uint32_t earfcn, uint32_t pci) {
+void rrc::new_phy_meas(float rsrp, float rsrq, uint32_t tti, int earfcn_i, int pci_i) {
+
+  if (earfcn_i < 0 || pci_i < 0) {
+    earfcn_i = serving_cell->earfcn;
+    pci_i    = serving_cell->phy_cell.id;
+  }
+
+  uint32_t earfcn = (uint32_t) earfcn_i;
+  uint32_t pci    = (uint32_t) pci_i;
+
+  // Measurements in RRC_CONNECTED go through measuremnt class to log reports etc.
   if (state != RRC_STATE_IDLE) {
     measurements.new_phy_meas(earfcn, pci, rsrp, rsrq, tti);
-  } else {
-    // If measurement is of the serving cell, evaluate cell reselection criteria
-    if ((earfcn == phy->get_current_earfcn() && pci == phy->get_current_pci()) || (earfcn == 0 && pci == 0)) {
-      cell_reselection_eval(rsrp, rsrq);
-      current_cell->rsrp = rsrp;
-      rrc_log->info("MEAS:  New measurement serving cell, rsrp=%f, rsrq=%f, tti=%d\n", rsrp, rsrq, tti);
-    } else {
-      // Add/update cell measurement
-      srslte_cell_t cell;
-      phy->get_current_cell(&cell, NULL);
-      cell.id = pci;
-      add_new_cell(earfcn, cell, rsrp);
 
-      rrc_log->info("MEAS:  New measurement PCI=%d, RSRP=%.1f dBm.\n", pci, rsrp);
+  // Measurements in RRC_IDLE update serving cell and check for reselection
+  } else {
+
+    // Update serving cell 
+    if (serving_cell->equals(earfcn, pci)) {
+      cell_reselection_eval(rsrp, rsrq);
+      serving_cell->rsrp = rsrp;
+      rrc_log->info("MEAS:  New measurement serving cell in IDLE, rsrp=%f, rsrq=%f, tti=%d\n", rsrp, rsrq, tti);
+      
+    // Or update/add neighbour cell
+    } else {
+      if (add_neighbour_cell(earfcn, pci, rsrp)) {
+        rrc_log->info("MEAS:  New measurement neighbour in IDLE, PCI=%d, RSRP=%.1f dBm.\n", pci, rsrp);
+      } else {
+        rrc_log->info("MEAS:  Neighbour Cell in IDLE PCI=%d, RSRP=%.1f dBm not added. Worse than current neighbours\n", pci, rsrp);
+      }
     }
 
-    srslte_cell_t best_cell;
+    // Verify cell selection criteria with strongest neighbour cell (always first)
-    uint32_t best_cell_idx = find_best_cell(phy->get_current_earfcn(), &best_cell);
+    if (cell_selection_eval(neighbour_cells[0]->rsrp)     &&
-
+        neighbour_cells[0]->rsrp > serving_cell->rsrp + 5)
-    // Verify cell selection criteria
-    if (cell_selection_eval(known_cells[best_cell_idx].rsrp)     &&
-        known_cells[best_cell_idx].rsrp > current_cell->rsrp + 5 &&
-        best_cell.id != phy->get_current_pci())
     {
-      rrc_log->info("Selecting best neighbour cell PCI=%d, rsrp=%.1f dBm\n", best_cell.id, known_cells[best_cell_idx].rsrp);
+      set_serving_cell(0);
+      rrc_log->info("Selecting best neighbour cell PCI=%d, rsrp=%.1f dBm\n", serving_cell->phy_cell.id, serving_cell->rsrp);
       state = RRC_STATE_CELL_SELECTING;
-      current_cell = &known_cells[best_cell_idx];
+      phy->cell_select(serving_cell->earfcn, serving_cell->phy_cell);
-      phy->cell_select(phy->get_current_earfcn(), best_cell);
     }
   }
 }
 
 void rrc::cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
 
-  // find if cell_id-earfcn combination already exists
+  bool found = false;
-  for (uint32_t i = 0; i < MAX_KNOWN_CELLS && known_cells[i].earfcn; i++) {
+  int cell_idx = -1; 
-    if (earfcn == known_cells[i].earfcn && phy_cell.id == known_cells[i].phy_cell.id) {
+  
-      current_cell = &known_cells[i];
+  if (serving_cell->equals(earfcn, phy_cell.id)) {
-      current_cell->rsrp    = rsrp;
+    serving_cell->rsrp = rsrp;
-      current_cell->in_sync = true;
+    serving_cell->in_sync = true;
-      rrc_log->info("Updating cell EARFCN=%d, PCI=%d, RSRP=%.1f dBm\n", current_cell->earfcn,
+    found = true;
-                    current_cell->phy_cell.id, current_cell->rsrp);
+  } else {
-
+    // Check if cell is in our list of neighbour cells 
-      if (!current_cell->has_valid_sib1) {
+    cell_idx = find_neighbour_cell(earfcn, phy_cell.id);
-        si_acquire_state = SI_ACQUIRE_SIB1;
+    if (cell_idx >= 0) {
-      } else if (state == RRC_STATE_PLMN_SELECTION) {
+      set_serving_cell(cell_idx);
-        for (uint32_t j = 0; j < current_cell->sib1.N_plmn_ids; j++) {
+      serving_cell->rsrp = rsrp;
-          nas->plmn_found(current_cell->sib1.plmn_id[j].id, current_cell->sib1.tracking_area_code);
+      serving_cell->in_sync = true;
-        }
+      found = true;
-        usleep(5000);
-        phy->cell_search_next();
-      }
-      return;
     }
   }
-  // add to list of known cells and set current_cell
+  if (found) {
-  current_cell = add_new_cell(earfcn, phy_cell, rsrp);
-  if(!current_cell) {
-    current_cell = &known_cells[0];
-    rrc_log->error("Couldn't add new cell\n");
-    return;
-  }
 
-  si_acquire_state = SI_ACQUIRE_SIB1;
+    if (!serving_cell->has_valid_sib1) {
-
+      si_acquire_state = SI_ACQUIRE_SIB1;
-  rrc_log->info("New Cell: PCI=%d, PRB=%d, Ports=%d, EARFCN=%d, RSRP=%.1f dBm, addr=0x%x\n",
+    } else if (state == RRC_STATE_PLMN_SELECTION) {
-                current_cell->phy_cell.id, current_cell->phy_cell.nof_prb, current_cell->phy_cell.nof_ports,
+      for (uint32_t j = 0; j < serving_cell->sib1.N_plmn_ids; j++) {
-                current_cell->earfcn, current_cell->rsrp, current_cell);
+        nas->plmn_found(serving_cell->sib1.plmn_id[j].id, serving_cell->sib1.tracking_area_code);
-}
-
-uint32_t rrc::find_best_cell(uint32_t earfcn, srslte_cell_t *cell) {
-  float    best_rsrp = -INFINITY;
-  uint32_t best_cell_idx = 0;
-  for (int i=0;i<MAX_KNOWN_CELLS;i++) {
-    if (known_cells[i].earfcn == earfcn) {
-      if (known_cells[i].rsrp > best_rsrp) {
-        best_rsrp     = known_cells[i].rsrp;
-        best_cell_idx = i;
       }
+      usleep(5000);
+      phy->cell_search_next();
+    }
+  } else {
+    // add to list of known cells and set current_cell
+    if (!add_neighbour_cell(earfcn, phy_cell, rsrp)) {
+      rrc_log->info("No more space for neighbour cells (detected cell RSRP=%.1f dBm worse than current %d neighbours)\n", 
+                    rsrp, NOF_NEIGHBOUR_CELLS);
+      usleep(5000);
+      phy->cell_search_next();
+    } else {
+      set_serving_cell(earfcn, phy_cell.id);
+
+      si_acquire_state = SI_ACQUIRE_SIB1;
     }
   }
-  if (cell) {
+
-    memcpy(cell, &known_cells[best_cell_idx].phy_cell, sizeof(srslte_cell_t));
+  rrc_log->info("%s %s cell EARFCN=%d, PCI=%d, RSRP=%.1f dBm\n",
-  }
+                found?"Updating":"Adding",
-  return best_cell_idx;
+                cell_idx>=0?"neighbour":"serving",
+                serving_cell->earfcn,
+                serving_cell->phy_cell.id,
+                serving_cell->rsrp);
 }
 
-rrc::cell_t* rrc::add_new_cell(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
+bool sort_rsrp(cell_t *u1, cell_t *u2) {
+  return !u1->greater(u2);
+}
+
+// Sort neighbour cells by decreasing order of RSRP
+void rrc::sort_neighbour_cells() {
+
+  for (uint32_t i=1;i<neighbour_cells.size();i++) {
+    if (neighbour_cells[i]->in_sync == false) {
+      rrc_log->info("Removing neighbour cell PCI=%d, out_of_sync\n", neighbour_cells[i]->phy_cell.id);
+      neighbour_cells.erase(std::remove(neighbour_cells.begin(), neighbour_cells.end(), neighbour_cells[i]), neighbour_cells.end());
+    }
+  }
+
+  std::sort(neighbour_cells.begin(), neighbour_cells.end(), sort_rsrp);
+
+  char ordered[512];
+  int n=0;
+  n += snprintf(ordered, 512, "[pci=%d, rsrsp=%.2f", neighbour_cells[0]->phy_cell.id, neighbour_cells[0]->rsrp);
+  for (uint32_t i=1;i<neighbour_cells.size();i++) {
+    n += snprintf(&ordered[n], 512-n, " | pci=%d, rsrp=%.2f", neighbour_cells[i]->phy_cell.id, neighbour_cells[i]->rsrp);
+  }
+  rrc_log->info("Sorted neighbour cells: %s]\n", ordered);
+}
+
+bool rrc::add_neighbour_cell(cell_t *new_cell) {
+  bool ret = false;
+  if (neighbour_cells.size() < NOF_NEIGHBOUR_CELLS - 1) {
+    ret = true;
+  } else if (!neighbour_cells[neighbour_cells.size()-1]->greater(new_cell)) {
+    // Delete old one
+    delete neighbour_cells[neighbour_cells.size()-1];
+    neighbour_cells.erase(std::remove(neighbour_cells.begin(), neighbour_cells.end(), neighbour_cells[neighbour_cells.size()-1]), neighbour_cells.end());
+    ret = true;
+  }
+  if (ret) {
+    neighbour_cells.push_back(new_cell);
+  }
+  rrc_log->info("Added neighbour cell EARFCN=%d, PCI=%d, nof_neighbours=%d\n",
+                new_cell->earfcn, new_cell->phy_cell.id, neighbour_cells.size());
+  sort_neighbour_cells();
+  return ret;
+}
+
+// If only neighbour PCI is provided, copy full cell from serving cell
+bool rrc::add_neighbour_cell(uint32_t earfcn, uint32_t pci, float rsrp) {
+  srslte_cell_t serving_phy;
+  serving_phy = serving_cell->phy_cell;
+  serving_phy.id = pci;
+  return add_neighbour_cell(earfcn, serving_phy, rsrp);
+}
+
+bool rrc::add_neighbour_cell(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
   if (earfcn == 0) {
-    return NULL;
+    earfcn = serving_cell->earfcn;
-  }
-  int idx = find_cell_idx(earfcn, phy_cell.id);
-  if (idx >= 0) {
-    known_cells[idx].rsrp = rsrp;
-    return &known_cells[idx];
   }
 
-  // if does not exist, find empty slot
+  // First check if already exists
-  int i=0;
+  int cell_idx = find_neighbour_cell(earfcn, phy_cell.id);
-  while(i<MAX_KNOWN_CELLS && known_cells[i].earfcn) {
+
-    i++;
+  rrc_log->info("Adding PCI=%d, earfcn=%d, cell_idx=%d\n", phy_cell.id, earfcn, cell_idx);
-  }
+
-  if (i==MAX_KNOWN_CELLS) {
+  // If exists, update RSRP, sort again and return
-    rrc_log->error("Can't add more cells\n");
+  if (cell_idx >= 0) {
-    return NULL;
+    neighbour_cells[cell_idx]->rsrp = rsrp;
+    sort_neighbour_cells();
+    return true; 
   }
 
-  known_cells[i].phy_cell = phy_cell;
+  // If not, create a new one
-  known_cells[i].rsrp = rsrp;
+  cell_t *new_cell = new cell_t(phy_cell, earfcn, rsrp);
-  known_cells[i].earfcn = earfcn;
+
-  known_cells[i].has_valid_sib1 = false;
+  return add_neighbour_cell(new_cell);
-  known_cells[i].has_valid_sib2 = false;
-  known_cells[i].has_valid_sib3 = false;
-  return &known_cells[i];
 }
 
-void rrc::add_neighbour_cell(uint32_t earfcn, uint32_t pci, float rsrp) {
+int rrc::find_neighbour_cell(uint32_t earfcn, uint32_t pci) {
-  int idx = find_cell_idx(earfcn, pci);
+  for (uint32_t i = 0; i < neighbour_cells.size(); i++) {
-  if (idx >= 0) {
+    if (neighbour_cells[i]->equals(earfcn, pci)) {
-    known_cells[idx].rsrp = rsrp;
-    return;
-  }
-
-  rrc_log->info("Added neighbour cell earfcn=%d, pci=%d, rsrp=%f\n", earfcn, pci, rsrp);
-
-  srslte_cell_t cell;
-  cell = current_cell->phy_cell;
-  cell.id = pci;
-  add_new_cell(earfcn, cell, rsrp);
-}
-
-int rrc::find_cell_idx(uint32_t earfcn, uint32_t pci) {
-  for (uint32_t i = 0; i < MAX_KNOWN_CELLS; i++) {
-    if (earfcn == known_cells[i].earfcn && pci == known_cells[i].phy_cell.id) {
       return (int) i;
     }
   }
@@ -720,24 +771,28 @@ float rrc::get_squal(float Qqualmeas) {
 *
 *******************************************************************************/
 
-// Detection of physical layer problems (5.3.11.1)
+// Detection of physical layer problems in RRC_CONNECTED (5.3.11.1)
 void rrc::out_of_sync() {
-  current_cell->in_sync = false;
+  serving_cell->in_sync = false;
-  if (!mac_timers->timer_get(t311)->is_running() && !mac_timers->timer_get(t310)->is_running()) {
+  if (state == RRC_STATE_CONNECTED) {
-    n310_cnt++;
+    if (!mac_timers->timer_get(t311)->is_running() && !mac_timers->timer_get(t310)->is_running()) {
-    if (n310_cnt == N310) {
+      n310_cnt++;
-      mac_timers->timer_get(t310)->reset();
+      if (n310_cnt == N310) {
-      mac_timers->timer_get(t310)->run();
+        mac_timers->timer_get(t310)->reset();
-      n310_cnt = 0;
+        mac_timers->timer_get(t310)->run();
-      phy->sync_reset();
+        n310_cnt = 0;
-      rrc_log->info("Detected %d out-of-sync from PHY. Trying to resync. Starting T310 timer\n", N310);
+        phy->sync_reset();
+        rrc_log->info("Detected %d out-of-sync from PHY. Trying to resync. Starting T310 timer\n", N310);
+      }
     }
+  } else {
+    phy->sync_reset();
   }
 }
 
 // Recovery of physical layer problems (5.3.11.2)
 void rrc::in_sync() {
-  current_cell->in_sync = true;
+  serving_cell->in_sync = true;
   if (mac_timers->timer_get(t310)->is_running()) {
     n311_cnt++;
     if (n311_cnt == N311) {
@@ -861,7 +916,7 @@ void rrc::send_con_restablish_request(LIBLTE_RRC_CON_REEST_REQ_CAUSE_ENUM cause,
   uint8_t *msg_ptr = varShortMAC;
 
   // ASN.1 encode byte-aligned VarShortMAC-Input
-  liblte_rrc_pack_cell_identity_ie(current_cell->sib1.cell_id, &msg_ptr);
+  liblte_rrc_pack_cell_identity_ie(serving_cell->sib1.cell_id, &msg_ptr);
   msg_ptr = &varShortMAC[4];
   liblte_rrc_pack_phys_cell_id_ie(phy->get_current_pci(), &msg_ptr);
   msg_ptr = &varShortMAC[4+2];
@@ -869,7 +924,7 @@ void rrc::send_con_restablish_request(LIBLTE_RRC_CON_REEST_REQ_CAUSE_ENUM cause,
   srslte_bit_pack_vector(varShortMAC, varShortMAC_packed, (4+2+4)*8);
 
   rrc_log->info("Generated varShortMAC: cellId=0x%x, PCI=%d, rnti=%d\n",
-                current_cell->sib1.cell_id, phy->get_current_pci(), crnti);
+                serving_cell->sib1.cell_id, phy->get_current_pci(), crnti);
 
   // Compute MAC-I
   uint8_t mac_key[4];
@@ -958,6 +1013,8 @@ void rrc::send_con_setup_complete(byte_buffer_t *nas_msg) {
   memcpy(ul_dcch_msg.msg.rrc_con_setup_complete.dedicated_info_nas.msg, nas_msg->msg, nas_msg->N_bytes);
   ul_dcch_msg.msg.rrc_con_setup_complete.dedicated_info_nas.N_bytes = nas_msg->N_bytes;
 
+  pool->deallocate(nas_msg);
+  
   send_ul_dcch_msg();
 }
 
@@ -995,9 +1052,9 @@ bool rrc::ho_prepare() {
   if (pending_mob_reconf) {
     rrc_log->info("Processing HO command to target PCell=%d\n", mob_reconf.mob_ctrl_info.target_pci);
 
-    int cell_idx = find_cell_idx(phy->get_current_earfcn(), mob_reconf.mob_ctrl_info.target_pci);
+    int target_cell_idx = find_neighbour_cell(serving_cell->earfcn, mob_reconf.mob_ctrl_info.target_pci);
-    if (cell_idx < 0) {
+    if (target_cell_idx < 0) {
-      rrc_log->error("Could not find target cell pci=%d\n", mob_reconf.mob_ctrl_info.target_pci);
+      rrc_log->error("Could not find target cell earfcn=%d, pci=%d\n", serving_cell->earfcn, mob_reconf.mob_ctrl_info.target_pci);
       return false;
     }
 
@@ -1005,16 +1062,12 @@ bool rrc::ho_prepare() {
     mac_timers->timer_get(t310)->stop();
     mac_timers->timer_get(t304)->set(this, liblte_rrc_t304_num[mob_reconf.mob_ctrl_info.t304]);
     if (mob_reconf.mob_ctrl_info.carrier_freq_eutra_present &&
-        mob_reconf.mob_ctrl_info.carrier_freq_eutra.dl_carrier_freq != current_cell->earfcn) {
+        mob_reconf.mob_ctrl_info.carrier_freq_eutra.dl_carrier_freq != serving_cell->earfcn) {
       rrc_log->warning("Received mobilityControlInfo for inter-frequency handover\n");
     }
 
-    // Save cell and current configuration
+    // Save serving cell and current configuration
-    ho_src_cell_idx = find_cell_idx(phy->get_current_earfcn(), phy->get_current_pci());
+    ho_src_cell = *serving_cell;
-    if (ho_src_cell_idx < 0) {
-      rrc_log->error("Source cell not found in known cells. Reconnecting to cell 0 in case of failure\n");
-      ho_src_cell_idx = 0;
-    }
     phy->get_config(&ho_src_phy_cfg);
     mac->get_config(&ho_src_mac_cfg);
     mac_interface_rrc::ue_rnti_t uernti;
@@ -1031,9 +1084,9 @@ bool rrc::ho_prepare() {
     mac->set_ho_rnti(mob_reconf.mob_ctrl_info.new_ue_id, mob_reconf.mob_ctrl_info.target_pci);
     apply_rr_config_common_dl(&mob_reconf.mob_ctrl_info.rr_cnfg_common);
 
-    rrc_log->info("Selecting new cell pci=%d\n", known_cells[cell_idx].phy_cell.id);
+    rrc_log->info("Selecting new cell pci=%d\n", neighbour_cells[target_cell_idx]->phy_cell.id);
-    if (!phy->cell_handover(known_cells[cell_idx].phy_cell)) {
+    if (!phy->cell_handover(neighbour_cells[target_cell_idx]->phy_cell)) {
-      rrc_log->error("Could not synchronize with target cell pci=%d\n", known_cells[cell_idx].phy_cell.id);
+      rrc_log->error("Could not synchronize with target cell pci=%d\n", neighbour_cells[target_cell_idx]->phy_cell.id);
       return false;
     }
 
@@ -1108,8 +1161,8 @@ void rrc::ho_ra_completed(bool ra_successful) {
 void rrc::ho_failed() {
 
   // Instruct PHY to resync with source PCI
-  if (!phy->cell_handover(known_cells[ho_src_cell_idx].phy_cell)) {
+  if (!phy->cell_handover(ho_src_cell.phy_cell)) {
-    rrc_log->error("Could not synchronize with target cell pci=%d\n", known_cells[ho_src_cell_idx].phy_cell.id);
+    rrc_log->error("Could not synchronize with target cell pci=%d\n", ho_src_cell.phy_cell.id);
     return;
   }
 
@@ -1163,14 +1216,37 @@ void rrc::handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGU
   }
 }
 
-  /* Actions upon reception of RRCConnectionRelease 5.3.8.3 */
+/* Actions upon reception of RRCConnectionRelease 5.3.8.3 */
 void rrc::rrc_connection_release() {
   // Save idleModeMobilityControlInfo, etc.
   state = RRC_STATE_LEAVE_CONNECTED;
   rrc_log->console("Received RRC Connection Release\n");
 }
 
-
+/* Actions upon leaving RRC_CONNECTED 5.3.12 */
+void rrc::leave_connected()
+{
+  rrc_log->console("RRC IDLE\n");
+  rrc_log->info("Leaving RRC_CONNECTED state\n");
+  drb_up = false;
+  measurements.reset();
+  pdcp->reset();
+  rlc->reset();
+  phy->reset();
+  mac->reset();
+  set_phy_default();
+  set_mac_default();
+  mac_timers->timer_get(t301)->stop();
+  mac_timers->timer_get(t310)->stop();
+  mac_timers->timer_get(t311)->stop();
+  mac_timers->timer_get(t304)->stop();
+  if (phy->sync_status()) {
+    // Instruct MAC to look for P-RNTI
+    mac->pcch_start_rx();
+    // Instruct PHY to measure serving cell for cell reselection
+    phy->meas_start(phy->get_current_earfcn(), phy->get_current_pci());
+  }
+}
 
 
 
@@ -1211,24 +1287,24 @@ void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu) {
     rrc_log->info("Processing SIB: %d\n", liblte_rrc_sys_info_block_type_num[dlsch_msg.sibs[i].sib_type]);
 
     if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1 == dlsch_msg.sibs[i].sib_type && SI_ACQUIRE_SIB1 == si_acquire_state) {
-      memcpy(&current_cell->sib1, &dlsch_msg.sibs[i].sib.sib1, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT));
+      memcpy(&serving_cell->sib1, &dlsch_msg.sibs[i].sib.sib1, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT));
-      current_cell->has_valid_sib1 = true;
+      serving_cell->has_valid_sib1 = true;
       handle_sib1();
-    } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2 == dlsch_msg.sibs[i].sib_type && !current_cell->has_valid_sib2) {
+    } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2 == dlsch_msg.sibs[i].sib_type && !serving_cell->has_valid_sib2) {
-      memcpy(&current_cell->sib2, &dlsch_msg.sibs[i].sib.sib2, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT));
+      memcpy(&serving_cell->sib2, &dlsch_msg.sibs[i].sib.sib2, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT));
-      current_cell->has_valid_sib2 = true;
+      serving_cell->has_valid_sib2 = true;
       handle_sib2();
-    } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_3 == dlsch_msg.sibs[i].sib_type && !current_cell->has_valid_sib3) {
+    } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_3 == dlsch_msg.sibs[i].sib_type && !serving_cell->has_valid_sib3) {
-      memcpy(&current_cell->sib3, &dlsch_msg.sibs[i].sib.sib3, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_3_STRUCT));
+      memcpy(&serving_cell->sib3, &dlsch_msg.sibs[i].sib.sib3, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_3_STRUCT));
-      current_cell->has_valid_sib3 = true;
+      serving_cell->has_valid_sib3 = true;
       handle_sib3();
-    }else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13 == dlsch_msg.sibs[i].sib_type && !current_cell->has_valid_sib13) {
+    }else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13 == dlsch_msg.sibs[i].sib_type && !serving_cell->has_valid_sib13) {
-      memcpy(&current_cell->sib13, &dlsch_msg.sibs[0].sib.sib13, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13_STRUCT));
+      memcpy(&serving_cell->sib13, &dlsch_msg.sibs[0].sib.sib13, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_13_STRUCT));
-      current_cell->has_valid_sib13 = true;
+      serving_cell->has_valid_sib13 = true;
       handle_sib13();
     }
   }
-  if(current_cell->has_valid_sib2) {
+  if(serving_cell->has_valid_sib2) {
     sysinfo_index++;
   }
 }
@@ -1236,16 +1312,16 @@ void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu) {
 void rrc::handle_sib1()
 {
   rrc_log->info("SIB1 received, CellID=%d, si_window=%d, sib2_period=%d\n",
-                current_cell->sib1.cell_id&0xfff,
+                serving_cell->sib1.cell_id&0xfff,
-                liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length],
+                liblte_rrc_si_window_length_num[serving_cell->sib1.si_window_length],
-                liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity]);
+                liblte_rrc_si_periodicity_num[serving_cell->sib1.sched_info[0].si_periodicity]);
 
   // Print SIB scheduling info
   uint32_t i,j;
-  for(i=0;i<current_cell->sib1.N_sched_info;i++){
+  for(i=0;i<serving_cell->sib1.N_sched_info;i++){
-    for(j=0;j<current_cell->sib1.sched_info[i].N_sib_mapping_info;j++){
+    for(j=0;j<serving_cell->sib1.sched_info[i].N_sib_mapping_info;j++){
-      LIBLTE_RRC_SIB_TYPE_ENUM t       = current_cell->sib1.sched_info[i].sib_mapping_info[j].sib_type;
+      LIBLTE_RRC_SIB_TYPE_ENUM t       = serving_cell->sib1.sched_info[i].sib_mapping_info[j].sib_type;
-      LIBLTE_RRC_SI_PERIODICITY_ENUM p = current_cell->sib1.sched_info[i].si_periodicity;
+      LIBLTE_RRC_SI_PERIODICITY_ENUM p = serving_cell->sib1.sched_info[i].si_periodicity;
       rrc_log->debug("SIB scheduling info, sib_type=%d, si_periodicity=%d\n",
                     liblte_rrc_sib_type_num[t],
                     liblte_rrc_si_periodicity_num[p]);
@@ -1253,16 +1329,16 @@ void rrc::handle_sib1()
   }
 
   // Set TDD Config
-  if(current_cell->sib1.tdd) {
+  if(serving_cell->sib1.tdd) {
-    phy->set_config_tdd(&current_cell->sib1.tdd_cnfg);
+    phy->set_config_tdd(&serving_cell->sib1.tdd_cnfg);
   }
 
-  current_cell->has_valid_sib1 = true;
+  serving_cell->has_valid_sib1 = true;
 
   // Send PLMN and TAC to NAS
   std::stringstream ss;
-  for (uint32_t i = 0; i < current_cell->sib1.N_plmn_ids; i++) {
+  for (uint32_t i = 0; i < serving_cell->sib1.N_plmn_ids; i++) {
-    nas->plmn_found(current_cell->sib1.plmn_id[i].id, current_cell->sib1.tracking_area_code);
+    nas->plmn_found(serving_cell->sib1.plmn_id[i].id, serving_cell->sib1.tracking_area_code);
   }
 
   // Jump to next state
@@ -1285,7 +1361,7 @@ void rrc::handle_sib2()
 {
   rrc_log->info("SIB2 received\n");
 
-  apply_sib2_configs(&current_cell->sib2);
+  apply_sib2_configs(&serving_cell->sib2);
 
 }
 
@@ -1293,7 +1369,7 @@ void rrc::handle_sib3()
 {
   rrc_log->info("SIB3 received\n");
 
-  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_3_STRUCT *sib3 = &current_cell->sib3;
+  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_3_STRUCT *sib3 = &serving_cell->sib3;
 
   // cellReselectionInfoCommon
   cell_resel_cfg.q_hyst = liblte_rrc_q_hyst_num[sib3->q_hyst];
@@ -1315,9 +1391,9 @@ void rrc::handle_sib13()
 {
   rrc_log->info("SIB13 received\n");
 
-//  mac->set_config_mbsfn_sib13(&current_cell->sib13.mbsfn_area_info_list_r9[0],
+//  mac->set_config_mbsfn_sib13(&serving_cell->sib13.mbsfn_area_info_list_r9[0],
-//                              current_cell->sib13.mbsfn_area_info_list_r9_size,
+//                              serving_cell->sib13.mbsfn_area_info_list_r9_size,
-//                              &current_cell->sib13.mbsfn_notification_config);
+//                              &serving_cell->sib13.mbsfn_notification_config);
 }
 
 
@@ -1497,7 +1573,7 @@ void rrc::parse_dl_ccch(byte_buffer_t *pdu) {
     case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REJ:
       rrc_log->info("Connection Reject received. Wait time: %d\n",
                     dl_ccch_msg.msg.rrc_con_rej.wait_time);
-      state = RRC_STATE_IDLE;
+      state = RRC_STATE_LEAVE_CONNECTED;
       break;
     case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_SETUP:
       rrc_log->info("Connection Setup received\n");
@@ -1605,7 +1681,7 @@ void rrc::parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu) {
 *
 *******************************************************************************/
 void rrc::enable_capabilities() {
-  bool enable_ul_64 = args.ue_category >= 5 && current_cell->sib2.rr_config_common_sib.pusch_cnfg.enable_64_qam;
+  bool enable_ul_64 = args.ue_category >= 5 && serving_cell->sib2.rr_config_common_sib.pusch_cnfg.enable_64_qam;
   rrc_log->info("%s 64QAM PUSCH\n", enable_ul_64 ? "Enabling" : "Disabling");
   phy->set_config_64qam_en(enable_ul_64);
 }
@@ -2270,14 +2346,20 @@ void rrc::rrc_meas::L3_filter(meas_value_t *value, float values[NOF_MEASUREMENTS
 void rrc::rrc_meas::new_phy_meas(uint32_t earfcn, uint32_t pci, float rsrp, float rsrq, uint32_t tti)
 {
   float values[NOF_MEASUREMENTS] = {rsrp, rsrq};
+
   // This indicates serving cell
-  if (earfcn == 0) {
+  if (parent->serving_cell->equals(earfcn, pci)) {
 
     log_h->info("MEAS:  New measurement serving cell, rsrp=%f, rsrq=%f, tti=%d\n", rsrp, rsrq, tti);
 
     L3_filter(&pcell_measurement, values);
+
+    // Update serving cell measurement
+    parent->serving_cell->rsrp = rsrp;
+
   } else {
-    // Add to known cells
+
+    // Add to list of neighbour cells
     parent->add_neighbour_cell(earfcn, pci, rsrp);
 
     log_h->info("MEAS:  New measurement earfcn=%d, pci=%d, rsrp=%f, rsrq=%f, tti=%d\n", earfcn, pci, rsrp, rsrq, tti);
@@ -2297,8 +2379,6 @@ void rrc::rrc_meas::new_phy_meas(uint32_t earfcn, uint32_t pci, float rsrp, floa
         return;
       }
     }
-
-    parent->rrc_log->warning("MEAS:  Received measurement from unknown EARFCN=%d\n", earfcn);
   }
 }
 
@@ -2349,8 +2429,8 @@ void rrc::rrc_meas::generate_report(uint32_t meas_id)
   report->pcell_rsrp_result = value_to_range(RSRP, pcell_measurement.ms[RSRP]);
   report->pcell_rsrq_result = value_to_range(RSRQ, pcell_measurement.ms[RSRQ]);
 
-  log_h->console("MEAS:  Generate report MeasId=%d, rsrp=%f rsrq=%f\n",
+  log_h->info("MEAS:  Generate report MeasId=%d, nof_reports_send=%d, Pcell rsrp=%f rsrq=%f\n",
-              report->meas_id, pcell_measurement.ms[RSRP], pcell_measurement.ms[RSRQ]);
+              report->meas_id, m->nof_reports_sent, pcell_measurement.ms[RSRP], pcell_measurement.ms[RSRQ]);
 
   // TODO: report up to 8 best cells
   for (std::map<uint32_t, meas_value_t>::iterator cell = m->cell_values.begin(); cell != m->cell_values.end(); ++cell)
@@ -2365,7 +2445,7 @@ void rrc::rrc_meas::generate_report(uint32_t meas_id)
       rc->meas_result.rsrp_result = value_to_range(RSRP, cell->second.ms[RSRP]);
       rc->meas_result.rsrq_result = value_to_range(RSRQ, cell->second.ms[RSRQ]);
 
-      log_h->info("MEAS:  Add neigh=%d, pci=%d, rsrp=%f, rsrq=%f\n",
+      log_h->info("MEAS:  Adding to report neighbour=%d, pci=%d, rsrp=%f, rsrq=%f\n",
                      report->meas_result_neigh_cells.eutra.n_result, rc->phys_cell_id,
                      cell->second.ms[RSRP], cell->second.ms[RSRQ]);
 
@@ -2585,10 +2665,14 @@ void rrc::rrc_meas::remove_meas_report(uint32_t report_id) {
 }
 
 void rrc::rrc_meas::remove_meas_id(uint32_t measId) {
-  mac_timers->timer_get(active[measId].periodic_timer)->stop();
+  if (active.count(measId)) {
-  mac_timers->timer_release_id(active[measId].periodic_timer);
+    mac_timers->timer_get(active[measId].periodic_timer)->stop();
-  log_h->info("MEAS: Removed measId=%d\n", measId);
+    mac_timers->timer_release_id(active[measId].periodic_timer);
-  active.erase(measId);
+    log_h->info("MEAS: Removed measId=%d\n", measId);
+    active.erase(measId);
+  } else {
+    log_h->warning("MEAS: Removing unexistent measId=%d\n", measId);
+  }
 }
 
 void rrc::rrc_meas::remove_meas_id(std::map<uint32_t, meas_t>::iterator it) {
@@ -2726,15 +2810,17 @@ void rrc::rrc_meas::parse_meas_config(LIBLTE_RRC_MEAS_CONFIG_STRUCT *cfg)
     for (uint32_t i=0;i<cfg->meas_id_to_add_mod_list.N_meas_id;i++) {
       LIBLTE_RRC_MEAS_ID_TO_ADD_MOD_STRUCT *measId = &cfg->meas_id_to_add_mod_list.meas_id_list[i];
       // Stop the timer if the entry exists or create the timer if not
+      bool is_new = false;
       if (active.count(measId->meas_id)) {
         mac_timers->timer_get(active[measId->meas_id].periodic_timer)->stop();
       } else {
+        is_new = true;
         active[measId->meas_id].periodic_timer   = mac_timers->timer_get_unique_id();
       }
       active[measId->meas_id].object_id = measId->meas_obj_id;
       active[measId->meas_id].report_id = measId->rep_cnfg_id;
-      log_h->info("MEAS: Added measId=%d, measObjectId=%d, reportConfigId=%d\n",
+      log_h->info("MEAS: %s measId=%d, measObjectId=%d, reportConfigId=%d\n",
-                  measId->meas_id, measId->meas_obj_id, measId->rep_cnfg_id);
+                  is_new?"Added":"Updated", measId->meas_id, measId->meas_obj_id, measId->rep_cnfg_id);
     }
   }
 

srsue/src/upper/usim.cc

@@ -53,13 +53,6 @@ void usim::init(usim_args_t *args, srslte::log *usim_log_)
     usim_log->console("Invalid length for OP: %d should be %d", args->op.length(), 32);
   }
 
-  if(4 == args->amf.length()) {
-    str_to_hex(args->amf, amf);
-  } else {
-    usim_log->error("Invalid length for AMF: %d should be %d", args->amf.length(), 4);
-    usim_log->console("Invalid length for AMF: %d should be %d", args->amf.length(), 4);
-  }
-
   if(15 == args->imsi.length()) {
     imsi = 0;
     for(i=0; i<15; i++)
@@ -356,6 +349,11 @@ void usim::gen_auth_res_milenage( uint8_t  *rand,
   {
     sqn[i] = autn_enb[i] ^ ak[i];
   }
+  // Extract AMF from autn
+  for(int i=0;i<2;i++)
+  {
+    amf[i]=autn_enb[6+i];
+  }
 
   // Generate MAC
   security_milenage_f1( k,
@@ -431,6 +429,10 @@ void usim::gen_auth_res_xor(uint8_t  *rand,
   for(i=0;i<6;i++) {
     sqn[i] = autn_enb[i] ^ ak[i];
   }
+  // Extract AMF from autn
+  for(int i=0;i<2;i++){
+      amf[i]=autn_enb[6+i];
+  }
 
   // Generate cdout
   for(i=0; i<6; i++) {

srsue/test/upper/nas_test.cc

@@ -117,7 +117,6 @@ int security_command_test()
 
   usim_args_t args;
   args.algo = "xor";
-  args.amf = "9001";
   args.imei = "353490069873319";
   args.imsi = "001010123456789";
   args.k = "00112233445566778899aabbccddeeff";
@@ -179,7 +178,6 @@ int mme_attach_request_test()
   srsue::usim usim;
   usim_args_t args;
   args.algo = "xor";
-  args.amf = "9001";
   args.imei = "353490069873319";
   args.imsi = "001010123456789";
   args.k = "00112233445566778899aabbccddeeff";

srsue/test/upper/usim_test.cc

@@ -74,7 +74,6 @@ int main(int argc, char **argv)
 
   usim_args_t args;
   args.algo = "milenage";
-  args.amf = "8000";
   args.imei = "35609204079301";
   args.imsi = "208930000000001";
   args.k = "8BAF473F2F8FD09487CCCBD7097C6862";

srsue/ue.conf.example

@@ -93,7 +93,6 @@ file_max_size = -1
 [usim]
 algo = xor
 op   = 63BFA50EE6523365FF14C1F45F88737D
-amf  = 9001
 k    = 00112233445566778899aabbccddeeff
 imsi = 001010123456789
 imei = 353490069873319
@@ -198,7 +197,7 @@ enable = false
 #sfo_correct_disable = false
 #sss_algorithm       = full
 #estimator_fil_w     = 0.1
-#average_subframe_enabled = false
+#average_subframe_enabled = true
 #sic_pss_enabled     = true
 #pregenerate_signals = false
 #metrics_csv_enable  = false