Merge branch 'master' of https://github.com/srsLTE/srsLTE

CHANGELOG

@@ -1,6 +1,9 @@
 Change Log for Releases
 ==============================
 
+## 18.03
+  * Many bug-fixes and improved stability and performance in all parts
+
 ## 17.12
   * Added support for MIMO 2x2 in srsENB (i.e. TM3/TM4)
   * Added srsEPC, a light-weight core network implementation

CMakeLists.txt

@@ -41,8 +41,6 @@ include(SRSLTEVersion) #sets version information
 include(SRSLTEPackage) #setup cpack
 
 include(CTest)
-set(CTEST_MEMORYCHECK_COMMAND valgrind)
-set(CTEST_MEMORYCHECK_COMMAND_OPTIONS "--trace-children=yes --leak-check=full" )
 
 configure_file(
     "${CMAKE_CURRENT_SOURCE_DIR}/CTestCustom.cmake.in"
@@ -74,6 +72,7 @@ option(ENABLE_BLADERF  "Enable BladeRF"                           ON)
 
 option(BUILD_STATIC    "Attempt to statically link external deps" OFF)
 option(RPATH           "Enable RPATH"                             OFF)
+option(ENABLE_ASAN     "Enable gcc address sanitizer"             OFF)
 
 option(USE_LTE_RATES   "Use standard LTE sampling rates"          OFF)
 
@@ -170,7 +169,6 @@ if(ENABLE_SRSUE OR ENABLE_SRSENB OR ENABLE_SRSEPC)
 
   set(BOOST_REQUIRED_COMPONENTS
       program_options
-      system
   )
   if(UNIX AND EXISTS "/usr/lib64")
       list(APPEND BOOST_LIBRARYDIR "/usr/lib64") #fedora 64-bit fix
@@ -228,13 +226,7 @@ macro(ADD_CXX_COMPILER_FLAG_IF_AVAILABLE flag have)
 endmacro(ADD_CXX_COMPILER_FLAG_IF_AVAILABLE)
 
 if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=${GCC_ARCH} -Wall -Wno-comment -Wno-reorder -Wno-unused-but-set-variable -Wno-unused-variable -std=c++03")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=${GCC_ARCH} -Wall -Wno-comment -Wno-reorder -Wno-unused-but-set-variable -Wno-unused-variable -Wformat -std=c++03")
-
-  if(${CMAKE_BUILD_TYPE} STREQUAL "Debug")
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -DDEBUG_MODE")
-  else(${CMAKE_BUILD_TYPE} STREQUAL "Debug")
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
-  endif(${CMAKE_BUILD_TYPE} STREQUAL "Debug")
 
   find_package(SSE)
   if (HAVE_AVX2)
@@ -254,9 +246,16 @@ if(CMAKE_C_COMPILER_ID MATCHES "GNU" OR CMAKE_C_COMPILER_ID MATCHES "Clang")
   set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=${GCC_ARCH} -Wall -Wno-comment -Wno-write-strings -Wno-format-extra-args -Winline -Wno-unused-result -Wno-format -std=c99 -D_GNU_SOURCE")
 
   if(${CMAKE_BUILD_TYPE} STREQUAL "Debug")
-    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -g -O0 -DDEBUG_MODE")
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -ggdb -O0 -DDEBUG_MODE -DBUILD_TYPE_DEBUG")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ggdb -O0 -DDEBUG_MODE -DBUILD_TYPE_DEBUG")
   else(${CMAKE_BUILD_TYPE} STREQUAL "Debug")
-    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3")
+    if(${CMAKE_BUILD_TYPE} STREQUAL "RelWithDebInfo")
+      set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -ggdb -DBUILD_TYPE_RELWITHDEBINFO")
+      set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ggdb -DBUILD_TYPE_RELWITHDEBINFO")
+    else(${CMAKE_BUILD_TYPE} STREQUAL "RelWithDebInfo")
+      set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -DBUILD_TYPE_RELEASE")
+      set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -DBUILD_TYPE_RELEASE")
+    endif(${CMAKE_BUILD_TYPE} STREQUAL "RelWithDebInfo")
   endif(${CMAKE_BUILD_TYPE} STREQUAL "Debug")
 
   if (USE_LTE_RATES)
@@ -303,6 +302,10 @@ if(CMAKE_C_COMPILER_ID MATCHES "GNU" OR CMAKE_C_COMPILER_ID MATCHES "Clang")
   if(NOT WIN32)
       ADD_CXX_COMPILER_FLAG_IF_AVAILABLE(-fvisibility=hidden HAVE_VISIBILITY_HIDDEN)
   endif(NOT WIN32)
+ if (ENABLE_ASAN)
+   set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fsanitize=address")
+   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address")
+ endif (ENABLE_ASAN)
 endif(CMAKE_C_COMPILER_ID MATCHES "GNU" OR CMAKE_C_COMPILER_ID MATCHES "Clang")
 
 if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")

CTestConfig.cmake

@@ -11,3 +11,4 @@ set(CTEST_DROP_METHOD "http")
 set(CTEST_DROP_SITE "my.cdash.org")
 set(CTEST_DROP_LOCATION "/submit.php?project=srsLTE")
 set(CTEST_DROP_SITE_CDASH TRUE)
+set(VALGRIND_COMMAND_OPTIONS "--trace-children=yes --leak-check=full --show-reachable=yes --vex-guest-max-insns=25")

README.md

@@ -102,7 +102,7 @@ Build Instructions
 
 For example, on Ubuntu 17.04, one can install the required libraries with:
 ```
-sudo apt-get install cmake libfftw3-dev libmbedtls-dev libboost-program-options-dev libboost-thread-dev libconfig++-dev libsctp-dev
+sudo apt-get install cmake libfftw3-dev libmbedtls-dev libboost-program-options-dev libconfig++-dev libsctp-dev
 ```
 Note that depending on your flavor and version of Linux, the actual package names may be different.
 

cmake/modules/SRSLTEVersion.cmake

@@ -18,7 +18,7 @@
 # and at http://www.gnu.org/licenses/.
 #
 
-SET(SRSLTE_VERSION_MAJOR 17)
+SET(SRSLTE_VERSION_MAJOR 18)
-SET(SRSLTE_VERSION_MINOR 12)
+SET(SRSLTE_VERSION_MINOR 3)
 SET(SRSLTE_VERSION_PATCH 0)
 SET(SRSLTE_VERSION_STRING "${SRSLTE_VERSION_MAJOR}.${SRSLTE_VERSION_MINOR}.${SRSLTE_VERSION_PATCH}")

lib/examples/pdsch_enodeb.c

@@ -84,6 +84,8 @@ int mbsfn_area_id = -1;
 char *rf_args = "";
 float rf_amp = 0.8, rf_gain = 70.0, rf_freq = 2400000000;
 
+float output_file_snr = +INFINITY;
+
 bool null_file_sink=false; 
 srslte_filesink_t fsink;
 srslte_ofdm_t ifft[SRSLTE_MAX_PORTS];
@@ -145,13 +147,14 @@ void usage(char *prog) {
   printf("\t-w Number of codewords/layers (multiplex mode only)* [Default %d]\n", multiplex_nof_layers);
   printf("\t-u listen TCP port for input data (-1 is random) [Default %d]\n", net_port);
   printf("\t-v [set srslte_verbose to debug, default none]\n");
+  printf("\t-s output file SNR [Default %f]\n", output_file_snr);
   printf("\n");
   printf("\t*: See 3GPP 36.212 Table  5.3.3.1.5-4 for more information\n");
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "aglfmoncpvutxbwM")) != -1) {
+  while ((opt = getopt(argc, argv, "aglfmoncpvutxbwMs")) != -1) {
 
     switch (opt) {
     case 'a':
@@ -200,6 +203,9 @@ void parse_args(int argc, char **argv) {
     case 'v':
       srslte_verbose++;
       break;
+    case 's':
+      output_file_snr = atof(argv[optind]);
+      break;
     default:
       usage(argv[0]);
       exit(-1);
@@ -989,7 +995,14 @@ int main(int argc, char **argv) {
       /* send to file or usrp */
       if (output_file_name) {
         if (!null_file_sink) {
-           srslte_filesink_write_multi(&fsink, (void**) output_buffer, sf_n_samples, cell.nof_ports);       
+          /* Apply AWGN */
+          if (output_file_snr != +INFINITY) {
+            float var = powf(10.0f, -(output_file_snr + 3.0f) / 20.0f);
+            for (int k = 0; k < cell.nof_ports; k++) {
+              srslte_ch_awgn_c(output_buffer[k], output_buffer[k], var, sf_n_samples);
+            }
+          }
+          srslte_filesink_write_multi(&fsink, (void**) output_buffer, sf_n_samples, cell.nof_ports);
         }
         usleep(1000);
       } else {

lib/examples/pdsch_ue.c

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

lib/include/srslte/asn1/gtpc.h

@@ -52,16 +52,15 @@ const uint8_t GTPC_V2 = 2;
  * n+2    |           Sequence            |
  * n+3    |            Spare              |
  ***************************************************************************/
-
+typedef struct gtpc_header
-  typedef struct gtpc_header
+{
-  {
+  uint8_t version;
-    uint8_t version;
+  bool piggyback;
-    bool piggyback;
+  bool teid_present;
-    bool teid_present;
+  uint8_t type;
-    uint8_t type;
+  uint64_t teid;
-    uint64_t teid;
+  uint64_t sequence;
-    uint64_t sequence;
+}gtpc_header_t;
-  } gtpc_header_t;
 
 /****************************************************************************
  * GTP-C v2 Payload
@@ -69,16 +68,17 @@ const uint8_t GTPC_V2 = 2;
  *
  * Union that hold the different structures for the possible message types.
  ***************************************************************************/
-
 typedef union gtpc_msg_choice
 {
   struct gtpc_create_session_request create_session_request;
   struct gtpc_create_session_response create_session_response;
   struct gtpc_modify_bearer_request modify_bearer_request;
   struct gtpc_modify_bearer_response modify_bearer_response;
+  struct gtpc_release_access_bearers_request release_access_bearers_request;
+  struct gtpc_release_access_bearers_response release_access_bearers_response;
   struct gtpc_delete_session_request delete_session_request;
   struct gtpc_delete_session_response delete_session_response;
-} gtpc_msg_choice_t;
+}gtpc_msg_choice_t;
 
 /****************************************************************************
  * GTP-C v2 Message
@@ -88,15 +88,10 @@ typedef union gtpc_msg_choice
  * of one GTP-C header and one union of structures, which can hold
  * all the possible GTP-C messages
  ***************************************************************************/
-
 typedef struct gtpc_pdu
 {
   struct gtpc_header header;
   union gtpc_msg_choice choice;
-} gtpc_pdu_t;
+}gtpc_pdu_t;
-
+}//namespace
-
-
-};
-
 #endif

lib/include/srslte/asn1/gtpc_ies.h

@@ -400,7 +400,8 @@ enum gtpc_interface_type
   S2B_U_PGW_GTP_U_INTERFACE
 };
 
-struct gtpc_f_teid_ie
+
+typedef struct gtpc_f_teid_ie
 {
   bool ipv4_present;
   bool ipv6_present;
@@ -408,7 +409,7 @@ struct gtpc_f_teid_ie
   uint32_t teid;
   in_addr_t  ipv4;
   struct in6_addr ipv6; //FIXME
-};
+} gtp_fteid_t;
 
 //TODO
 //TODO IEs between 8.22 and 8.28 missing

lib/include/srslte/asn1/gtpc_msg.h

@@ -410,5 +410,36 @@ struct gtpc_delete_session_response
   //Private extension
 };
 
+/****************************************************************************
+ *
+ * GTP-C v2 Release Access Bearers Request
+ * Ref: 3GPP TS 29.274 v10.14.0 Table 7.2.21.1-1
+ *
+ ***************************************************************************/
+struct gtpc_release_access_bearers_request
+{
+  bool list_of_rabs_present;
+  //Linked EPS Bearer ID
+  bool originating_node_present;
+  //Indication Flags
+  //Private Extension
+};
+
+  /****************************************************************************
+   *
+   * GTP-C v2 Delete Session Response
+   * Ref: 3GPP TS 29.274 v10.14.0 Table 7.2.22.1-1
+   *
+   ***************************************************************************/
+
+  struct gtpc_release_access_bearers_response
+  {
+    struct gtpc_cause_ie cause;
+    //Recovery
+    //Private extension
+  };
+
+
+
 } //namespace
 #endif //GTPC_V2_MSG_H

lib/include/srslte/asn1/liblte_mme.h

@@ -175,6 +175,7 @@ typedef struct{
     uint8 imsi[15];
     uint8 imei[15];
     uint8 imeisv[16];
+    uint32 tmsi;
 }LIBLTE_MME_MOBILE_ID_STRUCT;
 // Functions
 LIBLTE_ERROR_ENUM liblte_mme_pack_mobile_id_ie(LIBLTE_MME_MOBILE_ID_STRUCT  *mobile_id,

lib/include/srslte/build_info.h.in

@@ -32,10 +32,18 @@
 extern "C" {
 # endif
 
-#ifdef NDEBUG
+#ifdef BUILD_TYPE_RELEASE
-    static char build_mode[] = "Release";
+  static char build_mode[] = "Release";
 #else
+  #ifdef BUILD_TYPE_DEBUG
     static char build_mode[] = "Debug";
+  #else
+    #ifdef BUILD_TYPE_RELWITHDEBINFO
+      static char build_mode[] = "RelWithDebInfo";
+    #else
+      static char build_mode[] = "unknown";
+    #endif
+  #endif
 #endif
 
 // the configured build options for srsLTE

lib/include/srslte/common/buffer_pool.h

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

lib/include/srslte/common/common.h

@@ -63,7 +63,7 @@
 #define SRSLTE_MAX_BUFFER_SIZE_BYTES 12756
 #define SRSLTE_BUFFER_HEADER_OFFSET  1024
 
-//#define SRSLTE_BUFFER_POOL_LOG_ENABLED
+#define SRSLTE_BUFFER_POOL_LOG_ENABLED
 
 #ifdef SRSLTE_BUFFER_POOL_LOG_ENABLED
 #define pool_allocate (pool->allocate(__FUNCTION__))
@@ -96,6 +96,8 @@ static const char error_text[ERROR_N_ITEMS][20] = { "None",
                                                     "Can't start",
                                                     "Already started"};
 
+//#define ENABLE_TIMESTAMP
+
 /******************************************************************************
  * Byte and Bit buffers
  *
@@ -145,35 +147,34 @@ public:
     {
       return msg-buffer;
     }
+    // Returns the remaining space from what is reported to be the length of msg
+    uint32_t get_tailroom()
+    {
+      return (sizeof(buffer) - (msg-buffer) - N_bytes);
+    }
     long get_latency_us()
     {
+#ifdef ENABLE_TIMESTAMP
       if(!timestamp_is_set)
         return 0;
       gettimeofday(&timestamp[2], NULL); 
       get_time_interval(timestamp);
       return timestamp[0].tv_usec;
+#else
+      return 0;
+#endif
     }
     
     void set_timestamp() 
     {
-      gettimeofday(&timestamp[1], NULL); 
+#ifdef ENABLE_TIMESTAMP
-      timestamp_is_set = true; 
+      gettimeofday(&timestamp[1], NULL);
+      timestamp_is_set = true;
+#endif
     }
 
 private:
-  
-  
-  void get_time_interval(struct timeval * tdata) {
-
-    tdata[0].tv_sec = tdata[2].tv_sec - tdata[1].tv_sec;
-    tdata[0].tv_usec = tdata[2].tv_usec - tdata[1].tv_usec;
-    if (tdata[0].tv_usec < 0) {
-      tdata[0].tv_sec--;
-      tdata[0].tv_usec += 1000000;
-    }
-  }
 
-  
     struct timeval timestamp[3];
     bool           timestamp_is_set; 
     byte_buffer_t *next;
@@ -215,15 +216,21 @@ struct bit_buffer_t{
     }
     long get_latency_us()
     {
+#ifdef ENABLE_TIMESTAMP
       if(!timestamp_is_set)
         return 0;
       gettimeofday(&timestamp[2], NULL); 
       return timestamp[0].tv_usec;
+#else
+      return 0;
+#endif
     }
     void set_timestamp() 
     {
-      gettimeofday(&timestamp[1], NULL); 
+#ifdef ENABLE_TIMESTAMP
-      timestamp_is_set = true; 
+      gettimeofday(&timestamp[1], NULL);
+      timestamp_is_set = true;
+#endif
     }
 
 private: 

lib/include/srslte/common/log.h

@@ -84,6 +84,8 @@ public:
     level_text_short = true;
   }
 
+  virtual ~log() {};
+
   // This function shall be called at the start of every tti for printing tti
   void step(uint32_t tti_) {
     tti = tti_;
@@ -120,11 +122,11 @@ public:
   }
 
   // Pure virtual methods for logging
-  virtual void console(std::string message, ...) = 0;
+  virtual void console(const char * message, ...) __attribute__ ((format (printf, 2, 3))) = 0;
-  virtual void error(std::string message, ...)   = 0;
+  virtual void error(const char * message, ...)   __attribute__ ((format (printf, 2, 3))) = 0;
-  virtual void warning(std::string message, ...) = 0;
+  virtual void warning(const char * message, ...) __attribute__ ((format (printf, 2, 3))) = 0;
-  virtual void info(std::string message, ...)    = 0;
+  virtual void info(const char * message, ...)    __attribute__ ((format (printf, 2, 3))) = 0;
-  virtual void debug(std::string message, ...)   = 0;
+  virtual void debug(const char * message, ...)   __attribute__ ((format (printf, 2, 3))) = 0;
 
   // Same with hex dump
   virtual void error_hex(uint8_t *hex, int size, std::string message, ...){error("error_hex not implemented.\n");}

lib/include/srslte/common/log_filter.h

@@ -57,11 +57,11 @@ public:
 
   void init(std::string layer, logger *logger_, bool tti=false);
 
-  void console(std::string message, ...);
+  void console(const char * message, ...);
-  void error(std::string message, ...);
+  void error(const char * message, ...);
-  void warning(std::string message, ...);
+  void warning(const char * message, ...);
-  void info(std::string message, ...);
+  void info(const char * message, ...);
-  void debug(std::string message, ...);
+  void debug(const char * message, ...);
 
   void error_hex(uint8_t *hex, int size, std::string message, ...);
   void warning_hex(uint8_t *hex, int size, std::string message, ...);

lib/include/srslte/common/logger_stdout.h

@@ -42,7 +42,10 @@ namespace srslte {
   {
   public:
     void log(std::string *msg) {
-      fprintf(stdout, "%s", msg->c_str());
+      if (msg) {
+        fprintf(stdout, "%s", msg->c_str());
+        delete msg;
+      }
     }
   };
 

lib/include/srslte/common/msg_queue.h

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

lib/include/srslte/common/pdu.h

@@ -133,11 +133,15 @@ public:
   // Section 6.1.2
   void parse_packet(uint8_t *ptr) {
     uint8_t *init_ptr = ptr; 
-    nof_subheaders = 0; 
+    nof_subheaders = 0;
-    while(subheaders[nof_subheaders].read_subheader(&ptr)) {
+    bool ret = false;
-      nof_subheaders++;
+    do {
-    }
+      if (nof_subheaders < (int)max_subheaders) {
-    nof_subheaders++;
+        ret = subheaders[nof_subheaders].read_subheader(&ptr);
+        nof_subheaders++;
+      }
+    } while (ret && (nof_subheaders + 1) < (int)max_subheaders);
+
     for (int i=0;i<nof_subheaders;i++) {
       subheaders[i].read_payload(&ptr);
     }

lib/include/srslte/common/timeout.h

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

lib/include/srslte/common/timers.h

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

lib/include/srslte/interfaces/ue_interfaces.h

@@ -119,7 +119,7 @@ public:
   virtual uint32_t  get_ul_count() = 0;
   virtual bool      get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi) = 0;
   virtual bool      get_k_asme(uint8_t *k_asme_, uint32_t n) = 0;
-  virtual void      plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) = 0;
+  virtual bool      plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) = 0;
   virtual void      plmn_search_end() = 0;
 };
 
@@ -160,7 +160,7 @@ public:
   virtual void in_sync() = 0;
   virtual void out_of_sync() = 0;
   virtual void earfcn_end() = 0;
-  virtual void cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) = 0;
+  virtual void cell_camping(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp = NAN) = 0;
   virtual void new_phy_meas(float rsrp, float rsrq, uint32_t tti, int earfcn = -1, int pci = -1) = 0;
 };
 
@@ -173,7 +173,7 @@ public:
   virtual uint16_t get_mnc() = 0;
   virtual void enable_capabilities() = 0;
   virtual void plmn_search() = 0;
-  virtual void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) = 0;
+  virtual void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, bool connect_request = false) = 0;
   virtual std::string get_rb_name(uint32_t lcid) = 0;
 };
 
@@ -474,7 +474,8 @@ typedef struct {
   int cqi_max; 
   int cqi_fixed; 
   float snr_ema_coeff; 
-  std::string snr_estim_alg; 
+  std::string snr_estim_alg;
+  bool cfo_is_doppler;
   bool cfo_integer_enabled; 
   float cfo_correct_tol_hz;
   float cfo_pss_ema;
@@ -487,11 +488,14 @@ typedef struct {
   uint32_t cfo_ref_mask;
   bool average_subframe_enabled;
   int time_correct_period; 
-  bool sfo_correct_disable; 
+  std::string sss_algorithm;
-  std::string sss_algorithm; 
   float estimator_fil_w;   
   bool rssi_sensor_enabled;
   bool sic_pss_enabled;
+  float rx_gain_offset;
+  bool pdsch_csi_enabled;
+  uint32_t intra_freq_meas_len_ms;
+  uint32_t intra_freq_meas_period_ms;
 } phy_args_t; 
 
 
@@ -579,9 +583,8 @@ public:
 
   /* Cell search and selection procedures */
   virtual void cell_search_start() = 0;
-  virtual void cell_search_stop() = 0;
   virtual void cell_search_next() = 0;
-  virtual bool cell_select(uint32_t earfcn, srslte_cell_t cell) = 0;
+  virtual void cell_select(uint32_t earfcn, srslte_cell_t cell) = 0;
   virtual bool cell_handover(srslte_cell_t cell) = 0;
 
   /* Is the PHY downlink synchronized? */

lib/include/srslte/phy/agc/agc.h

@@ -42,8 +42,8 @@
 
 #include "srslte/config.h"
 
-#define SRSLTE_AGC_DEFAULT_TARGET 0.7
+#define SRSLTE_AGC_DEFAULT_TARGET 0.3
-#define SRSLTE_AGC_DEFAULT_BW     (5e-1)
+#define SRSLTE_AGC_DEFAULT_BW     0.7
 
 typedef enum SRSLTE_API {
   SRSLTE_AGC_MODE_ENERGY = 0, 

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

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

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

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

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

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

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

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

lib/include/srslte/phy/rf/rf.h

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

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

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

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

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

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

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

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

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

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

@@ -120,6 +120,8 @@ SRSLTE_API void srslte_vec_abs_square_cf_simd(const cf_t *x, float *z, const int
 /* Other Functions */
 SRSLTE_API void srslte_vec_lut_sss_simd(const short *x, const unsigned short *lut, short *y, const int len);
 
+SRSLTE_API void srslte_vec_convert_if_simd(const int16_t *x, float *z, const float scale, const int len);
+
 SRSLTE_API void srslte_vec_convert_fi_simd(const float *x, int16_t *z, const float scale, const int len);
 
 SRSLTE_API void srslte_vec_cp_simd(const cf_t *src, cf_t *dst, int len);

lib/include/srslte/radio/radio.h

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

lib/include/srslte/upper/rlc_am.h

@@ -71,6 +71,7 @@ class rlc_am
 {
 public:
   rlc_am();
+  ~rlc_am();
   void init(log          *rlc_entity_log_,
             uint32_t              lcid_,
             srsue::pdcp_interface_rlc   *pdcp_,

lib/include/srslte/upper/rlc_um.h

@@ -50,6 +50,7 @@ class rlc_um
 {
 public:
   rlc_um();
+  ~rlc_um();
 
   void init(log          *rlc_entity_log_,
             uint32_t              lcid_,

lib/src/asn1/liblte_mme.cc

@@ -296,9 +296,10 @@ LIBLTE_ERROR_ENUM liblte_mme_pack_mobile_id_ie(LIBLTE_MME_MOBILE_ID_STRUCT  *mob
                                                uint8                       **ie_ptr)
 {
     LIBLTE_ERROR_ENUM  err = LIBLTE_ERROR_INVALID_INPUTS;
-    uint8             *id;
+    uint8             *id = NULL;
+    uint32             id32 = 0;
     uint32             i;
-    uint8              length;
+    uint8              length = 0;
     bool               odd = false;
 
     if(mobile_id != NULL &&
@@ -317,6 +318,11 @@ LIBLTE_ERROR_ENUM liblte_mme_pack_mobile_id_ie(LIBLTE_MME_MOBILE_ID_STRUCT  *mob
             id  = mobile_id->imeisv;
             length = 9;
             odd = false;
+        }else if(LIBLTE_MME_MOBILE_ID_TYPE_TMSI == mobile_id->type_of_id){
+            id32 = mobile_id->tmsi;
+            length = 4;
+            odd = false;
+        }
         }else{
             // FIXME: Not handling these IDs
             return(err);
@@ -325,30 +331,48 @@ LIBLTE_ERROR_ENUM liblte_mme_pack_mobile_id_ie(LIBLTE_MME_MOBILE_ID_STRUCT  *mob
         // Length
         **ie_ptr = length;
         *ie_ptr += 1;
-
+        if(LIBLTE_MME_MOBILE_ID_TYPE_TMSI != mobile_id->type_of_id)
-        // | Identity digit 1 | odd/even | Id type |
-        if(odd)
         {
-          **ie_ptr  = (id[0] << 4) | (1 << 3) | mobile_id->type_of_id;
+            // | Identity digit 1 | odd/even | Id type |
-        }else{
+            if(odd)
-          **ie_ptr  = (id[0] << 4) | (0 << 3) | mobile_id->type_of_id;
+            {
-        }
+                **ie_ptr  = (id[0] << 4) | (1 << 3) | mobile_id->type_of_id;
-        *ie_ptr  += 1;
+            }else{
+                **ie_ptr  = (id[0] << 4) | (0 << 3) | mobile_id->type_of_id;
+            }
+            *ie_ptr  += 1;
 
-        // | Identity digit p+1 | Identity digit p |
+        
-        for(i=0; i<7; i++)
+            // | Identity digit p+1 | Identity digit p |
-        {
+            for(i=0; i<7; i++)
-            (*ie_ptr)[i] = (id[i*2+2] << 4) | id[i*2+1];
+            {
-        }
+                (*ie_ptr)[i] = (id[i*2+2] << 4) | id[i*2+1];
-        *ie_ptr += 7;
+            }
-        if(!odd)
+            *ie_ptr += 7;
-        {
+            if(!odd)
-          **ie_ptr = 0xF0 | id[15];
+            {
-          *ie_ptr += 1;
+              **ie_ptr = 0xF0 | id[15];
+              *ie_ptr += 1;
+            }
+
+            err = LIBLTE_SUCCESS;
         }
+        else{
 
-        err = LIBLTE_SUCCESS;
+          **ie_ptr  = (0xFF << 4) | (0 << 3) | mobile_id->type_of_id;
-    }
+          *ie_ptr  += 1;
+          //4-Byte based ids
+          **ie_ptr  = (id32 >> 24) & 0xFF;
+          *ie_ptr  += 1;
+          **ie_ptr  = (id32 >> 16) & 0xFF;
+          *ie_ptr  += 1;
+          **ie_ptr  = (id32 >> 8) & 0xFF;
+          *ie_ptr  += 1;
+          **ie_ptr  = id32 & 0xFF;
+          *ie_ptr  += 1;
+
+          err = LIBLTE_SUCCESS;
+        }
 
     return(err);
 }

lib/src/asn1/liblte_rrc.cc

@@ -11771,7 +11771,7 @@ LIBLTE_ERROR_ENUM liblte_rrc_pack_rrc_connection_reject_msg(LIBLTE_RRC_CONNECTIO
         liblte_value_2_bits(0, &msg_ptr, 1);
 
         // Wait Time
-        liblte_value_2_bits(con_rej->wait_time, &msg_ptr, 4);
+        liblte_value_2_bits(con_rej->wait_time - 1, &msg_ptr, 4);
 
         // Fill in the number of bits used
         msg->N_bits = msg_ptr - msg->msg;
@@ -11800,7 +11800,7 @@ LIBLTE_ERROR_ENUM liblte_rrc_unpack_rrc_connection_reject_msg(LIBLTE_BIT_MSG_STR
         liblte_rrc_warning_not_handled(liblte_bits_2_value(&msg_ptr, 1), __func__);;
 
         // Wait Time
-        con_rej->wait_time = liblte_bits_2_value(&msg_ptr, 4);
+        con_rej->wait_time = liblte_bits_2_value(&msg_ptr, 4) + 1;
 
         liblte_rrc_consume_noncrit_extension(ext, __func__, &msg_ptr);
         

lib/src/common/buffer_pool.cc

@@ -54,12 +54,5 @@ void byte_buffer_pool::cleanup(void)
   }
   pthread_mutex_unlock(&instance_mutex);
 }
-
   
-
-
-
-
-
-
 } // namespace srsue

lib/src/common/log_filter.cc

@@ -40,6 +40,7 @@ log_filter::log_filter()
   do_tti      = false;
   time_src    = NULL;
   time_format = TIME;
+  logger_h    = NULL;
 }
 
 log_filter::log_filter(std::string layer)
@@ -134,55 +135,55 @@ void log_filter::all_log(srslte::LOG_LEVEL_ENUM level,
   }
 }
 
-void log_filter::console(std::string message, ...) {
+void log_filter::console(const char * message, ...) {
   char     *args_msg;
   va_list   args;
   va_start(args, message);
-  if(vasprintf(&args_msg, message.c_str(), args) > 0)
+  if(vasprintf(&args_msg, message, args) > 0)
     printf("%s",args_msg); // Print directly to stdout
   va_end(args);
   free(args_msg);
 }
 
-void log_filter::error(std::string message, ...) {
+void log_filter::error(const char * message, ...) {
   if (level >= LOG_LEVEL_ERROR) {
     char     *args_msg;
     va_list   args;
     va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
+    if(vasprintf(&args_msg, message, args) > 0)
       all_log(LOG_LEVEL_ERROR, tti, args_msg);
     va_end(args);
     free(args_msg);
   }
 }
-void log_filter::warning(std::string message, ...) {
+void log_filter::warning(const char * message, ...) {
   if (level >= LOG_LEVEL_WARNING) {
     char     *args_msg;
     va_list   args;
     va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
+    if(vasprintf(&args_msg, message, args) > 0)
       all_log(LOG_LEVEL_WARNING, tti, args_msg);
     va_end(args);
     free(args_msg);
   }
 }
-void log_filter::info(std::string message, ...) {
+void log_filter::info(const char * message, ...) {
   if (level >= LOG_LEVEL_INFO) {
     char     *args_msg;
     va_list   args;
     va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
+    if(vasprintf(&args_msg, message, args) > 0)
       all_log(LOG_LEVEL_INFO, tti, args_msg);
     va_end(args);
     free(args_msg);
   }
 }
-void log_filter::debug(std::string message, ...) {
+void log_filter::debug(const char * message, ...) {
   if (level >= LOG_LEVEL_DEBUG) {
     char     *args_msg;
     va_list   args;
     va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
+    if(vasprintf(&args_msg, message, args) > 0)
       all_log(LOG_LEVEL_DEBUG, tti, args_msg);
     va_end(args);
     free(args_msg);

lib/src/common/logger_file.cc

@@ -35,6 +35,7 @@ namespace srslte{
 
 logger_file::logger_file()
   :inited(false)
+  ,logfile(NULL)
   ,not_done(true)
   ,cur_length(0)
   ,max_length(0)
@@ -46,7 +47,9 @@ logger_file::~logger_file() {
   if(inited) {
     wait_thread_finish();
     flush();
-    fclose(logfile);
+    if (logfile) {
+      fclose(logfile);
+    }
   }
 }
 

lib/src/common/pdu.cc

@@ -120,13 +120,18 @@ uint8_t* sch_pdu::write_packet(srslte::log *log_h)
 {
   int init_rem_len=rem_len; 
   sch_subh padding; 
-  padding.set_padding(); 
+  padding.set_padding();
-  
+
+  if (nof_subheaders <= 0 && nof_subheaders < (int)max_subheaders) {
+    log_h->error("Trying to write packet with invalid number of subheaders (nof_subheaders=%d).\n", nof_subheaders);
+    return NULL;
+  }
+
   if (init_rem_len < 0) {
     log_h->error("init_rem_len=%d\n", init_rem_len);
     return NULL; 
   }
-  
+
   /* If last SDU has zero payload, remove it. FIXME: Why happens this?? */
   if (subheaders[nof_subheaders-1].get_payload_size() == 0) {
     del_subh();

lib/src/common/pdu_queue.cc

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

lib/src/phy/agc/agc.c

@@ -177,7 +177,7 @@ void srslte_agc_process(srslte_agc_t *q, cf_t *signal, uint32_t len) {
           gg = expf(-0.5*q->bandwidth*logf(q->y_out/q->target));
           q->gain *= gg; 
         }          
-        DEBUG("AGC gain: %.2f (%.2f) y_out=%.3f, y=%.3f target=%.1f gg=%.2f\n", gain_db, gain_uhd_db, q->y_out, y, q->target, gg);
+        INFO("AGC gain: %.2f (%.2f) y_out=%.3f, y=%.3f target=%.1f gg=%.2f\n", gain_db, gain_uhd_db, q->y_out, y, q->target, gg);
       }
     }
   }

lib/src/phy/ch_estimation/chest_dl.c

@@ -88,7 +88,7 @@ int srslte_chest_dl_init(srslte_chest_dl_t *q, uint32_t max_prb)
       goto clean_exit;
     }
     
-    q->mbsfn_refs = calloc(SRSLTE_MAX_MBSFN_AREA_IDS, sizeof(srslte_refsignal_t*));
+    q->mbsfn_refs = calloc(SRSLTE_MAX_MBSFN_AREA_IDS, sizeof(srslte_refsignal_t));
     if (!q->mbsfn_refs) {
       fprintf(stderr, "Calloc error initializing mbsfn_refs (%d)\n", ret);
       goto clean_exit;
@@ -152,7 +152,9 @@ int srslte_chest_dl_init(srslte_chest_dl_t *q, uint32_t max_prb)
     }
     
     q->noise_alg = SRSLTE_NOISE_ALG_REFS; 
-    
+
+    q->rsrp_neighbour = false;
+
     q->smooth_filter_len = 3; 
     srslte_chest_dl_set_smooth_filter3_coeff(q, 0.1);
     
@@ -169,14 +171,14 @@ clean_exit:
 
 void srslte_chest_dl_free(srslte_chest_dl_t *q) 
 {
-  int i;
   if(&q->csr_refs)
     srslte_refsignal_free(&q->csr_refs);
 
   if (q->mbsfn_refs) {
-    for (i=0; i<SRSLTE_MAX_MBSFN_AREA_IDS; i++) {
+    for (int i=0; i<SRSLTE_MAX_MBSFN_AREA_IDS; i++) {
       if (q->mbsfn_refs[i]) {
         srslte_refsignal_free(q->mbsfn_refs[i]);
+        free(q->mbsfn_refs[i]);
       }
     }
     free(q->mbsfn_refs);
@@ -206,15 +208,18 @@ void srslte_chest_dl_free(srslte_chest_dl_t *q)
 
 
 int srslte_chest_dl_set_mbsfn_area_id(srslte_chest_dl_t *q, uint16_t mbsfn_area_id){
-  if(!q->mbsfn_refs[mbsfn_area_id]){
+  if (mbsfn_area_id < SRSLTE_MAX_MBSFN_AREA_IDS) {
-    q->mbsfn_refs[mbsfn_area_id] = calloc(1, sizeof(srslte_refsignal_t));
+    if(!q->mbsfn_refs[mbsfn_area_id]) {
-  }
+      q->mbsfn_refs[mbsfn_area_id] = calloc(1, sizeof(srslte_refsignal_t));
-  if(q->mbsfn_refs[mbsfn_area_id]) {
-    if(srslte_refsignal_mbsfn_init(q->mbsfn_refs[mbsfn_area_id], q->cell, mbsfn_area_id)) {
-      return SRSLTE_ERROR;
     }
-  }  
+    if(q->mbsfn_refs[mbsfn_area_id]) {
-  return SRSLTE_SUCCESS;
+      if(srslte_refsignal_mbsfn_init(q->mbsfn_refs[mbsfn_area_id], q->cell, mbsfn_area_id)) {
+        return SRSLTE_ERROR;
+      }
+    }
+    return SRSLTE_SUCCESS;
+  }
+  return SRSLTE_ERROR;
 }
 
 int srslte_chest_dl_set_cell(srslte_chest_dl_t *q, srslte_cell_t cell)
@@ -291,7 +296,7 @@ static float estimate_noise_pilots(srslte_chest_dl_t *q, uint32_t port_id, srslt
       norm /= norm3;
     }
   }
-  float power = norm*q->cell.nof_ports*srslte_vec_avg_power_cf(q->tmp_noise, nref);
+  float power = norm*srslte_vec_avg_power_cf(q->tmp_noise, nref);
   return power; 
 }
 
@@ -540,27 +545,32 @@ void chest_interpolate_noise_est(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, ui
       }
     } 
   }
-
-  /* Compute RSRP for the channel estimates in this port */
-  uint32_t npilots = SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id);
-  q->rsrp[rxant_id][port_id] = __real__ srslte_vec_dot_prod_conj_ccc(q->pilot_estimates, q->pilot_estimates, npilots) / npilots;
-  q->rssi[rxant_id][port_id] = srslte_chest_dl_rssi(q, input, port_id);
 }
 
-int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, uint32_t sf_idx, uint32_t port_id, uint32_t rxant_id) 
+int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, uint32_t sf_idx, uint32_t port_id, uint32_t rxant_id)
 {
+  uint32_t npilots = SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id);
+
   /* Get references from the input signal */
   srslte_refsignal_cs_get_sf(q->cell, port_id, input, q->pilot_recv_signal);
   
   /* Use the known CSR signal to compute Least-squares estimates */
   srslte_vec_prod_conj_ccc(q->pilot_recv_signal, q->csr_refs.pilots[port_id/2][sf_idx], 
-              q->pilot_estimates, SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id)); 
+              q->pilot_estimates, npilots);
 
+  /* Compute RSRP for the channel estimates in this port */
+  if (q->rsrp_neighbour) {
+    double energy = cabs(srslte_vec_acc_cc(q->pilot_estimates, npilots)/npilots);
+    q->rsrp_corr[rxant_id][port_id] = energy*energy;
+  }
+  q->rsrp[rxant_id][port_id] = srslte_vec_avg_power_cf(q->pilot_recv_signal, npilots);
+  q->rssi[rxant_id][port_id] = srslte_chest_dl_rssi(q, input, port_id);
 
   chest_interpolate_noise_est(q, input, ce, sf_idx, port_id, rxant_id, SRSLTE_SF_NORM);
 
   return 0;
 }
+
 int srslte_chest_dl_estimate_port_mbsfn(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, uint32_t sf_idx, uint32_t port_id, uint32_t rxant_id, uint16_t mbsfn_area_id)
 {
 
@@ -619,6 +629,10 @@ int srslte_chest_dl_estimate_multi_mbsfn(srslte_chest_dl_t *q, cf_t *input[SRSLT
   return SRSLTE_SUCCESS;
 }
 
+void srslte_chest_dl_set_rsrp_neighbour(srslte_chest_dl_t *q, bool rsrp_for_neighbour) {
+  q->rsrp_neighbour = rsrp_for_neighbour;
+}
+
 void srslte_chest_dl_average_subframe(srslte_chest_dl_t *q, bool enable)
 {
   q->average_subframe = enable;
@@ -639,7 +653,10 @@ float srslte_chest_dl_get_noise_estimate(srslte_chest_dl_t *q) {
   for (int i=0;i<q->last_nof_antennas;i++) {
     n += srslte_vec_acc_ff(q->noise_estimate[i], q->cell.nof_ports)/q->cell.nof_ports;
   }
-  return n/q->last_nof_antennas;
+  if (q->last_nof_antennas) {
+    n /= q->last_nof_antennas;
+  }
+  return n;
 }
 
 float srslte_chest_dl_get_snr(srslte_chest_dl_t *q) {
@@ -691,14 +708,49 @@ float srslte_chest_dl_get_rsrp_ant_port(srslte_chest_dl_t *q, uint32_t ant_idx,
 }
 
 float srslte_chest_dl_get_rsrp_port(srslte_chest_dl_t *q, uint32_t port) {
-  float n = 0;
+  float sum = 0.0f;
-  for (int i = 0; i < q->last_nof_antennas; i++) {
+  for (int j = 0; j < q->cell.nof_ports; ++j) {
-      n += q->rsrp[i][port];
+    sum +=q->rsrp[port][j];
+  }
+
+  if (q->cell.nof_ports) {
+    sum /= q->cell.nof_ports;
   }
-  return n / q->last_nof_antennas;
+
+  return sum;
+}
+
+float srslte_chest_dl_get_rsrp_neighbour_port(srslte_chest_dl_t *q, uint32_t port) {
+  float sum = 0.0f;
+  for (int j = 0; j < q->cell.nof_ports; ++j) {
+    sum +=q->rsrp_corr[port][j];
+  }
+
+  if (q->cell.nof_ports) {
+    sum /= q->cell.nof_ports;
+  }
+
+  return sum;
 }
 
 float srslte_chest_dl_get_rsrp(srslte_chest_dl_t *q) {
-  // Note: use only port 0 but average across antennas
+  float max = -1e9;
-  return srslte_chest_dl_get_rsrp_port(q, 0);
+  for (int i = 0; i < q->last_nof_antennas; ++i) {
+    float v = srslte_chest_dl_get_rsrp_port(q, i);
+    if (v > max) {
+      max = v;
+    }
+  }
+  return max;
+}
+
+float srslte_chest_dl_get_rsrp_neighbour(srslte_chest_dl_t *q) {
+  float max = -1e9;
+  for (int i = 0; i < q->last_nof_antennas; ++i) {
+    float v = srslte_chest_dl_get_rsrp_neighbour_port(q, i);
+    if (v > max) {
+      max = v;
+    }
+  }
+  return max;
 }

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

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

lib/src/phy/dft/dft_fftw.c

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

lib/src/phy/dft/ofdm.c

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

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

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

lib/src/phy/fec/softbuffer.c

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

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

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

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

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

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

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

lib/src/phy/fec/viterbi.c

@@ -166,9 +166,15 @@ void free37_avx2_16bit(void *o) {
   if (q->symbols_uc) {
     free(q->symbols_uc);
   }
+  if (q->symbols_us) {
+    free(q->symbols_us);
+  }
   if (q->tmp) {
     free(q->tmp);
   }
+  if (q->tmp_s) {
+    free(q->tmp_s);
+  }
   delete_viterbi37_avx2_16bit(q->ptr);
 }
 

lib/src/phy/mimo/precoding.c

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

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

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

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

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

lib/src/phy/phch/cqi.c

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

lib/src/phy/phch/pbch.c

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

lib/src/phy/phch/pcfich.c

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

lib/src/phy/phch/pdcch.c

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

lib/src/phy/phch/pdsch.c

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

lib/src/phy/phch/phich.c

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

lib/src/phy/phch/pmch.c

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

lib/src/phy/phch/prach.c

@@ -57,185 +57,177 @@ float save_corr[4096];
 uint32_t prach_Tcp[5] = {3168, 21024, 6240, 21024, 448};
 
 // Table 5.7.1-1 T_seq for preamble formats
-uint32_t prach_Tseq[5] = {24576, 24576, 2*24576, 2*24576, 4096};
+uint32_t prach_Tseq[5] = {24576, 24576, 2 * 24576, 2 * 24576, 4096};
 
 // Table 5.7.2-2 - N_cs values for unrestricted sets
-uint32_t prach_Ncs_unrestricted[16] = {0,13,15,18,22,26,32,38,46,59,76,93,119,167,279,419};
+uint32_t prach_Ncs_unrestricted[16] = {0, 13, 15, 18, 22, 26, 32, 38, 46, 59, 76, 93, 119, 167, 279, 419};
 
 #define MAX_N_zc 839
 
 // Table 5.7.2-2 - N_cs values for restricted sets
-uint32_t prach_Ncs_restricted[15] = {15,18,22,26,32,38,46,55,68,82,100,128,158,202,237};
+uint32_t prach_Ncs_restricted[15] = {15, 18, 22, 26, 32, 38, 46, 55, 68, 82, 100, 128, 158, 202, 237};
 
 // Table 5.7.2-3 - N_cs values for preamble format 4
-uint32_t prach_Ncs_format4[7] = {2,4,6,8,10,12,15};
+uint32_t prach_Ncs_format4[7] = {2, 4, 6, 8, 10, 12, 15};
 
 // Table 5.7.2-4 - Root ZC sequence order
 uint32_t prach_zc_roots[838] = {
-  129, 710, 140, 699, 120, 719, 210, 629, 168, 671,  84, 755,
+    129, 710, 140, 699, 120, 719, 210, 629, 168, 671, 84, 755,
-  105, 734,  93, 746,  70, 769,  60, 779,   2, 837,   1, 838,
+    105, 734, 93, 746, 70, 769, 60, 779, 2, 837, 1, 838,
-   56, 783, 112, 727, 148, 691,  80, 759,  42, 797,  40, 799,
+    56, 783, 112, 727, 148, 691, 80, 759, 42, 797, 40, 799,
-   35, 804,  73, 766, 146, 693,  31, 808,  28, 811,  30, 809,
+    35, 804, 73, 766, 146, 693, 31, 808, 28, 811, 30, 809,
-   27, 812,  29, 810,  24, 815,  48, 791,  68, 771,  74, 765,
+    27, 812, 29, 810, 24, 815, 48, 791, 68, 771, 74, 765,
-  178, 661, 136, 703,  86, 753,  78, 761,  43, 796,  39, 800,
+    178, 661, 136, 703, 86, 753, 78, 761, 43, 796, 39, 800,
-   20, 819,  21, 818,  95, 744, 202, 637, 190, 649, 181, 658,
+    20, 819, 21, 818, 95, 744, 202, 637, 190, 649, 181, 658,
-  137, 702, 125, 714, 151, 688, 217, 622, 128, 711, 142, 697,
+    137, 702, 125, 714, 151, 688, 217, 622, 128, 711, 142, 697,
-  122, 717, 203, 636, 118, 721, 110, 729,  89, 750, 103, 736,
+    122, 717, 203, 636, 118, 721, 110, 729, 89, 750, 103, 736,
-   61, 778,  55, 784,  15, 824,  14, 825,  12, 827,  23, 816,
+    61, 778, 55, 784, 15, 824, 14, 825, 12, 827, 23, 816,
-   34, 805,  37, 802,  46, 793, 207, 632, 179, 660, 145, 694,
+    34, 805, 37, 802, 46, 793, 207, 632, 179, 660, 145, 694,
-  130, 709, 223, 616, 228, 611, 227, 612, 132, 707, 133, 706,
+    130, 709, 223, 616, 228, 611, 227, 612, 132, 707, 133, 706,
-  143, 696, 135, 704, 161, 678, 201, 638, 173, 666, 106, 733,
+    143, 696, 135, 704, 161, 678, 201, 638, 173, 666, 106, 733,
-   83, 756,  91, 748,  66, 773,  53, 786,  10, 829,   9, 830,
+    83, 756, 91, 748, 66, 773, 53, 786, 10, 829, 9, 830,
-    7, 832,   8, 831,  16, 823,  47, 792,  64, 775,  57, 782,
+    7, 832, 8, 831, 16, 823, 47, 792, 64, 775, 57, 782,
-  104, 735, 101, 738, 108, 731, 208, 631, 184, 655, 197, 642,
+    104, 735, 101, 738, 108, 731, 208, 631, 184, 655, 197, 642,
-  191, 648, 121, 718, 141, 698, 149, 690, 216, 623, 218, 621,
+    191, 648, 121, 718, 141, 698, 149, 690, 216, 623, 218, 621,
-  152, 687, 144, 695, 134, 705, 138, 701, 199, 640, 162, 677,
+    152, 687, 144, 695, 134, 705, 138, 701, 199, 640, 162, 677,
-  176, 663, 119, 720, 158, 681, 164, 675, 174, 665, 171, 668,
+    176, 663, 119, 720, 158, 681, 164, 675, 174, 665, 171, 668,
-  170, 669,  87, 752, 169, 670,  88, 751, 107, 732,  81, 758,
+    170, 669, 87, 752, 169, 670, 88, 751, 107, 732, 81, 758,
-   82, 757, 100, 739,  98, 741,  71, 768,  59, 780,  65, 774,
+    82, 757, 100, 739, 98, 741, 71, 768, 59, 780, 65, 774,
-   50, 789,  49, 790,  26, 813,  17, 822,  13, 826,   6, 833,
+    50, 789, 49, 790, 26, 813, 17, 822, 13, 826, 6, 833,
-    5, 834,  33, 806,  51, 788,  75, 764,  99, 740,  96, 743,
+    5, 834, 33, 806, 51, 788, 75, 764, 99, 740, 96, 743,
-   97, 742, 166, 673, 172, 667, 175, 664, 187, 652, 163, 676,
+    97, 742, 166, 673, 172, 667, 175, 664, 187, 652, 163, 676,
-  185, 654, 200, 639, 114, 725, 189, 650, 115, 724, 194, 645,
+    185, 654, 200, 639, 114, 725, 189, 650, 115, 724, 194, 645,
-  195, 644, 192, 647, 182, 657, 157, 682, 156, 683, 211, 628,
+    195, 644, 192, 647, 182, 657, 157, 682, 156, 683, 211, 628,
-  154, 685, 123, 716, 139, 700, 212, 627, 153, 686, 213, 626,
+    154, 685, 123, 716, 139, 700, 212, 627, 153, 686, 213, 626,
-  215, 624, 150, 689, 225, 614, 224, 615, 221, 618, 220, 619,
+    215, 624, 150, 689, 225, 614, 224, 615, 221, 618, 220, 619,
-  127, 712, 147, 692, 124, 715, 193, 646, 205, 634, 206, 633,
+    127, 712, 147, 692, 124, 715, 193, 646, 205, 634, 206, 633,
-  116, 723, 160, 679, 186, 653, 167, 672,  79, 760,  85, 754,
+    116, 723, 160, 679, 186, 653, 167, 672, 79, 760, 85, 754,
-   77, 762,  92, 747,  58, 781,  62, 777,  69, 770,  54, 785,
+    77, 762, 92, 747, 58, 781, 62, 777, 69, 770, 54, 785,
-   36, 803,  32, 807,  25, 814,  18, 821,  11, 828,   4, 835,
+    36, 803, 32, 807, 25, 814, 18, 821, 11, 828, 4, 835,
-    3, 836,  19, 820,  22, 817,  41, 798,  38, 801,  44, 795,
+    3, 836, 19, 820, 22, 817, 41, 798, 38, 801, 44, 795,
-   52, 787,  45, 794,  63, 776,  67, 772,  72, 767,  76, 763,
+    52, 787, 45, 794, 63, 776, 67, 772, 72, 767, 76, 763,
-   94, 745, 102, 737,  90, 749, 109, 730, 165, 674, 111, 728,
+    94, 745, 102, 737, 90, 749, 109, 730, 165, 674, 111, 728,
-  209, 630, 204, 635, 117, 722, 188, 651, 159, 680, 198, 641,
+    209, 630, 204, 635, 117, 722, 188, 651, 159, 680, 198, 641,
-  113, 726, 183, 656, 180, 659, 177, 662, 196, 643, 155, 684,
+    113, 726, 183, 656, 180, 659, 177, 662, 196, 643, 155, 684,
-  214, 625, 126, 713, 131, 708, 219, 620, 222, 617, 226, 613,
+    214, 625, 126, 713, 131, 708, 219, 620, 222, 617, 226, 613,
-  230, 609, 232, 607, 262, 577, 252, 587, 418, 421, 416, 423,
+    230, 609, 232, 607, 262, 577, 252, 587, 418, 421, 416, 423,
-  413, 426, 411, 428, 376, 463, 395, 444, 283, 556, 285, 554,
+    413, 426, 411, 428, 376, 463, 395, 444, 283, 556, 285, 554,
-  379, 460, 390, 449, 363, 476, 384, 455, 388, 451, 386, 453,
+    379, 460, 390, 449, 363, 476, 384, 455, 388, 451, 386, 453,
-  361, 478, 387, 452, 360, 479, 310, 529, 354, 485, 328, 511,
+    361, 478, 387, 452, 360, 479, 310, 529, 354, 485, 328, 511,
-  315, 524, 337, 502, 349, 490, 335, 504, 324, 515, 323, 516,
+    315, 524, 337, 502, 349, 490, 335, 504, 324, 515, 323, 516,
-  320, 519, 334, 505, 359, 480, 295, 544, 385, 454, 292, 547,
+    320, 519, 334, 505, 359, 480, 295, 544, 385, 454, 292, 547,
-  291, 548, 381, 458, 399, 440, 380, 459, 397, 442, 369, 470,
+    291, 548, 381, 458, 399, 440, 380, 459, 397, 442, 369, 470,
-  377, 462, 410, 429, 407, 432, 281, 558, 414, 425, 247, 592,
+    377, 462, 410, 429, 407, 432, 281, 558, 414, 425, 247, 592,
-  277, 562, 271, 568, 272, 567, 264, 575, 259, 580, 237, 602,
+    277, 562, 271, 568, 272, 567, 264, 575, 259, 580, 237, 602,
-  239, 600, 244, 595, 243, 596, 275, 564, 278, 561, 250, 589,
+    239, 600, 244, 595, 243, 596, 275, 564, 278, 561, 250, 589,
-  246, 593, 417, 422, 248, 591, 394, 445, 393, 446, 370, 469,
+    246, 593, 417, 422, 248, 591, 394, 445, 393, 446, 370, 469,
-  365, 474, 300, 539, 299, 540, 364, 475, 362, 477, 298, 541,
+    365, 474, 300, 539, 299, 540, 364, 475, 362, 477, 298, 541,
-  312, 527, 313, 526, 314, 525, 353, 486, 352, 487, 343, 496,
+    312, 527, 313, 526, 314, 525, 353, 486, 352, 487, 343, 496,
-  327, 512, 350, 489, 326, 513, 319, 520, 332, 507, 333, 506,
+    327, 512, 350, 489, 326, 513, 319, 520, 332, 507, 333, 506,
-  348, 491, 347, 492, 322, 517, 330, 509, 338, 501, 341, 498,
+    348, 491, 347, 492, 322, 517, 330, 509, 338, 501, 341, 498,
-  340, 499, 342, 497, 301, 538, 366, 473, 401, 438, 371, 468,
+    340, 499, 342, 497, 301, 538, 366, 473, 401, 438, 371, 468,
-  408, 431, 375, 464, 249, 590, 269, 570, 238, 601, 234, 605,
+    408, 431, 375, 464, 249, 590, 269, 570, 238, 601, 234, 605,
-  257, 582, 273, 566, 255, 584, 254, 585, 245, 594, 251, 588,
+    257, 582, 273, 566, 255, 584, 254, 585, 245, 594, 251, 588,
-  412, 427, 372, 467, 282, 557, 403, 436, 396, 443, 392, 447,
+    412, 427, 372, 467, 282, 557, 403, 436, 396, 443, 392, 447,
-  391, 448, 382, 457, 389, 450, 294, 545, 297, 542, 311, 528,
+    391, 448, 382, 457, 389, 450, 294, 545, 297, 542, 311, 528,
-  344, 495, 345, 494, 318, 521, 331, 508, 325, 514, 321, 518,
+    344, 495, 345, 494, 318, 521, 331, 508, 325, 514, 321, 518,
-  346, 493, 339, 500, 351, 488, 306, 533, 289, 550, 400, 439,
+    346, 493, 339, 500, 351, 488, 306, 533, 289, 550, 400, 439,
-  378, 461, 374, 465, 415, 424, 270, 569, 241, 598, 231, 608,
+    378, 461, 374, 465, 415, 424, 270, 569, 241, 598, 231, 608,
-  260, 579, 268, 571, 276, 563, 409, 430, 398, 441, 290, 549,
+    260, 579, 268, 571, 276, 563, 409, 430, 398, 441, 290, 549,
-  304, 535, 308, 531, 358, 481, 316, 523, 293, 546, 288, 551,
+    304, 535, 308, 531, 358, 481, 316, 523, 293, 546, 288, 551,
-  284, 555, 368, 471, 253, 586, 256, 583, 263, 576, 242, 597,
+    284, 555, 368, 471, 253, 586, 256, 583, 263, 576, 242, 597,
-  274, 565, 402, 437, 383, 456, 357, 482, 329, 510, 317, 522,
+    274, 565, 402, 437, 383, 456, 357, 482, 329, 510, 317, 522,
-  307, 532, 286, 553, 287, 552, 266, 573, 261, 578, 236, 603,
+    307, 532, 286, 553, 287, 552, 266, 573, 261, 578, 236, 603,
-  303, 536, 356, 483, 355, 484, 405, 434, 404, 435, 406, 433,
+    303, 536, 356, 483, 355, 484, 405, 434, 404, 435, 406, 433,
-  235, 604, 267, 572, 302, 537, 309, 530, 265, 574, 233, 606,
+    235, 604, 267, 572, 302, 537, 309, 530, 265, 574, 233, 606,
-  367, 472, 296, 543, 336, 503, 305, 534, 373, 466, 280, 559,
+    367, 472, 296, 543, 336, 503, 305, 534, 373, 466, 280, 559,
-  279, 560, 419, 420, 240, 599, 258, 581, 229, 610};
+    279, 560, 419, 420, 240, 599, 258, 581, 229, 610};
 
 // Table 5.7.2-5 - Root ZC sequence order for preamble format 4
 uint32_t prach_zc_roots_format4[138] = {
-   1, 138,   2, 137,   3, 136,   4, 135,   5, 134,   6, 133,
+    1, 138, 2, 137, 3, 136, 4, 135, 5, 134, 6, 133,
-   7, 132,   8, 131,   9, 130,  10, 129,  11, 128,  12, 127,
+    7, 132, 8, 131, 9, 130, 10, 129, 11, 128, 12, 127,
-  13, 126,  14, 125,  15, 124,  16, 123,  17, 122,  18, 121,
+    13, 126, 14, 125, 15, 124, 16, 123, 17, 122, 18, 121,
-  19, 120,  20, 119,  21, 118,  22, 117,  23, 116,  24, 115,
+    19, 120, 20, 119, 21, 118, 22, 117, 23, 116, 24, 115,
-  25, 114,  26, 113,  27, 112,  28, 111,  29, 110,  30, 109,
+    25, 114, 26, 113, 27, 112, 28, 111, 29, 110, 30, 109,
-  31, 108,  32, 107,  33, 106,  34, 105,  35, 104,  36, 103,
+    31, 108, 32, 107, 33, 106, 34, 105, 35, 104, 36, 103,
-  37, 102,  38, 101,  39, 100,  40,  99,  41,  98,  42,  97,
+    37, 102, 38, 101, 39, 100, 40, 99, 41, 98, 42, 97,
-  43,  96,  44,  95,  45,  94,  46,  93,  47,  92,  48,  91,
+    43, 96, 44, 95, 45, 94, 46, 93, 47, 92, 48, 91,
-  49,  90,  50,  89,  51,  88,  52,  87,  53,  86,  54,  85,
+    49, 90, 50, 89, 51, 88, 52, 87, 53, 86, 54, 85,
-  55,  84,  56,  83,  57,  82,  58,  81,  59,  80,  60,  79,
+    55, 84, 56, 83, 57, 82, 58, 81, 59, 80, 60, 79,
-  61,  78,  62,  77,  63,  76,  64,  75,  65,  74,  66,  73,
+    61, 78, 62, 77, 63, 76, 64, 75, 65, 74, 66, 73,
-  67,  72,  68,  71,  69,  70};
+    67, 72, 68, 71, 69, 70};
-
+
-  
 srslte_prach_sf_config_t prach_sf_config[16] = {
-  {1, {1, 0, 0, 0, 0}},
+    {1, {1, 0, 0, 0, 0}},
-  {1, {4, 0, 0, 0, 0}},
+    {1, {4, 0, 0, 0, 0}},
-  {1, {7, 0, 0, 0, 0}},
+    {1, {7, 0, 0, 0, 0}},
-  {1, {1, 0, 0, 0, 0}},
+    {1, {1, 0, 0, 0, 0}},
-  {1, {4, 0, 0, 0, 0}},
+    {1, {4, 0, 0, 0, 0}},
-  {1, {7, 0, 0, 0, 0}},
+    {1, {7, 0, 0, 0, 0}},
-  {2, {1, 6, 0, 0, 0}},
+    {2, {1, 6, 0, 0, 0}},
-  {2, {2, 7, 0, 0, 0}},
+    {2, {2, 7, 0, 0, 0}},
-  {2, {3, 8, 0, 0, 0}},
+    {2, {3, 8, 0, 0, 0}},
-  {3, {1, 4, 7, 0, 0}},
+    {3, {1, 4, 7, 0, 0}},
-  {3, {2, 5, 8, 0, 0}},
+    {3, {2, 5, 8, 0, 0}},
-  {3, {3, 6, 9, 0, 0}},
+    {3, {3, 6, 9, 0, 0}},
-  {5, {0, 2, 4, 6, 8}},
+    {5, {0, 2, 4, 6, 8}},
-  {5, {1, 3, 5, 7, 9}},
+    {5, {1, 3, 5, 7, 9}},
-  {-1, {0, 0, 0, 0, 0}}, // this means all subframes 
+    {-1, {0, 0, 0, 0, 0}}, // this means all subframes
-  {1, {9, 0, 0, 0, 0}}};
+    {1, {9, 0, 0, 0, 0}}};
 
 uint32_t srslte_prach_get_preamble_format(uint32_t config_idx) {
-  return config_idx/16;
+  return config_idx / 16;
 }
 
 srslte_prach_sfn_t srslte_prach_get_sfn(uint32_t config_idx) {
-  if ((config_idx%16)<3 || (config_idx%16)==15) {
+  if ((config_idx % 16) < 3 || (config_idx % 16) == 15) {
     return SRSLTE_PRACH_SFN_EVEN;
   } else {
-    return SRSLTE_PRACH_SFN_ANY; 
+    return SRSLTE_PRACH_SFN_ANY;
   }
 }
 
 /* Returns true if current_tti is a valid opportunity for PRACH transmission and the is an allowed subframe, 
  * or allowed_subframe == -1
  */
-bool srslte_prach_tti_opportunity(srslte_prach_t *p, uint32_t current_tti, int allowed_subframe) 
+bool srslte_prach_tti_opportunity(srslte_prach_t *p, uint32_t current_tti, int allowed_subframe) {
-{
+  uint32_t config_idx = p->config_idx;
-  uint32_t config_idx = p->config_idx; 
   // Get SFN and sf_idx from the PRACH configuration index
-  srslte_prach_sfn_t prach_sfn = srslte_prach_get_sfn(config_idx);  
+  srslte_prach_sfn_t prach_sfn = srslte_prach_get_sfn(config_idx);
 
   // This is the only option which provides always an opportunity for PRACH transmission.
-  if(config_idx == 14) {
+  if (config_idx == 14) {
     return true;
   }
 
-  if ((prach_sfn == SRSLTE_PRACH_SFN_EVEN && ((current_tti/10)%2)==0) ||
+  if ((prach_sfn == SRSLTE_PRACH_SFN_EVEN && ((current_tti / 10) % 2) == 0) ||
-      prach_sfn == SRSLTE_PRACH_SFN_ANY) 
+      prach_sfn == SRSLTE_PRACH_SFN_ANY) {
-  {
     srslte_prach_sf_config_t sf_config;
     srslte_prach_sf_config(config_idx, &sf_config);
-    for (int i=0;i<sf_config.nof_sf;i++) {
+    for (int i = 0; i < sf_config.nof_sf; i++) {
-      if (((current_tti%10) == sf_config.sf[i] && allowed_subframe == -1) || 
+      if (((current_tti % 10) == sf_config.sf[i] && allowed_subframe == -1) ||
-          ((current_tti%10) == sf_config.sf[i] && (current_tti%10) == allowed_subframe))
+          ((current_tti % 10) == sf_config.sf[i] && (current_tti % 10) == allowed_subframe)) {
-      {
+        return true;
-        return true; 
       }
     }
   }
-  return false; 
+  return false;
 }
 
-
 void srslte_prach_sf_config(uint32_t config_idx, srslte_prach_sf_config_t *sf_config) {
-  memcpy(sf_config, &prach_sf_config[config_idx%16], sizeof(srslte_prach_sf_config_t));
+  memcpy(sf_config, &prach_sf_config[config_idx % 16], sizeof(srslte_prach_sf_config_t));
 }
-  
+
-  
 // For debug use only
-void print(void *d, uint32_t size, uint32_t len, char* file_str)
+void print(void *d, uint32_t size, uint32_t len, char *file_str) {
-{
   FILE *f;
   f = fopen(file_str, "wb");
-  fwrite(d , size, len, f);
+  fwrite(d, size, len, f);
   fclose(f);
 }
 
-int srslte_prach_gen_seqs(srslte_prach_t *p)
+int srslte_prach_gen_seqs(srslte_prach_t *p) {
-{
   uint32_t u = 0;
   uint32_t v = 1;
   int v_max = 0;
@@ -243,85 +235,91 @@ int srslte_prach_gen_seqs(srslte_prach_t *p)
   uint32_t d_u = 0;
   uint32_t d_start = 0;
   uint32_t N_shift = 0;
-  int      N_neg_shift = 0;
+  int N_neg_shift = 0;
   uint32_t N_group = 0;
   uint32_t C_v = 0;
   cf_t root[839];
 
   // Generate our 64 preamble sequences
-  for(int i=0;i<N_SEQS;i++){
+  for (int i = 0; i < N_SEQS; i++) {
 
-    if(v > v_max){
+    if (v > v_max) {
       // Get a new root sequence
-      if(4 == p->f){
+      if (4 == p->f) {
-        u = prach_zc_roots_format4[(p->rsi + p->N_roots)%138];
+        u = prach_zc_roots_format4[(p->rsi + p->N_roots) % 138];
-      }else{
+      } else {
-        u = prach_zc_roots[(p->rsi + p->N_roots)%838];
+        u = prach_zc_roots[(p->rsi + p->N_roots) % 838];
       }
 
-      for(int j=0;j<p->N_zc;j++){
+      for (int j = 0; j < p->N_zc; j++) {
-        double phase = -M_PI*u*j*(j+1)/p->N_zc;
+        double phase = -M_PI * u * j * (j + 1) / p->N_zc;
-        root[j] = cexp(phase*I);
+        root[j] = cexp(phase * I);
       }
       p->root_seqs_idx[p->N_roots++] = i;
 
       // Determine v_max
-      if(p->hs){
+      if (p->hs) {
         // High-speed cell
-        for(p_=1; p_<=p->N_zc; p_++){
+        for (p_ = 1; p_ <= p->N_zc; p_++) {
-            if(((p_*u) % p->N_zc) == 1)
+          if (((p_ * u) % p->N_zc) == 1)
-                break;
+            break;
         }
-        if(p_ < p->N_zc/2){
+        if (p_ < p->N_zc / 2) {
-            d_u = p_;
+          d_u = p_;
-        }else{
+        } else {
-            d_u = p->N_zc - p_;
+          d_u = p->N_zc - p_;
         }
-        if(d_u >= p->N_cs && d_u < p->N_zc/3){
+        if (d_u >= p->N_cs && d_u < p->N_zc / 3) {
-            N_shift = d_u/p->N_cs;
+          N_shift = d_u / p->N_cs;
-            d_start = 2*d_u + N_shift*p->N_cs;
+          d_start = 2 * d_u + N_shift * p->N_cs;
-            N_group = p->N_zc/d_start;
+          N_group = p->N_zc / d_start;
-            N_neg_shift = (p->N_zc - 2*d_u - N_group*d_start)/p->N_cs;
+          if (p->N_zc > 2 * d_u + N_group * d_start) {
-            if(N_neg_shift < 0)
+            N_neg_shift = (p->N_zc - 2 * d_u - N_group * d_start) / p->N_cs;
-                N_neg_shift = 0;
+          } else {
-        }else{
+            N_neg_shift = 0;
-            N_shift = (p->N_zc - 2*d_u)/p->N_cs;
+          }
-            d_start = p->N_zc - 2*d_u + N_shift*p->N_cs;
+        } else if (p->N_zc / 3 <= d_u && d_u <= (p->N_zc - p->N_cs) / 2) {
-            N_group = d_u/d_start;
+          N_shift = (p->N_zc - 2 * d_u) / p->N_cs;
-            N_neg_shift = (d_u - N_group*d_start)/p->N_cs;
+          d_start = p->N_zc - 2 * d_u + N_shift * p->N_cs;
-            if(N_neg_shift < 0)
+          N_group = d_u / d_start;
-                N_neg_shift = 0;
+          if (d_u > N_group * d_start) {
-            if(N_neg_shift > N_shift)
+            N_neg_shift = (d_u - N_group * d_start) / p->N_cs;
-                N_neg_shift = N_shift;
+          } else {
+            N_neg_shift = 0;
+          }
+          if (N_neg_shift > N_shift)
+            N_neg_shift = N_shift;
+        } else {
+          N_shift = 0;
         }
-        v_max = N_shift*N_group + N_neg_shift - 1;
+        v_max = N_shift * N_group + N_neg_shift - 1;
-        if(v_max < 0){
+        if (v_max < 0) {
           v_max = 0;
         }
-      }else{
+      } else {
         // Normal cell
-        if(0 == p->N_cs){
+        if (0 == p->N_cs) {
           v_max = 0;
-        }else{
+        } else {
-          v_max = (p->N_zc/p->N_cs)-1;
+          v_max = (p->N_zc / p->N_cs) - 1;
         }
       }
 
-      v=0;
+      v = 0;
     }
 
     // Shift root and add to set
-    if(p->hs){
+    if (p->hs) {
-      if(N_shift==0){
+      if (N_shift == 0) {
         C_v = 0;
-      }else{
+      } else {
-        C_v = d_start*floor(v/N_shift) + (v % N_shift)*p->N_cs;
+        C_v = d_start * floor(v / N_shift) + (v % N_shift) * p->N_cs;
       }
-    }else{
+    } else {
-      C_v = v*p->N_cs;
+      C_v = v * p->N_cs;
     }
-    for(int j=0;j<p->N_zc;j++){
+    for (int j = 0; j < p->N_zc; j++) {
-      p->seqs[i][j] = root[(j+C_v) % p->N_zc];
+      p->seqs[i][j] = root[(j + C_v) % p->N_zc];
     }
 
     v++;
@@ -329,69 +327,66 @@ int srslte_prach_gen_seqs(srslte_prach_t *p)
   return 0;
 }
 
-int srslte_prach_init_cfg(srslte_prach_t *p, srslte_prach_cfg_t *cfg, uint32_t nof_prb)
+int srslte_prach_init_cfg(srslte_prach_t *p, srslte_prach_cfg_t *cfg, uint32_t nof_prb) {
-{
   if (srslte_prach_init(p, srslte_symbol_sz(nof_prb))) {
     return -1;
   }
   return srslte_prach_set_cell(p,
-                           srslte_symbol_sz(nof_prb), 
+                               srslte_symbol_sz(nof_prb),
-                           cfg->config_idx, 
+                               cfg->config_idx,
-                           cfg->root_seq_idx,
+                               cfg->root_seq_idx,
-                           cfg->hs_flag,
+                               cfg->hs_flag,
-                           cfg->zero_corr_zone);
+                               cfg->zero_corr_zone);
 }
 
-int srslte_prach_init(srslte_prach_t *p, uint32_t max_N_ifft_ul)
+int srslte_prach_init(srslte_prach_t *p, uint32_t max_N_ifft_ul) {
-{
   int ret = SRSLTE_ERROR;
-  if(p                      != NULL      &&
+  if (p != NULL &&
-     max_N_ifft_ul          <  2049)
+      max_N_ifft_ul < 2049) {
-  {
     bzero(p, sizeof(srslte_prach_t));
 
     p->max_N_ifft_ul = max_N_ifft_ul;
 
     // Set up containers
-    p->prach_bins = srslte_vec_malloc(sizeof(cf_t)*MAX_N_zc);
+    p->prach_bins = srslte_vec_malloc(sizeof(cf_t) * MAX_N_zc);
-    p->corr_spec = srslte_vec_malloc(sizeof(cf_t)*MAX_N_zc);
+    p->corr_spec = srslte_vec_malloc(sizeof(cf_t) * MAX_N_zc);
-    p->corr = srslte_vec_malloc(sizeof(float)*MAX_N_zc);
+    p->corr = srslte_vec_malloc(sizeof(float) * MAX_N_zc);
 
     // Set up ZC FFTS
-    if(srslte_dft_plan(&p->zc_fft, MAX_N_zc, SRSLTE_DFT_FORWARD, SRSLTE_DFT_COMPLEX)){
+    if (srslte_dft_plan(&p->zc_fft, MAX_N_zc, SRSLTE_DFT_FORWARD, SRSLTE_DFT_COMPLEX)) {
       return SRSLTE_ERROR;
     }
     srslte_dft_plan_set_mirror(&p->zc_fft, false);
     srslte_dft_plan_set_norm(&p->zc_fft, true);
 
-    if(srslte_dft_plan(&p->zc_ifft, MAX_N_zc, SRSLTE_DFT_BACKWARD, SRSLTE_DFT_COMPLEX)){
+    if (srslte_dft_plan(&p->zc_ifft, MAX_N_zc, SRSLTE_DFT_BACKWARD, SRSLTE_DFT_COMPLEX)) {
       return SRSLTE_ERROR;
     }
     srslte_dft_plan_set_mirror(&p->zc_ifft, false);
     srslte_dft_plan_set_norm(&p->zc_ifft, false);
 
-    uint32_t fft_size_alloc = max_N_ifft_ul * DELTA_F/DELTA_F_RA;
+    uint32_t fft_size_alloc = max_N_ifft_ul * DELTA_F / DELTA_F_RA;
 
-    p->ifft_in = (cf_t*)srslte_vec_malloc(fft_size_alloc*sizeof(cf_t));
+    p->ifft_in = (cf_t *) srslte_vec_malloc(fft_size_alloc * sizeof(cf_t));
-    p->ifft_out = (cf_t*)srslte_vec_malloc(fft_size_alloc*sizeof(cf_t));
+    p->ifft_out = (cf_t *) srslte_vec_malloc(fft_size_alloc * sizeof(cf_t));
-    if(srslte_dft_plan(&p->ifft, fft_size_alloc, SRSLTE_DFT_BACKWARD, SRSLTE_DFT_COMPLEX)) {
+    if (srslte_dft_plan(&p->ifft, fft_size_alloc, SRSLTE_DFT_BACKWARD, SRSLTE_DFT_COMPLEX)) {
       fprintf(stderr, "Error creating DFT plan\n");
       return -1;
     }
     srslte_dft_plan_set_mirror(&p->ifft, true);
     srslte_dft_plan_set_norm(&p->ifft, true);
 
-    if(srslte_dft_plan(&p->fft, fft_size_alloc, SRSLTE_DFT_FORWARD, SRSLTE_DFT_COMPLEX)){
+    if (srslte_dft_plan(&p->fft, fft_size_alloc, SRSLTE_DFT_FORWARD, SRSLTE_DFT_COMPLEX)) {
       fprintf(stderr, "Error creating DFT plan\n");
       return -1;
     }
-    
+
-    p->signal_fft = srslte_vec_malloc(sizeof(cf_t)*fft_size_alloc);
+    p->signal_fft = srslte_vec_malloc(sizeof(cf_t) * fft_size_alloc);
     if (!p->signal_fft) {
       fprintf(stderr, "Error allocating memory\n");
-      return -1; 
+      return -1;
     }
-    
+
     srslte_dft_plan_set_mirror(&p->fft, true);
     srslte_dft_plan_set_norm(&p->fft, false);
 
@@ -403,20 +398,17 @@ int srslte_prach_init(srslte_prach_t *p, uint32_t max_N_ifft_ul)
   return ret;
 }
 
-
 int srslte_prach_set_cell(srslte_prach_t *p,
-                      uint32_t N_ifft_ul,
+                          uint32_t N_ifft_ul,
-                      uint32_t config_idx,
+                          uint32_t config_idx,
-                      uint32_t root_seq_index,
+                          uint32_t root_seq_index,
-                      bool high_speed_flag,
+                          bool high_speed_flag,
-                      uint32_t zero_corr_zone_config)
+                          uint32_t zero_corr_zone_config) {
-{
   int ret = SRSLTE_ERROR;
-  if(p                      != NULL      &&
+  if (p != NULL &&
-     N_ifft_ul              <  2049      &&
+      N_ifft_ul < 2049 &&
-     config_idx             <  64        &&
+      config_idx < 64 &&
-     root_seq_index         <  MAX_ROOTS)
+      root_seq_index < MAX_ROOTS) {
-  {
     if (N_ifft_ul > p->max_N_ifft_ul) {
       fprintf(stderr, "PRACH: Error in set_cell(): N_ifft_ul must be lower or equal max_N_ifft_ul in init()\n");
       return -1;
@@ -432,7 +424,7 @@ int srslte_prach_set_cell(srslte_prach_t *p,
 
 
     // Determine N_zc and N_cs
-    if(4 == preamble_format){
+    if (4 == preamble_format) {
       if (p->zczc < 7) {
         p->N_zc = 139;
         p->N_cs = prach_Ncs_format4[p->zczc];
@@ -440,16 +432,16 @@ int srslte_prach_set_cell(srslte_prach_t *p,
         fprintf(stderr, "Invalid zeroCorrelationZoneConfig=%d for format4\n", p->zczc);
         return SRSLTE_ERROR;
       }
-    }else{
+    } else {
       p->N_zc = MAX_N_zc;
-      if(p->hs){
+      if (p->hs) {
         if (p->zczc < 15) {
           p->N_cs = prach_Ncs_restricted[p->zczc];
         } else {
           fprintf(stderr, "Invalid zeroCorrelationZoneConfig=%d for restricted set\n", p->zczc);
           return SRSLTE_ERROR;
         }
-      }else{
+      } else {
         if (p->zczc < 16) {
           p->N_cs = prach_Ncs_unrestricted[p->zczc];
         } else {
@@ -461,10 +453,10 @@ int srslte_prach_set_cell(srslte_prach_t *p,
 
     // Set up ZC FFTS
     if (p->N_zc != MAX_N_zc) {
-      if(srslte_dft_replan(&p->zc_fft, p->N_zc)){
+      if (srslte_dft_replan(&p->zc_fft, p->N_zc)) {
         return SRSLTE_ERROR;
       }
-      if(srslte_dft_replan(&p->zc_ifft, p->N_zc)){
+      if (srslte_dft_replan(&p->zc_ifft, p->N_zc)) {
         return SRSLTE_ERROR;
       }
     }
@@ -474,16 +466,16 @@ int srslte_prach_set_cell(srslte_prach_t *p,
     srslte_prach_gen_seqs(p);
 
     // Generate sequence FFTs
-    for(int i=0;i<N_SEQS;i++){
+    for (int i = 0; i < N_SEQS; i++) {
       srslte_dft_run(&p->zc_fft, p->seqs[i], p->dft_seqs[i]);
     }
 
     // Create our FFT objects and buffers
     p->N_ifft_ul = N_ifft_ul;
-    if(4 == preamble_format){
+    if (4 == preamble_format) {
-      p->N_ifft_prach = p->N_ifft_ul * DELTA_F/DELTA_F_RA_4;
+      p->N_ifft_prach = p->N_ifft_ul * DELTA_F / DELTA_F_RA_4;
-    }else{
+    } else {
-      p->N_ifft_prach = p->N_ifft_ul * DELTA_F/DELTA_F_RA;
+      p->N_ifft_prach = p->N_ifft_ul * DELTA_F / DELTA_F_RA;
     }
 
     /* The deadzone specifies the number of samples at the end of the correlation window
@@ -496,19 +488,19 @@ int srslte_prach_set_cell(srslte_prach_t *p,
       p->deadzone = (uint32_t) ceil((float) samp_rate/((float) p->N_zc*subcarrier_spacing));
     }*/
 
-    if(srslte_dft_replan(&p->ifft, p->N_ifft_prach)) {
+    if (srslte_dft_replan(&p->ifft, p->N_ifft_prach)) {
       fprintf(stderr, "Error creating DFT plan\n");
       return -1;
     }
-    if(srslte_dft_replan(&p->fft, p->N_ifft_prach)){
+    if (srslte_dft_replan(&p->fft, p->N_ifft_prach)) {
       fprintf(stderr, "Error creating DFT plan\n");
       return -1;
     }
 
-    p->N_seq = prach_Tseq[p->f]*p->N_ifft_ul/2048;
+    p->N_seq = prach_Tseq[p->f] * p->N_ifft_ul / 2048;
-    p->N_cp  = prach_Tcp[p->f]*p->N_ifft_ul/2048;
+    p->N_cp = prach_Tcp[p->f] * p->N_ifft_ul / 2048;
-    p->T_seq = prach_Tseq[p->f]*SRSLTE_LTE_TS;
+    p->T_seq = prach_Tseq[p->f] * SRSLTE_LTE_TS;
-    p->T_tot = (prach_Tseq[p->f]+prach_Tcp[p->f])*SRSLTE_LTE_TS;
+    p->T_tot = (prach_Tseq[p->f] + prach_Tcp[p->f]) * SRSLTE_LTE_TS;
 
     ret = SRSLTE_SUCCESS;
   } else {
@@ -519,44 +511,42 @@ int srslte_prach_set_cell(srslte_prach_t *p,
 }
 
 int srslte_prach_gen(srslte_prach_t *p,
-              uint32_t seq_index,
+                     uint32_t seq_index,
-              uint32_t freq_offset,
+                     uint32_t freq_offset,
-              cf_t *signal)
+                     cf_t *signal) {
-{
   int ret = SRSLTE_ERROR;
-  if(p          != NULL   &&
+  if (p != NULL &&
-     seq_index  <  N_SEQS &&
+      seq_index < N_SEQS &&
-     signal     != NULL)
+      signal != NULL) {
-  {
     // Calculate parameters
     uint32_t N_rb_ul = srslte_nof_prb(p->N_ifft_ul);
-    uint32_t k_0 = freq_offset*N_RB_SC - N_rb_ul*N_RB_SC/2 + p->N_ifft_ul/2;
+    uint32_t k_0 = freq_offset * N_RB_SC - N_rb_ul * N_RB_SC / 2 + p->N_ifft_ul / 2;
-    uint32_t K = DELTA_F/DELTA_F_RA;
+    uint32_t K = DELTA_F / DELTA_F_RA;
-    uint32_t begin = PHI + (K*k_0) + (K/2);
+    uint32_t begin = PHI + (K * k_0) + (K / 2);
 
     if (6 + freq_offset > N_rb_ul) {
       fprintf(stderr, "Error no space for PRACH: frequency offset=%d, N_rb_ul=%d\n", freq_offset, N_rb_ul);
-      return ret; 
+      return ret;
     }
-    
+
-    DEBUG("N_zc: %d, N_cp: %d, N_seq: %d, N_ifft_prach=%d begin: %d\n", 
+    DEBUG("N_zc: %d, N_cp: %d, N_seq: %d, N_ifft_prach=%d begin: %d\n",
           p->N_zc, p->N_cp, p->N_seq, p->N_ifft_prach, begin);
-    
+
     // Map dft-precoded sequence to ifft bins
-    memset(p->ifft_in, 0, begin*sizeof(cf_t));
+    memset(p->ifft_in, 0, begin * sizeof(cf_t));
     memcpy(&p->ifft_in[begin], p->dft_seqs[seq_index], p->N_zc * sizeof(cf_t));
-    memset(&p->ifft_in[begin+p->N_zc], 0, (p->N_ifft_prach - begin - p->N_zc) * sizeof(cf_t));
+    memset(&p->ifft_in[begin + p->N_zc], 0, (p->N_ifft_prach - begin - p->N_zc) * sizeof(cf_t));
 
     srslte_dft_run(&p->ifft, p->ifft_in, p->ifft_out);
 
     // Copy CP into buffer
-    memcpy(signal, &p->ifft_out[p->N_ifft_prach-p->N_cp], p->N_cp*sizeof(cf_t));
+    memcpy(signal, &p->ifft_out[p->N_ifft_prach - p->N_cp], p->N_cp * sizeof(cf_t));
 
     // Copy preamble sequence into buffer
-    for(int i=0;i<p->N_seq;i++){
+    for (int i = 0; i < p->N_seq; i++) {
-      signal[p->N_cp+i] = p->ifft_out[i%p->N_ifft_prach];
+      signal[p->N_cp + i] = p->ifft_out[i % p->N_ifft_prach];
     }
-    
+
     ret = SRSLTE_SUCCESS;
   }
 
@@ -564,7 +554,7 @@ int srslte_prach_gen(srslte_prach_t *p,
 }
 
 void srslte_prach_set_detect_factor(srslte_prach_t *p, float ratio) {
-  p->detect_factor = ratio; 
+  p->detect_factor = ratio;
 }
 
 int srslte_prach_detect(srslte_prach_t *p,
@@ -572,8 +562,7 @@ int srslte_prach_detect(srslte_prach_t *p,
                         cf_t *signal,
                         uint32_t sig_len,
                         uint32_t *indices,
-                        uint32_t *n_indices)
+                        uint32_t *n_indices) {
-{
   return srslte_prach_detect_offset(p, freq_offset, signal, sig_len, indices, NULL, NULL, n_indices);
 }
 
@@ -583,21 +572,19 @@ int srslte_prach_detect_offset(srslte_prach_t *p,
                                uint32_t sig_len,
                                uint32_t *indices,
                                float *t_offsets,
-                               float    *peak_to_avg,
+                               float *peak_to_avg,
-                               uint32_t *n_indices)
+                               uint32_t *n_indices) {
-{
   int ret = SRSLTE_ERROR;
-  if(p       != NULL &&
+  if (p != NULL &&
-     signal  != NULL &&
+      signal != NULL &&
-     sig_len > 0     &&
+      sig_len > 0 &&
-     indices != NULL)
+      indices != NULL) {
-  {
+
-    
+    if (sig_len < p->N_ifft_prach) {
-    if(sig_len < p->N_ifft_prach){
       fprintf(stderr, "srslte_prach_detect: Signal length is %d and should be %d\n", sig_len, p->N_ifft_prach);
       return SRSLTE_ERROR_INVALID_INPUTS;
     }
-    
+
     // FFT incoming signal
     srslte_dft_run(&p->fft, signal, p->signal_fft);
 
@@ -605,66 +592,65 @@ int srslte_prach_detect_offset(srslte_prach_t *p,
 
     // Extract bins of interest
     uint32_t N_rb_ul = srslte_nof_prb(p->N_ifft_ul);
-    uint32_t k_0 = freq_offset*N_RB_SC - N_rb_ul*N_RB_SC/2 + p->N_ifft_ul/2;
+    uint32_t k_0 = freq_offset * N_RB_SC - N_rb_ul * N_RB_SC / 2 + p->N_ifft_ul / 2;
-    uint32_t K = DELTA_F/DELTA_F_RA;
+    uint32_t K = DELTA_F / DELTA_F_RA;
-    uint32_t begin = PHI + (K*k_0) + (K/2);
+    uint32_t begin = PHI + (K * k_0) + (K / 2);
 
-    memcpy(p->prach_bins, &p->signal_fft[begin], p->N_zc*sizeof(cf_t));
+    memcpy(p->prach_bins, &p->signal_fft[begin], p->N_zc * sizeof(cf_t));
-    
+
-    for(int i=0;i<p->N_roots;i++){
+    for (int i = 0; i < p->N_roots; i++) {
       cf_t *root_spec = p->dft_seqs[p->root_seqs_idx[i]];
-     
+
       srslte_vec_prod_conj_ccc(p->prach_bins, root_spec, p->corr_spec, p->N_zc);
 
       srslte_dft_run(&p->zc_ifft, p->corr_spec, p->corr_spec);
-      
+
       srslte_vec_abs_square_cf(p->corr_spec, p->corr, p->N_zc);
 
-      float corr_ave = srslte_vec_acc_ff(p->corr, p->N_zc)/p->N_zc;
+      float corr_ave = srslte_vec_acc_ff(p->corr, p->N_zc) / p->N_zc;
-      
+
       uint32_t winsize = 0;
-      if(p->N_cs != 0){
+      if (p->N_cs != 0) {
         winsize = p->N_cs;
-      }else{
+      } else {
         winsize = p->N_zc;
       }
-      uint32_t n_wins = p->N_zc/winsize;
+      uint32_t n_wins = p->N_zc / winsize;
-
+
-      float max_peak = 0; 
+      float max_peak = 0;
-      for(int j=0;j<n_wins;j++) {
+      for (int j = 0; j < n_wins; j++) {
-        uint32_t start = (p->N_zc-(j*p->N_cs))%p->N_zc;
+        uint32_t start = (p->N_zc - (j * p->N_cs)) % p->N_zc;
-        uint32_t end = start+winsize;
+        uint32_t end = start + winsize;
-        if (end>p->deadzone) {
+        if (end > p->deadzone) {
-          end-=p->deadzone;
+          end -= p->deadzone;
         }
         start += p->deadzone;
         p->peak_values[j] = 0;
-        for(int k=start;k<end;k++) {
+        for (int k = start; k < end; k++) {
-          if(p->corr[k] > p->peak_values[j]) {
+          if (p->corr[k] > p->peak_values[j]) {
-            p->peak_values[j]  = p->corr[k];
+            p->peak_values[j] = p->corr[k];
-            p->peak_offsets[j] = k-start; 
+            p->peak_offsets[j] = k - start;
             if (p->peak_values[j] > max_peak) {
               max_peak = p->peak_values[j];
             }
           }
         }
       }
-      if (max_peak > p->detect_factor*corr_ave) {
+      if (max_peak > p->detect_factor * corr_ave) {
-        for (int j=0;j<n_wins;j++) {
+        for (int j = 0; j < n_wins; j++) {
-          if(p->peak_values[j] > p->detect_factor*corr_ave)
+          if (p->peak_values[j] > p->detect_factor * corr_ave) {
-          {
             //printf("saving prach correlation\n");
             //memcpy(save_corr, p->corr, p->N_zc*sizeof(float));
             if (indices) {
-              indices[*n_indices]     = (i*n_wins)+j;
+              indices[*n_indices] = (i * n_wins) + j;
             }
             if (peak_to_avg) {
-              peak_to_avg[*n_indices] = p->peak_values[j]/corr_ave; 
+              peak_to_avg[*n_indices] = p->peak_values[j] / corr_ave;
             }
             if (t_offsets) {
-              t_offsets[*n_indices] = (float) p->peak_offsets[j]*p->T_seq/p->N_zc; 
+              t_offsets[*n_indices] = (float) p->peak_offsets[j] * p->T_seq / p->N_zc;
-            }            
+            }
-            (*n_indices)++;          
+            (*n_indices)++;
           }
         }
       }
@@ -687,41 +673,37 @@ int srslte_prach_free(srslte_prach_t *p) {
   srslte_dft_plan_free(&p->zc_ifft);
 
   if (p->signal_fft) {
-    free(p->signal_fft); 
+    free(p->signal_fft);
   }
-  
+
   bzero(p, sizeof(srslte_prach_t));
 
   return 0;
 }
 
-int srslte_prach_print_seqs(srslte_prach_t *p)
+int srslte_prach_print_seqs(srslte_prach_t *p) {
-{
+  for (int i = 0; i < N_SEQS; i++) {
-  for(int i=0; i<N_SEQS;i++)
-  {
     FILE *f;
     char str[32];
     sprintf(str, "prach_seq_%d.bin", i);
     f = fopen(str, "wb");
-    fwrite(p->seqs[i] , sizeof(cf_t), p->N_zc, f);
+    fwrite(p->seqs[i], sizeof(cf_t), p->N_zc, f);
     fclose(f);
   }
-  for(int i=0; i<N_SEQS;i++)
+  for (int i = 0; i < N_SEQS; i++) {
-  {
     FILE *f;
     char str[32];
     sprintf(str, "prach_dft_seq_%d.bin", i);
     f = fopen(str, "wb");
-    fwrite(p->dft_seqs[i] , sizeof(cf_t), p->N_zc, f);
+    fwrite(p->dft_seqs[i], sizeof(cf_t), p->N_zc, f);
     fclose(f);
   }
-  for(int i=0;i<p->N_roots;i++)
+  for (int i = 0; i < p->N_roots; i++) {
-  {
     FILE *f;
     char str[32];
     sprintf(str, "prach_root_seq_%d.bin", i);
     f = fopen(str, "wb");
-    fwrite(p->seqs[p->root_seqs_idx[i]] , sizeof(cf_t), p->N_zc, f);
+    fwrite(p->seqs[p->root_seqs_idx[i]], sizeof(cf_t), p->N_zc, f);
     fclose(f);
   }
   return 0;

lib/src/phy/phch/pucch.c

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

lib/src/phy/phch/pusch.c

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

lib/src/phy/phch/sch.c

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

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

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

lib/src/phy/rf/rf_uhd_imp.c

@@ -35,6 +35,8 @@
 #include "srslte/phy/rf/rf.h"
 #include "uhd_c_api.h"
 
+#define HAVE_ASYNC_THREAD 0
+
 typedef struct {
   char *devname; 
   uhd_usrp_handle usrp;
@@ -80,13 +82,14 @@ static void log_overflow(rf_uhd_handler_t *h) {
 static void log_late(rf_uhd_handler_t *h, bool is_rx) {
   if (h->uhd_error_handler) {
     srslte_rf_error_t error;
-    error.opt = is_rx?1:0;
     bzero(&error, sizeof(srslte_rf_error_t));
+    error.opt = is_rx?1:0;
     error.type = SRSLTE_RF_ERROR_LATE;
     h->uhd_error_handler(error);
   }
 }
 
+#if HAVE_ASYNC_THREAD
 static void log_underflow(rf_uhd_handler_t *h) {  
   if (h->uhd_error_handler) {
     srslte_rf_error_t error; 
@@ -95,7 +98,22 @@ static void log_underflow(rf_uhd_handler_t *h) {
     h->uhd_error_handler(error);
   }
 }
+#endif
+
+static void log_rx_error(rf_uhd_handler_t *h) {
+  if (h->uhd_error_handler) {
+    char error_string[512];
+    uhd_usrp_last_error(h->usrp, error_string, 512);
+    fprintf(stderr, "USRP reported the following error: %s\n", error_string);
+
+    srslte_rf_error_t error;
+    bzero(&error, sizeof(srslte_rf_error_t));
+    error.type = SRSLTE_RF_ERROR_RX;
+    h->uhd_error_handler(error);
+  }
+}
 
+#if HAVE_ASYNC_THREAD
 static void* async_thread(void *h) {
   rf_uhd_handler_t *handler = (rf_uhd_handler_t*) h; 
   uhd_async_metadata_handle md; 
@@ -122,6 +140,7 @@ static void* async_thread(void *h) {
   uhd_async_metadata_free(&md);
   return NULL; 
 }
+#endif
 
 void rf_uhd_suppress_stdout(void *h) {
   rf_uhd_register_msg_handler_c(suppress_handler);
@@ -334,11 +353,12 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
       perror("malloc");
       return -1; 
     }
+    bzero(handler, sizeof(rf_uhd_handler_t));
     *h = handler; 
-    
+
     /* Set priority to UHD threads */
     uhd_set_thread_priority(uhd_default_thread_priority, true);
-    
+
     /* Find available devices */
     uhd_string_vector_handle devices_str;
     uhd_string_vector_make(&devices_str);
@@ -556,12 +576,14 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
     rf_uhd_set_rx_gain(handler, max_gain*0.7);
     uhd_meta_range_free(&gain_range);
 
+#if HAVE_ASYNC_THREAD
     // Start low priority thread to receive async commands
-    handler->async_thread_running = true; 
+    handler->async_thread_running = true;
     if (pthread_create(&handler->async_thread, NULL, async_thread, handler)) {
       perror("pthread_create");
       return -1; 
     }
+#endif
 
     /* Restore priorities  */
     uhd_set_thread_priority(0, false);
@@ -738,6 +760,7 @@ int rf_uhd_recv_with_time_multi(void *h,
                                              num_rx_samples, md, 1.0, false, &rxd_samples);
       if (error) {
         fprintf(stderr, "Error receiving from UHD: %d\n", error);
+        log_rx_error(handler);
         return -1; 
       }
 
@@ -760,8 +783,12 @@ int rf_uhd_recv_with_time_multi(void *h,
       }
     }
   } else {
-    return uhd_rx_streamer_recv(handler->rx_stream, data, 
+    uhd_error error = uhd_rx_streamer_recv(handler->rx_stream, data, nsamples, md, 0.0, false, &rxd_samples);
-                                nsamples, md, 0.0, false, &rxd_samples);
+    if (error) {
+      fprintf(stderr, "Error receiving from UHD: %d\n", error);
+      log_rx_error(handler);
+      return -1;
+    }
   }
   if (secs && frac_secs) {
     uhd_rx_metadata_time_spec(handler->rx_md_first, secs, frac_secs);

lib/src/phy/rf/rf_utils.c

@@ -186,9 +186,11 @@ int rf_cell_search(srslte_rf_t *rf, uint32_t nof_rx_antennas,
     ret = srslte_ue_cellsearch_scan(&cs, found_cells, &max_peak_cell); 
   }
   if (ret < 0) {
+    srslte_rf_stop_rx_stream(rf);
     fprintf(stderr, "Error searching cell\n");
     return SRSLTE_ERROR;
   } else if (ret == 0) {
+    srslte_rf_stop_rx_stream(rf);
     fprintf(stderr, "Could not find any cell in this frequency\n");
     return SRSLTE_SUCCESS;
   }

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

lib/src/phy/sync/test/pss_usrp.c

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

lib/src/phy/ue/ue_dl.c

@@ -71,7 +71,6 @@ int srslte_ue_dl_init(srslte_ue_dl_t *q,
     q->pmch_pkt_errors = 0;
     q->pmch_pkts_total = 0;
     q->pending_ul_dci_rnti = 0; 
-    q->sample_offset = 0; 
     q->nof_rx_antennas = nof_rx_antennas;
 
     for (int j = 0; j < SRSLTE_MAX_PORTS; j++) {
@@ -147,12 +146,7 @@ int srslte_ue_dl_init(srslte_ue_dl_t *q,
         goto clean_exit;
       }
     }
-    if (srslte_cfo_init(&q->sfo_correct, max_prb*SRSLTE_NRE)) {
+
-      fprintf(stderr, "Error initiating SFO correct\n");
-      goto clean_exit;
-    }
-    srslte_cfo_set_tol(&q->sfo_correct, 1e-5f/q->fft[0].symbol_sz);
-    
     ret = SRSLTE_SUCCESS;
   } else {
     fprintf(stderr, "Invalid parametres\n");
@@ -178,7 +172,6 @@ void srslte_ue_dl_free(srslte_ue_dl_t *q) {
     srslte_pdcch_free(&q->pdcch);
     srslte_pdsch_free(&q->pdsch);
     srslte_pmch_free(&q->pmch);
-    srslte_cfo_free(&q->sfo_correct);
     for (int i = 0; i < SRSLTE_MAX_TB; i++) {
       srslte_softbuffer_rx_free(q->softbuffers[i]);
       if (q->softbuffers[i]) {
@@ -209,7 +202,6 @@ int srslte_ue_dl_set_cell(srslte_ue_dl_t *q, srslte_cell_t cell)
     q->pkt_errors = 0;
     q->pkts_total = 0;
     q->pending_ul_dci_rnti = 0;
-    q->sample_offset = 0;
 
     if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
       if (q->cell.nof_prb != 0) {
@@ -220,11 +212,6 @@ int srslte_ue_dl_set_cell(srslte_ue_dl_t *q, srslte_cell_t cell)
         fprintf(stderr, "Error resizing REGs\n");
         return SRSLTE_ERROR;
       }
-      if (srslte_cfo_resize(&q->sfo_correct, q->cell.nof_prb*SRSLTE_NRE)) {
-        fprintf(stderr, "Error resizing SFO correct\n");
-        return SRSLTE_ERROR;
-      }
-      srslte_cfo_set_tol(&q->sfo_correct, 1e-5f/q->fft[0].symbol_sz);
       for (int port = 0; port < q->nof_rx_antennas; port++) {
         if (srslte_ofdm_rx_set_prb(&q->fft[port], q->cell.cp, q->cell.nof_prb)) {
           fprintf(stderr, "Error resizing FFT\n");
@@ -348,10 +335,6 @@ void srslte_ue_dl_reset(srslte_ue_dl_t *q) {
   bzero(&q->pdsch_cfg, sizeof(srslte_pdsch_cfg_t));
 }
 
-void srslte_ue_dl_set_sample_offset(srslte_ue_dl_t * q, float sample_offset) {
-  q->sample_offset = sample_offset; 
-}
-
 /** Applies the following operations to a subframe of synchronized samples: 
  *    - OFDM demodulation
  *    - Channel estimation 
@@ -395,17 +378,6 @@ int srslte_ue_dl_decode_fft_estimate_noguru(srslte_ue_dl_t *q, cf_t *input[SRSLT
     /* Run FFT for all subframe data */
     for (int j=0;j<q->nof_rx_antennas;j++) {
       srslte_ofdm_rx_sf_ng(&q->fft[j], input[j], q->sf_symbols_m[j]);
-
-      /* Correct SFO multiplying by complex exponential in the time domain */
-      if (q->sample_offset) {
-        int nsym = SRSLTE_CP_NSYMB(q->cell.cp);
-        for (int i=0;i<2*nsym;i++) {
-          srslte_cfo_correct(&q->sfo_correct,
-                             &q->sf_symbols_m[j][i*q->cell.nof_prb*SRSLTE_NRE],
-                             &q->sf_symbols_m[j][i*q->cell.nof_prb*SRSLTE_NRE],
-                             q->sample_offset / q->fft[j].symbol_sz);
-        }
-      }
     }
     return srslte_ue_dl_decode_estimate_mbsfn(q, sf_idx, cfi, SRSLTE_SF_NORM);
   } else {
@@ -460,15 +432,15 @@ int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint3
         pmi = grant->pinfo - 1;
       } else {
         ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", nof_tb, grant->pinfo);
-        return SRSLTE_ERROR;
+        pmi = grant->pinfo % 4;
       }
     } else {
-      if (grant->pinfo < 2) {
+      if (grant->pinfo == 2) {
-        pmi = grant->pinfo;
+        ERROR("Not implemented codebook index (nof_tb=%d, pinfo=%d)", nof_tb, grant->pinfo);
-      } else {
+      } else if (grant->pinfo > 2) {
-        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", nof_tb, grant->pinfo);
+        ERROR("Reserved codebook index (nof_tb=%d, pinfo=%d)", nof_tb, grant->pinfo);
-        return SRSLTE_ERROR;
       }
+      pmi = grant->pinfo % 2;
     }
   }
   if(SRSLTE_SF_MBSFN == grant->sf_type) {

lib/src/phy/ue/ue_sync.c

@@ -144,18 +144,11 @@ void srslte_ue_sync_reset(srslte_ue_sync_t *q) {
 
 
 int srslte_ue_sync_start_agc(srslte_ue_sync_t *q, double (set_gain_callback)(void*, double), float init_gain_value) {
-  uint32_t nframes; 
+  int n = srslte_agc_init_uhd(&q->agc, SRSLTE_AGC_MODE_PEAK_AMPLITUDE, 0, set_gain_callback, q->stream);
-  if (q->nof_recv_sf == 1) {
-    nframes = 10; 
-  } else {
-    nframes = 0; 
-  }
-  int n = srslte_agc_init_uhd(&q->agc, SRSLTE_AGC_MODE_PEAK_AMPLITUDE, nframes, set_gain_callback, q->stream); 
   q->do_agc = n==0?true:false;
   if (q->do_agc) {
     srslte_agc_set_gain(&q->agc, init_gain_value);
-    srslte_agc_set_target(&q->agc, 0.3);
+    srslte_ue_sync_set_agc_period(q, 4);
-    srslte_agc_set_bandwidth(&q->agc, 0.8);
   }
   return n; 
 }
@@ -329,7 +322,6 @@ int srslte_ue_sync_set_cell(srslte_ue_sync_t *q, srslte_cell_t cell)
     memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
     q->fft_size = srslte_symbol_sz(q->cell.nof_prb);
     q->sf_len = SRSLTE_SF_LEN(q->fft_size);
-    q->agc_period = 0;
 
     if (cell.id == 1000) {
 
@@ -763,7 +755,9 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
           if (q->do_agc) {
             srslte_agc_process(&q->agc, input_buffer[0], q->sf_len);        
           }
-          
+
+          INFO("SYNC FIND: sf_idx=%d, ret=%d, next_state=%d\n", q->sf_idx, ret, q->state);
+
         break;
         case SF_TRACK:
          
@@ -789,7 +783,7 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
             if (q->do_agc && (q->agc_period == 0 || 
                              (q->agc_period && (q->frame_total_cnt%q->agc_period) == 0))) 
             {
-              srslte_agc_process(&q->agc, input_buffer[0], q->sf_len);        
+              srslte_agc_process(&q->agc, input_buffer[0], q->sf_len);
             }
 
             /* Track PSS/SSS around the expected PSS position
@@ -825,6 +819,9 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
 
             q->frame_total_cnt++;
           }
+
+          INFO("SYNC TRACK: sf_idx=%d, ret=%d, next_state=%d\n", q->sf_idx, ret, q->state);
+
         break;
       }
     }

lib/src/phy/ue/ue_ul.c

@@ -355,7 +355,7 @@ int srslte_ue_ul_pucch_encode(srslte_ue_ul_t *q, srslte_uci_data_t uci_data,
     }
     
     if (q->normalize_en) {
-      float norm_factor = (float) 0.8*q->cell.nof_prb/5;
+      float norm_factor = (float) q->cell.nof_prb/15/10;
       srslte_vec_sc_prod_cfc(output_signal, norm_factor, output_signal, SRSLTE_SF_LEN_PRB(q->cell.nof_prb));
     }
     ret = SRSLTE_SUCCESS; 

lib/src/phy/utils/ringbuffer.c

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

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

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

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

@@ -265,6 +265,7 @@ TEST(srslte_vec_sum_fff,
 
          free(x);
          free(y);
+         free(z);
 )
 
 TEST(srslte_vec_sub_fff,
@@ -287,6 +288,7 @@ TEST(srslte_vec_sub_fff,
 
          free(x);
          free(y);
+         free(z);
 )
 
 TEST(srslte_vec_dot_prod_ccc,
@@ -354,6 +356,7 @@ TEST(srslte_vec_prod_ccc,
   }
 
   free(x);
+  free(y);
   free(z);
 )
 
@@ -407,6 +410,7 @@ TEST(srslte_vec_prod_conj_ccc,
   }
 
   free(x);
+  free(y);
   free(z);
 )
 
@@ -452,6 +456,28 @@ TEST(srslte_vec_convert_fi,
   free(z);
 )
 
+TEST(srslte_vec_convert_if,
+  MALLOC(int16_t, x);
+  MALLOC(float, z);
+      float scale = 1000.0f;
+
+  float gold;
+  float k = 1.0f/scale;
+  for (int i = 0; i < block_size; i++) {
+    x[i] = (int16_t) RANDOM_S();
+  }
+
+  TEST_CALL(srslte_vec_convert_if(x, scale, z, block_size))
+
+  for (int i = 0; i < block_size; i++) {
+      gold = ((float)x[i]) * k;
+      mse += fabsf(gold - z[i]);
+  }
+
+  free(x);
+  free(z);
+)
+
 TEST(srslte_vec_prod_fff,
   MALLOC(float, x);
   MALLOC(float, y);
@@ -596,7 +622,7 @@ TEST(srslte_vec_div_ccc,
 
          for (int i = 0; i < block_size; i++) {
            gold = x[i] / y[i];
-           mse += cabsf(gold - z[i]);
+           mse += cabsf(gold - z[i]) / cabsf(gold);
          }
          mse /= block_size;
 
@@ -614,14 +640,14 @@ TEST(srslte_vec_div_cfc,
          cf_t gold;
          for (int i = 0; i < block_size; i++) {
            x[i] = RANDOM_CF();
-           y[i] = RANDOM_F();
+           y[i] = RANDOM_F() + 0.0001f;
          }
 
          TEST_CALL(srslte_vec_div_cfc(x, y, z, block_size))
 
          for (int i = 0; i < block_size; i++) {
            gold = x[i] / y[i];
-           mse += cabsf(gold - z[i])/cabsf(gold);
+           mse += cabsf(gold - z[i]) / cabsf(gold);
          }
          mse /= block_size;
 
@@ -638,15 +664,15 @@ TEST(srslte_vec_div_fff,
 
          cf_t gold;
          for (int i = 0; i < block_size; i++) {
-           x[i] = RANDOM_F() + 0.0001;
+           x[i] = RANDOM_F();
-           y[i] = RANDOM_F()+ 0.0001;
+           y[i] = RANDOM_F() + 0.0001f;
          }
 
          TEST_CALL(srslte_vec_div_fff(x, y, z, block_size))
 
          for (int i = 0; i < block_size; i++) {
            gold = x[i] / y[i];
-           mse += cabsf(gold - z[i]);
+           mse += cabsf(gold - z[i]) / cabsf(gold);
          }
          mse /= block_size;
 
@@ -753,6 +779,9 @@ int main(int argc, char **argv) {
     passed[func_count][size_count] = test_srslte_vec_convert_fi(func_names[func_count], &timmings[func_count][size_count], block_size);
     func_count++;
 
+     passed[func_count][size_count] = test_srslte_vec_convert_if(func_names[func_count], &timmings[func_count][size_count], block_size);
+    func_count++;
+
     passed[func_count][size_count] = test_srslte_vec_prod_fff(func_names[func_count], &timmings[func_count][size_count], block_size);
     func_count++;
 

lib/src/phy/utils/vector.c

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

lib/src/phy/utils/vector_simd.c

@@ -228,6 +228,36 @@ void srslte_vec_lut_sss_simd(const short *x, const unsigned short *lut, short *y
   }
 }
 
+void srslte_vec_convert_if_simd(const int16_t *x, float *z, const float scale, const int len) {
+  int i = 0;
+  const float gain = 1.0f / scale;
+
+#ifdef LV_HAVE_SSE
+  __m128 s = _mm_set1_ps(gain);
+  if (SRSLTE_IS_ALIGNED(z)) {
+    for (; i < len - 3; i += 4) {
+      __m64 *ptr = (__m64 *) &x[i];
+      __m128 fl = _mm_cvtpi16_ps(*ptr);
+      __m128 v = _mm_mul_ps(fl, s);
+
+      _mm_store_ps(&z[i], v);
+    }
+  } else {
+    for (; i < len - 3; i += 4) {
+      __m64 *ptr = (__m64 *) &x[i];
+      __m128 fl = _mm_cvtpi16_ps(*ptr);
+      __m128 v = _mm_mul_ps(fl, s);
+
+      _mm_storeu_ps(&z[i], v);
+    }
+  }
+#endif /* LV_HAVE_SSE */
+
+  for (; i < len; i++) {
+    z[i] = ((float) x[i]) * gain;
+  }
+}
+
 void srslte_vec_convert_fi_simd(const float *x, int16_t *z, const float scale, const int len) {
   int i = 0;
 
@@ -1220,4 +1250,4 @@ void srslte_vec_interleave_add_simd(const cf_t *x, const cf_t *y, cf_t *z, const
     z[k++] += x[i];
     z[k++] += y[i];
   }
-}
+}

lib/src/radio/radio.cc

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

lib/src/radio/radio_multi.cc

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

lib/src/upper/pdcp.cc

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

lib/src/upper/rlc.cc

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

lib/src/upper/rlc_am.cc

@@ -68,6 +68,20 @@ rlc_am::rlc_am() : tx_sdu_queue(16)
   do_status     = false;
 }
 
+rlc_am::~rlc_am()
+{
+  // reset RLC and dealloc SDUs
+  stop();
+
+  if(rx_sdu) {
+    pool->deallocate(rx_sdu);
+  }
+
+  if(tx_sdu) {
+    pool->deallocate(tx_sdu);
+  }
+}
+
 void rlc_am::init(srslte::log                 *log_,
                   uint32_t                     lcid_,
                   srsue::pdcp_interface_rlc   *pdcp_,
@@ -112,10 +126,12 @@ void rlc_am::reset()
 
   pthread_mutex_lock(&mutex);
   reordering_timeout.reset();
-  if(tx_sdu)
+  if(tx_sdu) {
-    tx_sdu->reset();
+    pool->deallocate(tx_sdu);
-  if(rx_sdu)
+  }
-    rx_sdu->reset();
+  if(rx_sdu) {
+    pool->deallocate(rx_sdu);
+  }
 
   vt_a    = 0;
   vt_ms   = RLC_AM_WINDOW_SIZE;
@@ -182,7 +198,7 @@ uint32_t rlc_am::get_bearer()
 void rlc_am::write_sdu(byte_buffer_t *sdu)
 {
   tx_sdu_queue.write(sdu);
-  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU, tx_sdu_len=%d", rrc->get_rb_name(lcid).c_str(), tx_sdu_queue.size());
+  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU (%d B, tx_sdu_queue_len=%d)", rrc->get_rb_name(lcid).c_str(), sdu->N_bytes, tx_sdu_queue.size());
 }
 
 /****************************************************************************
@@ -257,6 +273,27 @@ uint32_t rlc_am::get_buffer_state()
     goto unlock_and_return;
   }
 
+  // check if pollRetx timer expired (Section 5.2.2.3 in TS 36.322)
+  if (poll_retx()) {
+    // if both tx and retx buffer are empty, retransmit next PDU to be ack'ed
+    log->info("Poll reTx timer expired (lcid=%d)\n", lcid);
+    if ((tx_window.size() > 0 && retx_queue.size() == 0 && tx_sdu_queue.size() == 0)) {
+      std::map<uint32_t, rlc_amd_tx_pdu_t>::iterator it = tx_window.find(vt_s - 1);
+      if (it != tx_window.end()) {
+        log->info("Schedule last PDU (SN=%d) for reTx.\n", vt_s - 1);
+        rlc_amd_retx_t retx;
+        retx.is_segment = false;
+        retx.so_start = 0;
+        retx.so_end = tx_window[vt_s - 1].buf->N_bytes;
+        retx.sn = vt_s - 1;
+        retx_queue.push_back(retx);
+      } else {
+        log->error("Found invalid PDU in tx_window.\n");
+      }
+      poll_retx_timeout.start(cfg.t_poll_retx);
+    }
+  }
+
   // Bytes needed for retx
   if(retx_queue.size() > 0) {
     rlc_amd_retx_t retx = retx_queue.front();
@@ -305,7 +342,7 @@ int rlc_am::read_pdu(uint8_t *payload, uint32_t nof_bytes)
   pthread_mutex_lock(&mutex);
 
   log->debug("MAC opportunity - %d bytes\n", nof_bytes);
-  log->debug("tx_window size - %d PDUs\n", tx_window.size());
+  log->debug("tx_window size - %zu PDUs\n", tx_window.size());
 
   // Tx STATUS if requested
   if(do_status && !status_prohibited()) {
@@ -587,7 +624,8 @@ 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-2;
+
+  pdu_space = nof_bytes-head_len;
   if(pdu_space < (retx.so_end-retx.so_start))
     retx.so_end = retx.so_start+pdu_space;
 
@@ -603,10 +641,13 @@ int rlc_am::build_segment(uint8_t *payload, uint32_t nof_bytes, rlc_amd_retx_t r
     if(lower >= retx.so_end)
       break;
 
+    if(pdu_space <= 2)
+      break;
+
     upper += old_header.li[i];
 
     head_len    = rlc_am_packed_length(&new_header);
-    pdu_space   = nof_bytes-head_len-2;
+    pdu_space   = nof_bytes-head_len;
     if(pdu_space < (retx.so_end-retx.so_start))
       retx.so_end = retx.so_start+pdu_space;
 
@@ -657,7 +698,7 @@ int rlc_am::build_segment(uint8_t *payload, uint32_t nof_bytes, rlc_amd_retx_t r
   if(pdu_len > (int)nof_bytes) {
     log->error("%s Retx PDU segment length error. Available: %d, Used: %d\n",
                rrc->get_rb_name(lcid).c_str(), nof_bytes, pdu_len);
-    log->debug("%s Retx PDU segment length error. Header len: %d, Payload len: %d, N_li: %d\n",
+    log->debug("%s Retx PDU segment length error. Header len: %ld, Payload len: %d, N_li: %d\n",
                rrc->get_rb_name(lcid).c_str(), (ptr-payload), len, new_header.N_li);
   }
   return pdu_len;
@@ -786,6 +827,12 @@ int  rlc_am::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
                rrc->get_rb_name(lcid).c_str(), to_move, pdu_space, head_len);
   }
 
+  // Make sure, at least one SDU (segment) has been added until this point
+  if (pdu->N_bytes == 0) {
+    log->error("Generated empty RLC PDU.\n");
+    return 0;
+  }
+
   if(tx_sdu)
     header.fi |= RLC_FI_FIELD_NOT_END_ALIGNED; // Last byte does not correspond to last byte of SDU
 
@@ -807,7 +854,6 @@ int  rlc_am::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
   // Set SN
   header.sn = vt_s;
   vt_s = (vt_s + 1)%MOD;
-  log->info("%s PDU scheduled for tx. SN: %d\n", rrc->get_rb_name(lcid).c_str(), header.sn);
 
   // Place PDU in tx_window, write header and TX
   tx_window[header.sn].buf        = pdu;
@@ -818,6 +864,7 @@ int  rlc_am::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
   uint8_t *ptr = payload;
   rlc_am_write_data_pdu_header(&header, &ptr);
   memcpy(ptr, pdu->msg, pdu->N_bytes);
+  log->info_hex(payload, pdu->N_bytes, "%s PDU scheduled for tx. SN: %d (%d B)\n", rrc->get_rb_name(lcid).c_str(), header.sn, pdu->N_bytes);
 
   debug_state();
   return (ptr-payload) + pdu->N_bytes;
@@ -827,8 +874,8 @@ void rlc_am::handle_data_pdu(uint8_t *payload, uint32_t nof_bytes, rlc_amd_pdu_h
 {
   std::map<uint32_t, rlc_amd_rx_pdu_t>::iterator it;
 
-  log->info_hex(payload, nof_bytes, "%s Rx data PDU SN: %d",
+  log->info_hex(payload, nof_bytes, "%s Rx data PDU SN: %d (%d B), %s",
-                rrc->get_rb_name(lcid).c_str(), header.sn);
+                rrc->get_rb_name(lcid).c_str(), header.sn, nof_bytes, rlc_fi_field_text[header.fi]);
 
   if(!inside_rx_window(header.sn)) {
     if(header.p) {
@@ -1116,38 +1163,55 @@ void rlc_am::reassemble_rx_sdus()
 #endif
     }
   }
+
   // Iterate through rx_window, assembling and delivering SDUs
   while(rx_window.end() != rx_window.find(vr_r))
   {
     // Handle any SDU segments
     for(uint32_t i=0; i<rx_window[vr_r].header.N_li; i++)
     {
-      int len = rx_window[vr_r].header.li[i];
+      uint32_t len = rx_window[vr_r].header.li[i];
-      memcpy(&rx_sdu->msg[rx_sdu->N_bytes], rx_window[vr_r].buf->msg, len);
+      if (rx_sdu->get_tailroom() >= len) {
-      rx_sdu->N_bytes += len;
+        memcpy(&rx_sdu->msg[rx_sdu->N_bytes], rx_window[vr_r].buf->msg, len);
-      rx_window[vr_r].buf->msg += len;
+        rx_sdu->N_bytes += len;
-      rx_window[vr_r].buf->N_bytes -= len;
+        rx_window[vr_r].buf->msg += len;
-      log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU", rrc->get_rb_name(lcid).c_str());
+        rx_window[vr_r].buf->N_bytes -= len;
-      rx_sdu->set_timestamp();
+        log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU (%d B)", rrc->get_rb_name(lcid).c_str(), rx_sdu->N_bytes);
-      pdcp->write_pdu(lcid, rx_sdu);
+        rx_sdu->set_timestamp();
-      rx_sdu = pool_allocate;
+        pdcp->write_pdu(lcid, rx_sdu);
-      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() (2)\n");
+          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");
+          log->error("Fatal Error: Could not allocate PDU in reassemble_rx_sdus() (2)\n");
-        return;
+          return;
 #endif
+        }
+      } else {
+        log->error("Cannot fit RLC PDU in SDU buffer, dropping both.\n");
+        pool->deallocate(rx_sdu);
+        pool->deallocate(rx_window[vr_r].buf);
+        rx_window.erase(vr_r);
       }
     }
 
     // Handle last segment
-    memcpy(&rx_sdu->msg[rx_sdu->N_bytes], rx_window[vr_r].buf->msg, rx_window[vr_r].buf->N_bytes);
+    uint32_t len = rx_window[vr_r].buf->N_bytes;
-    rx_sdu->N_bytes += rx_window[vr_r].buf->N_bytes;
+    if (rx_sdu->get_tailroom() >= len) {
-    if(rlc_am_end_aligned(rx_window[vr_r].header.fi))
+      memcpy(&rx_sdu->msg[rx_sdu->N_bytes], rx_window[vr_r].buf->msg, len);
-    {
+      rx_sdu->N_bytes += rx_window[vr_r].buf->N_bytes;
-      log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU", rrc->get_rb_name(lcid).c_str());
+    } else {
+      log->error("Cannot fit RLC PDU in SDU buffer, dropping both.\n");
+      pool->deallocate(rx_sdu);
+      pool->deallocate(rx_window[vr_r].buf);
+      rx_window.erase(vr_r);
+    }
+
+    if(rlc_am_end_aligned(rx_window[vr_r].header.fi)) {
+      log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU (%d B)", rrc->get_rb_name(lcid).c_str(), rx_sdu->N_bytes);
       rx_sdu->set_timestamp();
       pdcp->write_pdu(lcid, rx_sdu);
       rx_sdu = pool_allocate;
@@ -1209,7 +1273,7 @@ void rlc_am::print_rx_segments()
   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;
+      ss << "    SN:" << segit->header.sn << " SO:" << segit->header.so << " N:" << segit->buf->N_bytes <<  " N_li: " << segit->header.N_li << std::endl;
     }
   }
   log->debug("%s\n", ss.str().c_str());
@@ -1217,15 +1281,33 @@ void rlc_am::print_rx_segments()
 
 bool rlc_am::add_segment_and_check(rlc_amd_rx_pdu_segments_t *pdu, rlc_amd_rx_pdu_t *segment)
 {
-  // Ordered insert
+  // Check for first segment
-  std::list<rlc_amd_rx_pdu_t>::iterator tmpit;
+  if(0 == segment->header.so) {
-  std::list<rlc_amd_rx_pdu_t>::iterator it = pdu->segments.begin();
+    std::list<rlc_amd_rx_pdu_t>::iterator it;
-  while(it != pdu->segments.end() && it->header.so < segment->header.so)
+    for(it = pdu->segments.begin(); it != pdu->segments.end(); it++) {
-    it++;
+      pool->deallocate(it->buf);
-  pdu->segments.insert(it, *segment);
+    }
+    pdu->segments.clear();
+    pdu->segments.push_back(*segment);
+    return false;
+  }
+
+  // Check segment offset
+  uint32_t n = 0;
+  if(!pdu->segments.empty()) {
+    rlc_amd_rx_pdu_t &back = pdu->segments.back();
+    n = back.header.so + back.buf->N_bytes;
+  }
+  if(segment->header.so != n) {
+    pool->deallocate(segment->buf);
+    return false;
+  } else {
+    pdu->segments.push_back(*segment);
+  }
 
   // Check for complete
   uint32_t so = 0;
+  std::list<rlc_amd_rx_pdu_t>::iterator it, tmpit;
   for(it = pdu->segments.begin(); it != pdu->segments.end(); it++) {
     if(so != it->header.so)
       return false;
@@ -1287,6 +1369,7 @@ bool rlc_am::add_segment_and_check(rlc_amd_rx_pdu_segments_t *pdu, rlc_amd_rx_pd
   }
 
   handle_data_pdu(full_pdu->msg, full_pdu->N_bytes, header);
+  pool->deallocate(full_pdu);
   return true;
 }
 

lib/src/upper/rlc_tm.cc

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

lib/src/upper/rlc_um.cc

@@ -59,6 +59,11 @@ rlc_um::rlc_um() : tx_sdu_queue(16)
   pdu_lost = false;
 }
 
+rlc_um::~rlc_um()
+{
+  stop();
+}
+
 void rlc_um::init(srslte::log                 *log_,
                   uint32_t                     lcid_,
                   srsue::pdcp_interface_rlc   *pdcp_,
@@ -114,12 +119,13 @@ void rlc_um::empty_queue() {
 void rlc_um::stop()
 {
   reset();
-  mac_timers->timer_release_id(reordering_timer_id);
+  if (mac_timers) {
+    mac_timers->timer_release_id(reordering_timer_id);
+  }
 }
 
 void rlc_um::reset()
 {
-  
   // Empty tx_sdu_queue before locking the mutex 
   empty_queue();
 
@@ -129,12 +135,17 @@ void rlc_um::reset()
   vr_ux    = 0;
   vr_uh    = 0;
   pdu_lost = false;
-  if(rx_sdu)
+  if(rx_sdu) {
-    rx_sdu->reset();
+    pool->deallocate(rx_sdu);
-  if(tx_sdu)
+  }
-    tx_sdu->reset();
+
-  if(mac_timers)
+  if(tx_sdu) {
+    pool->deallocate(tx_sdu);
+  }
+
+  if(mac_timers) {
     reordering_timer->stop();
+  }
   
   // Drop all messages in RX window
   std::map<uint32_t, rlc_umd_pdu_t>::iterator it;
@@ -162,7 +173,7 @@ uint32_t rlc_um::get_bearer()
 void rlc_um::write_sdu(byte_buffer_t *sdu)
 {
   tx_sdu_queue.write(sdu);
-  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU, tx_sdu_len=%d", rrc->get_rb_name(lcid).c_str(), tx_sdu_queue.size());
+  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU (%d B ,tx_sdu_queue_len=%d)", rrc->get_rb_name(lcid).c_str(), sdu->N_bytes, tx_sdu_queue.size());
 }
 
 /****************************************************************************
@@ -437,8 +448,13 @@ void rlc_um::handle_data_pdu(uint8_t *payload, uint32_t nof_bytes)
 
 void rlc_um::reassemble_rx_sdus()
 {
-  if(!rx_sdu)
+  if(!rx_sdu) {
     rx_sdu = pool_allocate;
+    if (!rx_sdu) {
+      log->error("Fatal Error: Couldn't allocate buffer in rlc_um::reassemble_rx_sdus().\n");
+      return;
+    }
+  }
 
   // First catch up with lower edge of reordering window
   while(!inside_reordering_window(vr_ur))
@@ -463,6 +479,10 @@ void rlc_um::reassemble_rx_sdus()
           rx_sdu->set_timestamp();
           pdcp->write_pdu(lcid, rx_sdu);
           rx_sdu = pool_allocate;
+          if (!rx_sdu) {
+            log->error("Fatal Error: Couldn't allocate buffer in rlc_um::reassemble_rx_sdus().\n");
+            return;
+          }
         }
         pdu_lost = false;
       }
@@ -483,6 +503,10 @@ void rlc_um::reassemble_rx_sdus()
           rx_sdu->set_timestamp();
           pdcp->write_pdu(lcid, rx_sdu);
           rx_sdu = pool_allocate;
+          if (!rx_sdu) {
+            log->error("Fatal Error: Couldn't allocate buffer in rlc_um::reassemble_rx_sdus().\n");
+            return;
+          }
         }
         pdu_lost = false;
       }
@@ -517,6 +541,10 @@ void rlc_um::reassemble_rx_sdus()
         rx_sdu->set_timestamp();
         pdcp->write_pdu(lcid, rx_sdu);
         rx_sdu = pool_allocate;
+        if (!rx_sdu) {
+          log->error("Fatal Error: Couldn't allocate buffer in rlc_um::reassemble_rx_sdus().\n");
+          return;
+        }
       }
       pdu_lost = false;
     }
@@ -546,6 +574,10 @@ void rlc_um::reassemble_rx_sdus()
         rx_sdu->set_timestamp();
         pdcp->write_pdu(lcid, rx_sdu);
         rx_sdu = pool_allocate;
+        if (!rx_sdu) {
+          log->error("Fatal Error: Couldn't allocate buffer in rlc_um::reassemble_rx_sdus().\n");
+          return;
+        }
       }
       pdu_lost = false;
     }

lib/test/common/timeout_test.cc

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

lib/test/upper/CMakeLists.txt

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

lib/test/upper/rlc_am_stress_test.cc

@@ -31,23 +31,77 @@
 #include "srslte/common/logger_stdout.h"
 #include "srslte/common/threads.h"
 #include "srslte/upper/rlc.h"
+#include <boost/program_options.hpp>
+#include <boost/program_options/parsers.hpp>
 #include <assert.h>
-#define NBUFS 5
 
+using namespace std;
 using namespace srsue;
 using namespace srslte;
+namespace bpo = boost::program_options;
+
+typedef struct {
+  uint32_t test_duration_sec;
+  float    error_rate;
+  uint32_t sdu_gen_delay_usec;
+  uint32_t pdu_tx_delay_usec;
+  bool     reestablish;
+  uint32_t log_level;
+} stress_test_args_t;
+
+void parse_args(stress_test_args_t *args, int argc, char *argv[]) {
+
+  // Command line only options
+  bpo::options_description general("General options");
+
+  general.add_options()
+  ("help,h", "Produce help message")
+  ("version,v", "Print version information and exit");
+
+  // Command line or config file options
+  bpo::options_description common("Configuration options");
+  common.add_options()
+  ("duration",      bpo::value<uint32_t>(&args->test_duration_sec)->default_value(10), "Duration (sec)")
+  ("sdu_gen_delay", bpo::value<uint32_t>(&args->sdu_gen_delay_usec)->default_value(10), "SDU generation delay (usec)")
+  ("pdu_tx_delay",  bpo::value<uint32_t>(&args->pdu_tx_delay_usec)->default_value(10), "Delay in MAC for transfering PDU from tx'ing RLC to rx'ing RLC (usec)")
+  ("error_rate",    bpo::value<float>(&args->error_rate)->default_value(0.1), "Rate at which RLC PDUs are dropped")
+  ("reestablish",   bpo::value<bool>(&args->reestablish)->default_value(false), "Mimic RLC reestablish during execution")
+  ("loglevel",      bpo::value<uint32_t>(&args->log_level)->default_value(srslte::LOG_LEVEL_DEBUG), "Log level (1=Error,2=Warning,3=Info,4=Debug");
+
+  // these options are allowed on the command line
+  bpo::options_description cmdline_options;
+  cmdline_options.add(common).add(general);
+
+  // parse the command line and store result in vm
+  bpo::variables_map vm;
+  bpo::store(bpo::command_line_parser(argc, argv).options(cmdline_options).run(), vm);
+  bpo::notify(vm);
+
+  // help option was given - print usage and exit
+  if (vm.count("help")) {
+    cout << "Usage: " << argv[0] << " [OPTIONS] config_file" << endl << endl;
+    cout << common << endl << general << endl;
+    exit(0);
+  }
+
+  if (args->log_level > 4) {
+    args->log_level = 4;
+    printf("Set log level to %d (%s)\n", args->log_level, srslte::log_level_text[args->log_level]);
+  }
+}
 
 class mac_reader
     :public thread
 {
 public:
-  mac_reader(rlc_interface_mac *rlc1_, rlc_interface_mac *rlc2_, float fail_rate_)
+  mac_reader(rlc_interface_mac *rlc1_, rlc_interface_mac *rlc2_, float fail_rate_, uint32_t pdu_tx_delay_usec_)
   {
     rlc1 = rlc1_;
     rlc2 = rlc2_;
     fail_rate = fail_rate_;
     run_enable = true;
     running = false;
+    pdu_tx_delay_usec = pdu_tx_delay_usec_;
   }
 
   void stop()
@@ -82,14 +136,16 @@ private:
       if(((float)rand()/RAND_MAX > fail_rate) && read>0) {
         rlc2->write_pdu(1, pdu->msg, opp_size);
       }
-      usleep(1000);
+      usleep(pdu_tx_delay_usec);
     }
     running = false;
+    byte_buffer_pool::get_instance()->deallocate(pdu);
   }
 
   rlc_interface_mac *rlc1;
   rlc_interface_mac *rlc2;
   float fail_rate;
+  uint32_t pdu_tx_delay_usec;
 
   bool run_enable;
   bool running;
@@ -99,9 +155,9 @@ class mac_dummy
     :public srslte::mac_interface_timers
 {
 public:
-  mac_dummy(rlc_interface_mac *rlc1_, rlc_interface_mac *rlc2_, float fail_rate_)
+  mac_dummy(rlc_interface_mac *rlc1_, rlc_interface_mac *rlc2_, float fail_rate_, uint32_t pdu_tx_delay)
-    :r1(rlc1_, rlc2_, fail_rate_)
+    :r1(rlc1_, rlc2_, fail_rate_, pdu_tx_delay)
-    ,r2(rlc2_, rlc1_, fail_rate_)
+    ,r2(rlc2_, rlc1_, fail_rate_, pdu_tx_delay)
   {
   }
 
@@ -139,10 +195,13 @@ class rlc_am_tester
     ,public thread
 {
 public:
-  rlc_am_tester(rlc_interface_pdcp *rlc_){
+  rlc_am_tester(rlc_interface_pdcp *rlc_, std::string name_, uint32_t sdu_gen_delay_usec_){
     rlc = rlc_;
     run_enable = true;
     running = false;
+    rx_pdus = 0;
+    name = name_;
+    sdu_gen_delay_usec = sdu_gen_delay_usec_;
   }
 
   void stop()
@@ -164,6 +223,7 @@ public:
   {
     assert(lcid == 1);
     byte_buffer_pool::get_instance()->deallocate(sdu);
+    std::cout << "rlc_am_tester " << name << " received " << rx_pdus++ << " PDUs" << std::endl;
   }
   void write_pdu_bcch_bch(byte_buffer_t *sdu) {}
   void write_pdu_bcch_dlsch(byte_buffer_t *sdu) {}
@@ -187,38 +247,41 @@ private:
       pdu->N_bytes = 1500;
       pdu->msg[0]   = sn++;
       rlc->write_sdu(1, pdu);
-      usleep(1000);
+      usleep(sdu_gen_delay_usec);
     }
     running = false;
   }
 
   bool run_enable;
   bool running;
+  long rx_pdus;
+
+  std::string name;
+
+  uint32_t sdu_gen_delay_usec;
 
   rlc_interface_pdcp *rlc;
 };
 
-void stress_test()
+void stress_test(stress_test_args_t args)
 {
   srslte::log_filter log1("RLC_AM_1");
   srslte::log_filter log2("RLC_AM_2");
-  log1.set_level(srslte::LOG_LEVEL_DEBUG);
+  log1.set_level((LOG_LEVEL_ENUM)args.log_level);
-  log2.set_level(srslte::LOG_LEVEL_DEBUG);
+  log2.set_level((LOG_LEVEL_ENUM)args.log_level);
   log1.set_hex_limit(-1);
   log2.set_hex_limit(-1);
 
-  float fail_rate = 0.1;
-
   rlc rlc1;
   rlc rlc2;
 
-  rlc_am_tester tester1(&rlc1);
+  rlc_am_tester tester1(&rlc1, "tester1", args.sdu_gen_delay_usec);
-  rlc_am_tester tester2(&rlc2);
+  rlc_am_tester tester2(&rlc2, "tester2", args.sdu_gen_delay_usec);
-  mac_dummy     mac(&rlc1, &rlc2, fail_rate);
+  mac_dummy     mac(&rlc1, &rlc2, args.error_rate, args.pdu_tx_delay_usec);
   ue_interface  ue;
 
   rlc1.init(&tester1, &tester1, &ue, &log1, &mac, 0);
-  rlc2.init(&tester1, &tester1, &ue, &log2, &mac, 0);
+  rlc2.init(&tester2, &tester2, &ue, &log2, &mac, 0);
 
   LIBLTE_RRC_RLC_CONFIG_STRUCT cnfg;
   cnfg.rlc_mode = LIBLTE_RRC_RLC_MODE_AM;
@@ -235,10 +298,17 @@ void stress_test()
   rlc2.add_bearer(1, cnfg_);
 
   tester1.start(7);
-  //tester2.start(7);
+  tester2.start(7);
   mac.start();
 
-  usleep(100e6);
+  for (uint32_t i = 0; i < args.test_duration_sec; i++) {
+    // if enabled, mimic reestablishment every second
+    if (args.reestablish) {
+      rlc1.reestablish();
+      rlc2.reestablish();
+    }
+    usleep(1e6);
+  }
 
   tester1.stop();
   tester2.stop();
@@ -247,6 +317,9 @@ void stress_test()
 
 
 int main(int argc, char **argv) {
-  stress_test();
+  stress_test_args_t args;
+  parse_args(&args, argc, argv);
+
+  stress_test(args);
   byte_buffer_pool::get_instance()->cleanup();
 }

lib/test/upper/rlc_am_test.cc

@@ -59,6 +59,14 @@ public:
     n_sdus = 0;
   }
 
+  ~rlc_am_tester(){
+    for (uint32_t i = 0; i < 10; i++) {
+      if (sdus[i] != NULL) {
+        byte_buffer_pool::get_instance()->deallocate(sdus[i]);
+      }
+    }
+  }
+
   // PDCP interface
   void write_pdu(uint32_t lcid, byte_buffer_t *sdu)
   {
@@ -482,17 +490,17 @@ void resegment_test_1()
 
   // Read the retx PDU from RLC1 and force resegmentation
   byte_buffer_t retx1;
-  len = rlc1.read_pdu(retx1.msg, 11); // 4 byte header + 5 data
+  len = rlc1.read_pdu(retx1.msg, 9); // 4 byte header + 5 data
   retx1.N_bytes = len;
 
   // Write the retx PDU to RLC2
   rlc2.write_pdu(retx1.msg, retx1.N_bytes);
 
-  assert(9 == rlc1.get_buffer_state()); // 4 byte header + 5 data
+  assert(9 == rlc1.get_buffer_state());
 
   // Read the remaining segment
   byte_buffer_t retx2;
-  len = rlc1.read_pdu(retx2.msg, 11); // 4 byte header + 5 data
+  len = rlc1.read_pdu(retx2.msg, 9); // 4 byte header + 5 data
   retx2.N_bytes = len;
 
   // Write the retx PDU to RLC2
@@ -591,16 +599,16 @@ 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, 18); // 6 byte header + 10 data
+  retx1.N_bytes = rlc1.read_pdu(retx1.msg, 16); // 6 byte header + 10 data
 
   // Write the retx PDU to RLC2
   rlc2.write_pdu(retx1.msg, retx1.N_bytes);
 
-  assert(16 == rlc1.get_buffer_state()); // 6 byte header + 10 data
+  assert(16 == rlc1.get_buffer_state());
 
   // Read the remaining segment
   byte_buffer_t retx2;
-  retx2.N_bytes = rlc1.read_pdu(retx2.msg, 18); // 6 byte header + 10 data
+  retx2.N_bytes = rlc1.read_pdu(retx2.msg, 16); // 6 byte header + 10 data
 
   // Write the retx PDU to RLC2
   rlc2.write_pdu(retx2.msg, retx2.N_bytes);
@@ -696,14 +704,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, 16); // 4 byte header + 10 data
+  retx1.N_bytes = rlc1.read_pdu(retx1.msg, 14); // 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, 16); // 4 byte header + 10 data
+  retx2.N_bytes = rlc1.read_pdu(retx2.msg, 14); // 4 byte header + 10 data
 
   // Write the retx PDU to RLC2
   rlc2.write_pdu(retx2.msg, retx2.N_bytes);
@@ -799,14 +807,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, 23); // 6 byte header + 15 data
+  retx1.N_bytes = rlc1.read_pdu(retx1.msg, 21); // 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, 23); // 6 byte header + 15 data
+  retx2.N_bytes = rlc1.read_pdu(retx2.msg, 21); // 6 byte header + 15 data
 
   // Write the retx PDU to RLC2
   rlc2.write_pdu(retx2.msg, retx2.N_bytes);
@@ -902,14 +910,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, 29); // 7 byte header + 20 data
+  retx1.N_bytes = rlc1.read_pdu(retx1.msg, 27); // 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, 29); // 7 byte header + 20 data
+  retx2.N_bytes = rlc1.read_pdu(retx2.msg, 27); // 7 byte header + 20 data
 
   // Write the retx PDU to RLC2
   rlc2.write_pdu(retx2.msg, retx2.N_bytes);
@@ -1023,11 +1031,11 @@ void resegment_test_6()
   // Write the retx PDU to RLC2
   rlc2.write_pdu(retx1.msg, retx1.N_bytes);
 
-  assert(157 == rlc1.get_buffer_state());
+  assert(155 == rlc1.get_buffer_state());
 
   // Read the remaining segment
   byte_buffer_t retx2;
-  len = rlc1.read_pdu(retx2.msg, 159);
+  len = rlc1.read_pdu(retx2.msg, 157);
   retx2.N_bytes = len;
 
   // Write the retx PDU to RLC2
@@ -1048,24 +1056,91 @@ void resegment_test_6()
   }
 }
 
+void reset_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);
+  rlc_am_tester     tester;
+  mac_dummy_timers  timers;
+
+  rlc_am rlc1;
+  int len;
+
+  log1.set_level(srslte::LOG_LEVEL_DEBUG);
+
+  rlc1.init(&log1, 1, &tester, &tester, &timers);
+
+  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;
+
+  rlc1.configure(&cnfg);
+
+  // Push 1 SDU of size 10 into RLC1
+  byte_buffer_t sdu_buf;
+  *sdu_buf.msg    = 1; // Write the index into the buffer
+  sdu_buf.N_bytes = 100;
+  rlc1.write_sdu(&sdu_buf);
+
+  // read 1 PDU from RLC1 and force segmentation
+  byte_buffer_t pdu_bufs;
+  len = rlc1.read_pdu(pdu_bufs.msg, 4);
+  pdu_bufs.N_bytes = len;
+
+  // reset RLC1
+  rlc1.reset();
+
+  // read another PDU segment from RLC1
+  len = rlc1.read_pdu(pdu_bufs.msg, 4);
+  pdu_bufs.N_bytes = len;
+
+  // now empty RLC buffer
+  len = rlc1.read_pdu(pdu_bufs.msg, 100);
+  pdu_bufs.N_bytes = len;
+
+  assert(0 == rlc1.get_buffer_state());
+}
+
 int main(int argc, char **argv) {
   basic_test();
   byte_buffer_pool::get_instance()->cleanup();
+
   concat_test();
   byte_buffer_pool::get_instance()->cleanup();
+
   segment_test();
   byte_buffer_pool::get_instance()->cleanup();
+
   retx_test();
   byte_buffer_pool::get_instance()->cleanup();
+
   resegment_test_1();
   byte_buffer_pool::get_instance()->cleanup();
+
   resegment_test_2();
   byte_buffer_pool::get_instance()->cleanup();
+
   resegment_test_3();
   byte_buffer_pool::get_instance()->cleanup();
+
   resegment_test_4();
   byte_buffer_pool::get_instance()->cleanup();
+
   resegment_test_5();
   byte_buffer_pool::get_instance()->cleanup();
+
   resegment_test_6();
+  byte_buffer_pool::get_instance()->cleanup();
+
+  reset_test();
+  byte_buffer_pool::get_instance()->cleanup();
 }

lib/test/upper/rlc_um_test.cc

@@ -62,6 +62,14 @@ public:
     n_sdus = 0;
   }
 
+  ~rlc_um_tester(){
+    for (uint32_t i = 0; i < NBUFS; i++) {
+      if (sdus[i] != NULL) {
+        byte_buffer_pool::get_instance()->deallocate(sdus[i]);
+      }
+    }
+  }
+
   // PDCP interface
   void write_pdu(uint32_t lcid, byte_buffer_t *sdu)
   {

srsenb/enb.conf.example

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

srsenb/hdr/enb.h

@@ -188,7 +188,7 @@ private:
   srslte::logger        *logger;
 
   srslte::log_filter  rf_log;
-  std::vector<void*>  phy_log;
+  std::vector<srslte::log_filter*>  phy_log;
   srslte::log_filter  mac_log;
   srslte::log_filter  rlc_log;
   srslte::log_filter  pdcp_log;

srsenb/hdr/mac/scheduler.h

@@ -100,8 +100,8 @@ public:
   uint32_t get_ul_buffer(uint16_t rnti); 
   uint32_t get_dl_buffer(uint16_t rnti); 
 
-  int dl_rlc_buffer_state(uint16_t rnti, uint32_t lc_id, uint32_t tx_queue, uint32_t retx_queue); 
+  int dl_rlc_buffer_state(uint16_t rnti, uint32_t lc_id, uint32_t tx_queue, uint32_t retx_queue);
-  int dl_mac_buffer_state(uint16_t rnti, uint32_t ce_code); 
+  int dl_mac_buffer_state(uint16_t rnti, uint32_t ce_code);
     
   int dl_ant_info(uint16_t rnti, LIBLTE_RRC_ANTENNA_INFO_DEDICATED_STRUCT *dedicated);
   int dl_ack_info(uint32_t tti, uint16_t rnti, uint32_t tb_idx, bool ack);
@@ -217,7 +217,7 @@ private:
   
   bool configured;
   
-  pthread_mutex_t mutex; 
+  pthread_mutex_t mutex, mutex2;
   
   
 };

srsenb/hdr/phy/phy.h

@@ -28,6 +28,7 @@
 #define ENBPHY_H
 
 #include "srslte/common/log.h"
+#include "srslte/common/log_filter.h"
 #include "phy/txrx.h"
 #include "phy/phch_worker.h"
 #include "phy/phch_common.h"
@@ -54,8 +55,8 @@ class phy : public phy_interface_mac,
 public:
 
   phy();
-  bool init(phy_args_t *args, phy_cfg_t *common_cfg, srslte::radio *radio_handler, mac_interface_phy *mac, srslte::log* log_h);
+  bool init(phy_args_t *args, phy_cfg_t *common_cfg, srslte::radio *radio_handler, mac_interface_phy *mac, srslte::log_filter* log_h);
-  bool init(phy_args_t *args, phy_cfg_t *common_cfg, srslte::radio *radio_handler, mac_interface_phy *mac, std::vector<void*> log_vec);
+  bool init(phy_args_t *args, phy_cfg_t *common_cfg, srslte::radio *radio_handler, mac_interface_phy *mac, std::vector<srslte::log_filter *> log_vec);
   void stop();
   
   /* MAC->PHY interface */

srsenb/src/enb.cc

@@ -25,33 +25,33 @@
  */
 
 #include <boost/algorithm/string.hpp>
-#include <boost/thread/mutex.hpp>
 #include <enb.h>
 #include "enb.h"
 
 namespace srsenb {
 
 enb*          enb::instance = NULL;
-boost::mutex  enb_instance_mutex;
+pthread_mutex_t enb_instance_mutex = PTHREAD_MUTEX_INITIALIZER;
-
 
 enb* enb::get_instance(void)
 {
-  boost::mutex::scoped_lock lock(enb_instance_mutex);
+  pthread_mutex_lock(&enb_instance_mutex);
   if(NULL == instance) {
-      instance = new enb();
+    instance = new enb();
   }
+  pthread_mutex_unlock(&enb_instance_mutex);
   return(instance);
 }
 void enb::cleanup(void)
 {
   srslte_dft_exit();
   srslte::byte_buffer_pool::cleanup();
-  boost::mutex::scoped_lock lock(enb_instance_mutex);
+  pthread_mutex_lock(&enb_instance_mutex);
   if(NULL != instance) {
       delete instance;
       instance = NULL;
   }
+  pthread_mutex_unlock(&enb_instance_mutex);
 }
 
 enb::enb() : started(false) {
@@ -66,6 +66,9 @@ enb::enb() : started(false) {
 
 enb::~enb()
 {
+  for (uint32_t i = 0; i < phy_log.size(); i++) {
+    delete (phy_log[i]);
+  }
 }
 
 bool enb::init(all_args_t *args_)
@@ -88,7 +91,7 @@ bool enb::init(all_args_t *args_)
     char tmp[16];
     sprintf(tmp, "PHY%d",i);
     mylog->init(tmp, logger, true);
-    phy_log.push_back((void*) mylog); 
+    phy_log.push_back(mylog);
   }
   mac_log.init("MAC ", logger, true);
   rlc_log.init("RLC ", logger);
@@ -294,7 +297,7 @@ void enb::handle_rf_msg(srslte_rf_error_t error)
     str.erase(std::remove(str.begin(), str.end(), '\n'), str.end());
     str.erase(std::remove(str.begin(), str.end(), '\r'), str.end());
     str.push_back('\n');
-    rf_log.info(str);
+    rf_log.info(str.c_str());
   }
 }
 

srsenb/src/enb_cfg_parser.cc

@@ -1072,7 +1072,9 @@ int field_qci::parse(libconfig::Setting &root)
       parser::field_enum_num<LIBLTE_RRC_SN_FIELD_LENGTH_ENUM,uint8> sn_field_len
         ("sn_field_length", &rlc_cfg->sn_field_len, 
         liblte_rrc_sn_field_length_num, LIBLTE_RRC_SN_FIELD_LENGTH_N_ITEMS);      
-      sn_field_len.parse(q["rlc_config"]["ul_um"]);
+      if (sn_field_len.parse(q["rlc_config"]["ul_um"])) {
+        fprintf(stderr, "Error can't find sn_field_length in section ul_um\n");
+      }
     }
     
     if (q["rlc_config"].exists("dl_um")) {
@@ -1085,12 +1087,16 @@ int field_qci::parse(libconfig::Setting &root)
       parser::field_enum_num<LIBLTE_RRC_SN_FIELD_LENGTH_ENUM,uint8> sn_field_len
         ("sn_field_length", &rlc_cfg->sn_field_len, 
         liblte_rrc_sn_field_length_num, LIBLTE_RRC_SN_FIELD_LENGTH_N_ITEMS);      
-      sn_field_len.parse(q["rlc_config"]["dl_um"]);
+      if (sn_field_len.parse(q["rlc_config"]["dl_um"])) {
+        fprintf(stderr, "Error can't find sn_field_length in section dl_um\n");
+      }
       
       parser::field_enum_num<LIBLTE_RRC_T_REORDERING_ENUM,int32> t_reordering
         ("t_reordering", &rlc_cfg->t_reordering, 
         liblte_rrc_t_reordering_num, LIBLTE_RRC_T_REORDERING_N_ITEMS);      
-      t_reordering.parse(q["rlc_config"]["dl_um"]);
+      if (t_reordering.parse(q["rlc_config"]["dl_um"])) {
+        fprintf(stderr, "Error can't find t_reordering in section dl_um\n");
+      }
     }    
       
     // Parse RLC-AM section 
@@ -1100,22 +1106,30 @@ int field_qci::parse(libconfig::Setting &root)
       parser::field_enum_num<LIBLTE_RRC_T_POLL_RETRANSMIT_ENUM,int32> t_poll_retx
         ("t_poll_retx", &rlc_cfg->t_poll_retx, 
         liblte_rrc_t_poll_retransmit_num, LIBLTE_RRC_T_POLL_RETRANSMIT_N_ITEMS);      
-      t_poll_retx.parse(q["rlc_config"]["ul_am"]);
+      if (t_poll_retx.parse(q["rlc_config"]["ul_am"])) {
+        fprintf(stderr, "Error can't find t_poll_retx in section ul_am\n");
+      }
       
       parser::field_enum_num<LIBLTE_RRC_POLL_PDU_ENUM,int32> poll_pdu
         ("poll_pdu", &rlc_cfg->poll_pdu, 
         liblte_rrc_poll_pdu_num, LIBLTE_RRC_POLL_PDU_N_ITEMS);      
-      poll_pdu.parse(q["rlc_config"]["ul_am"]);
+      if (poll_pdu.parse(q["rlc_config"]["ul_am"])) {
+        fprintf(stderr, "Error can't find poll_pdu in section ul_am\n");
+      }
 
       parser::field_enum_num<LIBLTE_RRC_POLL_BYTE_ENUM,int32> poll_byte
         ("poll_byte", &rlc_cfg->poll_byte, 
         liblte_rrc_poll_byte_num, LIBLTE_RRC_POLL_BYTE_N_ITEMS);      
-      poll_byte.parse(q["rlc_config"]["ul_am"]);
+      if (poll_byte.parse(q["rlc_config"]["ul_am"])) {
+        fprintf(stderr, "Error can't find poll_byte in section ul_am\n");
+      }
 
       parser::field_enum_num<LIBLTE_RRC_MAX_RETX_THRESHOLD_ENUM,uint32_t> max_retx_thresh
         ("max_retx_thresh", &rlc_cfg->max_retx_thresh, 
         liblte_rrc_max_retx_threshold_num, LIBLTE_RRC_MAX_RETX_THRESHOLD_N_ITEMS);      
-      max_retx_thresh.parse(q["rlc_config"]["ul_am"]);      
+      if (max_retx_thresh.parse(q["rlc_config"]["ul_am"])) {
+        fprintf(stderr, "Error can't find max_retx_thresh in section ul_am\n");
+      }
     }
     
     if (q["rlc_config"].exists("dl_am")) {      
@@ -1124,12 +1138,16 @@ int field_qci::parse(libconfig::Setting &root)
       parser::field_enum_num<LIBLTE_RRC_T_REORDERING_ENUM,int32> t_reordering
         ("t_reordering", &rlc_cfg->t_reordering, 
         liblte_rrc_t_reordering_num, LIBLTE_RRC_T_REORDERING_N_ITEMS);      
-      t_reordering.parse(q["rlc_config"]["dl_am"]);
+      if (t_reordering.parse(q["rlc_config"]["dl_am"])) {
+        fprintf(stderr, "Error can't find t_reordering in section dl_am\n");
+      }
       
       parser::field_enum_num<LIBLTE_RRC_T_STATUS_PROHIBIT_ENUM,int32> t_status_prohibit
         ("t_status_prohibit", &rlc_cfg->t_status_prohibit, 
         liblte_rrc_t_status_prohibit_num, LIBLTE_RRC_T_STATUS_PROHIBIT_N_ITEMS);      
-      t_status_prohibit.parse(q["rlc_config"]["dl_am"]);
+      if (t_status_prohibit.parse(q["rlc_config"]["dl_am"])) {
+        fprintf(stderr, "Error can't find t_status_prohibit in section dl_am\n");
+      }
     }    
     
 
@@ -1141,17 +1159,23 @@ int field_qci::parse(libconfig::Setting &root)
     LIBLTE_RRC_UL_SPECIFIC_PARAMETERS_STRUCT *lc_cfg = &cfg[qci].lc_cfg; 
 
     parser::field<uint8> priority ("priority", &lc_cfg->priority);      
-    priority.parse(q["logical_channel_config"]);
+    if (priority.parse(q["logical_channel_config"])) {
+      fprintf(stderr, "Error can't find logical_channel_config in section priority\n");
+    }
     
     parser::field_enum_num<LIBLTE_RRC_PRIORITIZED_BIT_RATE_ENUM,int32> prioritized_bit_rate
         ("prioritized_bit_rate", &lc_cfg->prioritized_bit_rate, 
         liblte_rrc_prioritized_bit_rate_num, LIBLTE_RRC_PRIORITIZED_BIT_RATE_N_ITEMS);      
-    prioritized_bit_rate.parse(q["logical_channel_config"]);
+    if (prioritized_bit_rate.parse(q["logical_channel_config"])) {
+      fprintf(stderr, "Error can't find prioritized_bit_rate in section logical_channel_config\n");
+    }
 
     parser::field_enum_num<LIBLTE_RRC_BUCKET_SIZE_DURATION_ENUM,int16> bucket_size_duration
         ("bucket_size_duration", &lc_cfg->bucket_size_duration, 
         liblte_rrc_bucket_size_duration_num, LIBLTE_RRC_BUCKET_SIZE_DURATION_N_ITEMS);      
-    bucket_size_duration.parse(q["logical_channel_config"]);
+    if (bucket_size_duration.parse(q["logical_channel_config"])) {
+      fprintf(stderr, "Error can't find bucket_size_duration in section logical_channel_config\n");
+    }
 
     parser::field<uint8> log_chan_group ("log_chan_group", &lc_cfg->log_chan_group);      
     if (log_chan_group.parse(q["logical_channel_config"])) {

srsenb/src/mac/scheduler.cc

@@ -1,3 +1,28 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2017 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of srsLTE.
+ *
+ * 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 <string.h>
 
@@ -39,6 +64,7 @@ sched::sched() : bc_aggr_level(0), rar_aggr_level(0), avail_rbg(0), P(0), start_
   }
 
   pthread_mutex_init(&mutex, NULL);
+  pthread_mutex_init(&mutex2, NULL);
   reset();
 }
 
@@ -46,6 +72,7 @@ sched::~sched()
 {
   srslte_regs_free(&regs);
   pthread_mutex_destroy(&mutex);
+  pthread_mutex_destroy(&mutex2);
 }
 
 void sched::init(rrc_interface_mac *rrc_, srslte::log* log)
@@ -148,13 +175,15 @@ int sched::ue_cfg(uint16_t rnti, sched_interface::ue_cfg_t *ue_cfg)
 int sched::ue_rem(uint16_t rnti)
 {
   pthread_mutex_lock(&mutex);
-  int ret = 0; 
+  pthread_mutex_lock(&mutex2);
+  int ret = 0;
   if (ue_db.count(rnti)) {         
     ue_db.erase(rnti);
   } else {
     Error("User rnti=0x%x not found\n", rnti);
     ret = -1;
   }
+  pthread_mutex_unlock(&mutex2);
   pthread_mutex_unlock(&mutex);
   return ret; 
 }
@@ -192,13 +221,15 @@ int sched::bearer_ue_cfg(uint16_t rnti, uint32_t lc_id, sched_interface::ue_bear
 int sched::bearer_ue_rem(uint16_t rnti, uint32_t lc_id)
 {
   pthread_mutex_lock(&mutex);
-  int ret = 0; 
+  pthread_mutex_lock(&mutex2);
+  int ret = 0;
   if (ue_db.count(rnti)) {         
     ue_db[rnti].rem_bearer(lc_id);
   } else {
     Error("User rnti=0x%x not found\n", rnti);
     ret = -1;
   }
+  pthread_mutex_unlock(&mutex2);
   pthread_mutex_unlock(&mutex);
   return ret; 
 }
@@ -229,32 +260,42 @@ uint32_t sched::get_ul_buffer(uint16_t rnti)
   return ret; 
 }
 
+/* \Warning: This function is not mutexed because it can produce late changes on the buffer state while
+ * the scheduler is already allocating data, resulting in empty grants.
+ * Ideally we would like the scheduler to query the RLC for buffer states in order to get the most updated
+ * buffer state with the minimum overhead. However, the current architecture is designed to be compliant
+ * with the FAPI interface
+ *
+ * We add a new mutex used only in ue_rem to avoid the UE being removed in between the access to
+ * ue_db.count() and the access to the std::map.
+ */
 int sched::dl_rlc_buffer_state(uint16_t rnti, uint32_t lc_id, uint32_t tx_queue, uint32_t retx_queue)
 {
-  pthread_mutex_lock(&mutex);
+  pthread_mutex_lock(&mutex2);
-  int ret = 0; 
+  int ret = 0;
-  if (ue_db.count(rnti)) {         
+  if (ue_db.count(rnti)) {
     ue_db[rnti].dl_buffer_state(lc_id, tx_queue, retx_queue);
   } else {
     Error("User rnti=0x%x not found\n", rnti);
     ret = -1;
   }
-  pthread_mutex_unlock(&mutex);
+  pthread_mutex_unlock(&mutex2);
-  return ret; 
+  return ret;
 }
 
+/* \Warning Read comment in dl_rlc_buffer_state() */
 int sched::dl_mac_buffer_state(uint16_t rnti, uint32_t ce_code)
 {
-  pthread_mutex_lock(&mutex);
+  pthread_mutex_lock(&mutex2);
-  int ret = 0; 
+  int ret = 0;
   if (ue_db.count(rnti)) {         
     ue_db[rnti].mac_buffer_state(ce_code);
   } else {
     Error("User rnti=0x%x not found\n", rnti);
     ret = -1;
   }
-  pthread_mutex_unlock(&mutex);
+  pthread_mutex_unlock(&mutex2);
-  return ret; 
+  return ret;
 }
 
 int sched::dl_ant_info(uint16_t rnti, LIBLTE_RRC_ANTENNA_INFO_DEDICATED_STRUCT *dl_ant_info) {
@@ -651,10 +692,11 @@ int sched::dl_sched_data(dl_sched_data_t data[MAX_DATA_LIST])
   
   int nof_data_elems = 0; 
   for(std::map<uint16_t, sched_ue>::iterator iter=ue_db.begin(); iter!=ue_db.end(); ++iter) {
-    sched_ue *user      = (sched_ue*) &iter->second;
+    sched_ue *user = (sched_ue*) &iter->second;
-    uint16_t rnti = (uint16_t) iter->first; 
+    uint16_t rnti  = (uint16_t) iter->first;
 
-    dl_harq_proc *h = dl_metric->get_user_allocation(user); 
+    uint32_t data_before = user->get_pending_dl_new_data(current_tti);
+    dl_harq_proc *h = dl_metric->get_user_allocation(user);
     srslte_dci_format_t dci_format = user->get_dci_format();
     data[nof_data_elems].dci_format = dci_format;
 
@@ -681,10 +723,12 @@ int sched::dl_sched_data(dl_sched_data_t data[MAX_DATA_LIST])
             Error("DCI format (%d) not implemented\n", dci_format);
         }
         if (tbs > 0) {
-          log_h->info("SCHED: DL %s rnti=0x%x, pid=%d, mask=0x%x, dci=%d,%d, n_rtx=%d, tbs=%d, buffer=%d, tb_en={%s,%s}\n",
+          log_h->info("SCHED: DL %s rnti=0x%x, pid=%d, mask=0x%x, dci=%d,%d, n_rtx=%d, tbs=%d, buffer=%d/%d, tb_en={%s,%s}\n",
                       !is_newtx?"retx":"tx", rnti, h->get_id(), h->get_rbgmask(), 
                       data[nof_data_elems].dci_location.L, data[nof_data_elems].dci_location.ncce, h->nof_retx(0) + h->nof_retx(1),
-                      tbs, user->get_pending_dl_new_data(current_tti), data[nof_data_elems].dci.tb_en[0]?"y":"n",
+                      tbs,
+                      data_before, user->get_pending_dl_new_data(current_tti),
+                      data[nof_data_elems].dci.tb_en[0]?"y":"n",
                       data[nof_data_elems].dci.tb_en[1]?"y":"n");
           nof_data_elems++;
         } else {