diff --git a/CMakeLists.txt b/CMakeLists.txt
index f46486c47..c12fc5a66 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -130,6 +130,12 @@ if (STOP_ON_WARNING)
   add_definitions(-DSTOP_ON_WARNING)
 endif()
 
+# Test for Atomics
+include(CheckAtomic)
+if(NOT HAVE_CXX_ATOMICS_WITHOUT_LIB OR NOT HAVE_CXX_ATOMICS64_WITHOUT_LIB)
+    set(ATOMIC_LIBS "atomic")
+endif()
+
 ########################################################################
 # Find dependencies
 ########################################################################
diff --git a/COPYRIGHT b/COPYRIGHT
index c1643382d..7b6dc37e7 100644
--- a/COPYRIGHT
+++ b/COPYRIGHT
@@ -51,6 +51,10 @@ Files:  lib/include/srsran/common/backward.hpp
 Copyright: 2013, Google Inc.
 License: MIT
 
+Files:  cmake/modules/CheckAtomic.cmake
+Copyright: 2015, Charles J. Cliffe
+License: MIT
+
 License: AGPL-3+
   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU Affero General Public License as
diff --git a/cmake/modules/CheckAtomic.cmake b/cmake/modules/CheckAtomic.cmake
new file mode 100644
index 000000000..c8168d60d
--- /dev/null
+++ b/cmake/modules/CheckAtomic.cmake
@@ -0,0 +1,92 @@
+#
+# Copyright 2013-2021 Software Radio Systems Limited
+#
+# By using this file, you agree to the terms and conditions set
+# forth in the LICENSE file which can be found at the top level of
+# the distribution.
+#
+
+# Adopted from https://github.com/pothosware/SoapyRTLSDR
+# Copyright: 2015, Charles J. Cliffe
+# License: MIT
+
+# - Try to find if atomics need -latomic linking
+# Once done this will define
+#  HAVE_CXX_ATOMICS_WITHOUT_LIB - Wether atomic types work without -latomic
+#  HAVE_CXX_ATOMICS64_WITHOUT_LIB - Wether 64 bit atomic types work without -latomic
+
+INCLUDE(CheckCXXSourceCompiles)
+INCLUDE(CheckLibraryExists)
+
+# Sometimes linking against libatomic is required for atomic ops, if
+# the platform doesn't support lock-free atomics.
+
+function(check_working_cxx_atomics varname)
+set(OLD_CMAKE_REQUIRED_FLAGS ${CMAKE_REQUIRED_FLAGS})
+set(CMAKE_REQUIRED_FLAGS "${CMAKE_REQUIRED_FLAGS} -std=c++11")
+CHECK_CXX_SOURCE_COMPILES("
+#include <atomic>
+std::atomic<int> x;
+int main() {
+return std::atomic_is_lock_free(&x);
+}
+" ${varname})
+set(CMAKE_REQUIRED_FLAGS ${OLD_CMAKE_REQUIRED_FLAGS})
+endfunction(check_working_cxx_atomics)
+
+function(check_working_cxx_atomics64 varname)
+set(OLD_CMAKE_REQUIRED_FLAGS ${CMAKE_REQUIRED_FLAGS})
+set(CMAKE_REQUIRED_FLAGS "-std=c++11 ${CMAKE_REQUIRED_FLAGS}")
+CHECK_CXX_SOURCE_COMPILES("
+#include <atomic>
+#include <cstdint>
+std::atomic<uint64_t> x (0);
+int main() {
+uint64_t i = x.load(std::memory_order_relaxed);
+return std::atomic_is_lock_free(&x);
+}
+" ${varname})
+set(CMAKE_REQUIRED_FLAGS ${OLD_CMAKE_REQUIRED_FLAGS})
+endfunction(check_working_cxx_atomics64)
+
+# Check for atomic operations.
+if(MSVC)
+  # This isn't necessary on MSVC.
+  set(HAVE_CXX_ATOMICS_WITHOUT_LIB True)
+else()
+  # First check if atomics work without the library.
+  check_working_cxx_atomics(HAVE_CXX_ATOMICS_WITHOUT_LIB)
+endif()
+
+# If not, check if the library exists, and atomics work with it.
+if(NOT HAVE_CXX_ATOMICS_WITHOUT_LIB)
+  check_library_exists(atomic __atomic_fetch_add_4 "" HAVE_LIBATOMIC)
+  if(NOT HAVE_LIBATOMIC)
+    message(STATUS "Host compiler appears to require libatomic, but cannot locate it.")
+  endif()
+  list(APPEND CMAKE_REQUIRED_LIBRARIES "atomic")
+  check_working_cxx_atomics(HAVE_CXX_ATOMICS_WITH_LIB)
+  if (NOT HAVE_CXX_ATOMICS_WITH_LIB)
+    message(FATAL_ERROR "Host compiler must support std::atomic!")
+  endif()
+endif()
+
+# Check for 64 bit atomic operations.
+if(MSVC)
+  set(HAVE_CXX_ATOMICS64_WITHOUT_LIB True)
+else()
+  check_working_cxx_atomics64(HAVE_CXX_ATOMICS64_WITHOUT_LIB)
+endif()
+
+# If not, check if the library exists, and atomics work with it.
+if(NOT HAVE_CXX_ATOMICS64_WITHOUT_LIB)
+  check_library_exists(atomic __atomic_load_8 "" HAVE_LIBATOMIC64)
+  if(NOT HAVE_LIBATOMIC64)
+    message(STATUS "Host compiler appears to require libatomic, but cannot locate it.")
+  endif()
+  list(APPEND CMAKE_REQUIRED_LIBRARIES "atomic")
+  check_working_cxx_atomics64(HAVE_CXX_ATOMICS64_WITH_LIB)
+  if (NOT HAVE_CXX_ATOMICS64_WITH_LIB)
+    message(FATAL_ERROR "Host compiler must support std::atomic!")
+  endif()
+endif()
\ No newline at end of file
diff --git a/debian/copyright b/debian/copyright
index ad1510717..729597848 100644
--- a/debian/copyright
+++ b/debian/copyright
@@ -58,6 +58,9 @@ Files:  lib/include/srsran/common/backward.hpp
 Copyright: 2013, Google Inc.
 License: MIT
 
+Files:  cmake/modules/CheckAtomic.cmake
+Copyright: 2015, Charles J. Cliffe
+License: MIT
 
 License: AGPL-3+
   This program is free software: you can redistribute it and/or modify
diff --git a/lib/include/srsran/common/multiqueue.h b/lib/include/srsran/common/multiqueue.h
index 8fc87a351..f8a3e5a50 100644
--- a/lib/include/srsran/common/multiqueue.h
+++ b/lib/include/srsran/common/multiqueue.h
@@ -28,6 +28,7 @@
 #ifndef SRSRAN_MULTIQUEUE_H
 #define SRSRAN_MULTIQUEUE_H
 
+#include "srsran/adt/circular_buffer.h"
 #include "srsran/adt/move_callback.h"
 #include <algorithm>
 #include <condition_variable>
@@ -40,92 +41,210 @@ namespace srsran {
 
 #define MULTIQUEUE_DEFAULT_CAPACITY (8192) // Default per-queue capacity
 
+/**
+ * N-to-1 Message-Passing Broker that manages the creation, destruction of input ports, and popping of messages that
+ * are pushed to these ports.
+ * Each port provides a thread-safe push(...) / try_push(...) interface to enqueue messages
+ * The class will pop from the several created ports in a round-robin fashion.
+ * The popping() interface is not safe-thread. That means, that it is expected that only one thread will
+ * be popping tasks.
+ * @tparam myobj message type
+ */
 template <typename myobj>
 class multiqueue_handler
 {
-  class circular_buffer
+  class input_port_impl
   {
   public:
-    circular_buffer(uint32_t cap) : buffer(cap + 1) {}
-    circular_buffer(circular_buffer&& other) noexcept
+    input_port_impl(uint32_t cap, multiqueue_handler<myobj>* parent_) : buffer(cap), parent(parent_) {}
+    input_port_impl(const input_port_impl&) = delete;
+    input_port_impl(input_port_impl&&)      = delete;
+    input_port_impl& operator=(const input_port_impl&) = delete;
+    input_port_impl& operator=(input_port_impl&&) = delete;
+    ~input_port_impl() { deactivate_blocking(); }
+
+    size_t capacity() const { return buffer.max_size(); }
+    size_t size() const
     {
-      active       = other.active;
-      other.active = false;
-      widx         = other.widx;
-      ridx         = other.ridx;
-      buffer       = std::move(other.buffer);
+      std::lock_guard<std::mutex> lock(q_mutex);
+      return buffer.size();
+    }
+    bool active() const
+    {
+      std::lock_guard<std::mutex> lock(q_mutex);
+      return active_;
     }
 
-    std::condition_variable cv_full;
-    bool                    active = true;
+    void set_active(bool val)
+    {
+      std::unique_lock<std::mutex> lock(q_mutex);
+      if (val == active_) {
+        // no-op
+        return;
+      }
+      active_                = val;
+      consumer_notify_needed = true;
 
-    bool   empty() const { return widx == ridx; }
-    size_t size() const { return widx >= ridx ? widx - ridx : widx + (buffer.size() - ridx); }
-    bool   full() const { return (ridx > 0) ? widx == ridx - 1 : widx == buffer.size() - 1; }
-    size_t capacity() const { return buffer.size() - 1; }
+      if (not active_) {
+        buffer.clear();
+        lock.unlock();
+        // unlock blocked pushing threads
+        cv_full.notify_all();
+      }
+    }
+
+    void deactivate_blocking()
+    {
+      set_active(false);
+
+      // wait for all the pushers to unlock
+      std::unique_lock<std::mutex> lock(q_mutex);
+      while (nof_waiting > 0) {
+        cv_exit.wait(lock);
+      }
+    }
 
     template <typename T>
     void push(T&& o) noexcept
     {
-      buffer[widx++] = std::forward<T>(o);
-      if (widx >= buffer.size()) {
-        widx = 0;
-      }
+      push_(&o, true);
     }
 
-    void pop() noexcept
+    bool try_push(const myobj& o) { return push_(&o, false); }
+
+    srsran::error_type<myobj> try_push(myobj&& o)
     {
-      ridx++;
-      if (ridx >= buffer.size()) {
-        ridx = 0;
+      if (push_(&o, false)) {
+        return {};
       }
+      return {std::move(o)};
     }
 
-    myobj&       front() noexcept { return buffer[ridx]; }
-    const myobj& front() const noexcept { return buffer[ridx]; }
+    bool try_pop(myobj& obj)
+    {
+      std::unique_lock<std::mutex> lock(q_mutex);
+      if (buffer.empty()) {
+        consumer_notify_needed = true;
+        return false;
+      }
+      obj = std::move(buffer.top());
+      buffer.pop();
+      consumer_notify_needed = false;
+      if (nof_waiting > 0) {
+        lock.unlock();
+        cv_full.notify_one();
+      }
+      return true;
+    }
 
   private:
-    std::vector<myobj> buffer;
-    size_t             widx = 0, ridx = 0;
+    template <typename T>
+    bool push_(T* o, bool blocking) noexcept
+    {
+      std::unique_lock<std::mutex> lock(q_mutex);
+      if (not blocking) {
+        // non-blocking case
+        if (not active_ or buffer.full()) {
+          return false;
+        }
+      } else {
+        // blocking case
+        while (active_ and buffer.full()) {
+          nof_waiting++;
+          cv_full.wait(lock);
+          nof_waiting--;
+        }
+        if (not active_) {
+          lock.unlock();
+          cv_exit.notify_one();
+          return false;
+        }
+      }
+      buffer.push(std::forward<T>(*o));
+      if (consumer_notify_needed) {
+        // Note: The consumer thread only needs to be notified and awaken when queues transition from empty to non-empty
+        //       To ensure that the consumer noticed that the queue was empty before a push, we store the last
+        //       try_pop() return in a member variable.
+        //       Doing this reduces the contention of multiple producers for the same condition variable
+        consumer_notify_needed = false;
+        lock.unlock();
+        parent->signal_pushed_data();
+      }
+      return true;
+    }
+
+    multiqueue_handler<myobj>* parent = nullptr;
+
+    mutable std::mutex                 q_mutex;
+    srsran::dyn_circular_buffer<myobj> buffer;
+    std::condition_variable            cv_full, cv_exit;
+    bool                               active_                = true;
+    bool                               consumer_notify_needed = true;
+    int                                nof_waiting            = 0;
   };
 
 public:
   class queue_handle
   {
   public:
-    queue_handle() = default;
-    queue_handle(multiqueue_handler<myobj>* parent_, int id) : parent(parent_), queue_id(id) {}
+    explicit queue_handle(input_port_impl* impl_ = nullptr) : impl(impl_) {}
     template <typename FwdRef>
     void push(FwdRef&& value)
     {
-      parent->push(queue_id, std::forward<FwdRef>(value));
+      impl->push(std::forward<FwdRef>(value));
     }
-    bool                   try_push(const myobj& value) { return parent->try_push(queue_id, value); }
-    std::pair<bool, myobj> try_push(myobj&& value) { return parent->try_push(queue_id, std::move(value)); }
-    size_t                 size() { return parent->size(queue_id); }
+    bool                      try_push(const myobj& value) { return impl->try_push(value); }
+    srsran::error_type<myobj> try_push(myobj&& value) { return impl->try_push(std::move(value)); }
+    void                      reset()
+    {
+      if (impl != nullptr) {
+        impl->deactivate_blocking();
+        impl = nullptr;
+      }
+    }
+
+    size_t size() { return impl->size(); }
+    size_t capacity() { return impl->capacity(); }
+    bool   active() const { return impl != nullptr and impl->active(); }
+    bool   empty() const { return impl->size() == 0; }
+
+    bool operator==(const queue_handle& other) const { return impl == other.impl; }
+    bool operator!=(const queue_handle& other) const { return impl != other.impl; }
 
   private:
-    multiqueue_handler<myobj>* parent   = nullptr;
-    int                        queue_id = -1;
+    struct recycle_op {
+      void operator()(input_port_impl* p)
+      {
+        if (p != nullptr) {
+          p->deactivate_blocking();
+        }
+      }
+    };
+    std::unique_ptr<input_port_impl, recycle_op> impl;
   };
 
-  explicit multiqueue_handler(uint32_t capacity_ = MULTIQUEUE_DEFAULT_CAPACITY) : capacity(capacity_) {}
-  ~multiqueue_handler() { reset(); }
+  explicit multiqueue_handler(uint32_t default_capacity_ = MULTIQUEUE_DEFAULT_CAPACITY) :
+    default_capacity(default_capacity_)
+  {}
+  ~multiqueue_handler() { stop(); }
 
-  void reset()
+  void stop()
   {
     std::unique_lock<std::mutex> lock(mutex);
     running = false;
-    while (nof_threads_waiting > 0) {
-      uint32_t size = queues.size();
+    for (auto& q : queues) {
+      // signal deactivation to pushing threads in a non-blocking way
+      q.set_active(false);
+    }
+    while (wait_state) {
+      pushed_data = true;
       cv_empty.notify_one();
-      for (uint32_t i = 0; i < size; ++i) {
-        queues[i].cv_full.notify_all();
-      }
-      // wait for all threads to unblock
       cv_exit.wait(lock);
     }
-    queues.clear();
+    for (auto& q : queues) {
+      // ensure the queues are finished being deactivated
+      q.deactivate_blocking();
+    }
   }
 
   /**
@@ -133,197 +252,108 @@ public:
    * @param capacity_ The capacity of the queue.
    * @return The index of the newly created (or reused) queue within the vector of queues.
    */
-  int add_queue(uint32_t capacity_)
+  queue_handle add_queue(uint32_t capacity_)
   {
     uint32_t                    qidx = 0;
     std::lock_guard<std::mutex> lock(mutex);
     if (not running) {
-      return -1;
+      return queue_handle();
     }
-    for (; qidx < queues.size() and queues[qidx].active; ++qidx)
+    for (; qidx < queues.size() and (queues[qidx].active() or (queues[qidx].capacity() != capacity_)); ++qidx)
       ;
 
     // check if there is a free queue of the required size
-    if (qidx == queues.size() || queues[qidx].capacity() != capacity_) {
+    if (qidx == queues.size()) {
       // create new queue
-      queues.emplace_back(capacity_);
+      queues.emplace_back(capacity_, this);
       qidx = queues.size() - 1; // update qidx to the last element
     } else {
-      queues[qidx].active = true;
+      queues[qidx].set_active(true);
     }
-    return (int)qidx;
+    return queue_handle(&queues[qidx]);
   }
 
   /**
    * Add queue using the default capacity of the underlying multiqueue
    * @return The queue index
    */
-  int add_queue() { return add_queue(capacity); }
+  queue_handle add_queue() { return add_queue(default_capacity); }
 
-  int nof_queues()
+  uint32_t nof_queues() const
   {
     std::lock_guard<std::mutex> lock(mutex);
     uint32_t                    count = 0;
     for (uint32_t i = 0; i < queues.size(); ++i) {
-      count += queues[i].active ? 1 : 0;
+      count += queues[i].active() ? 1 : 0;
     }
     return count;
   }
 
-  template <typename FwdRef>
-  void push(int q_idx, FwdRef&& value)
-  {
-    {
-      std::unique_lock<std::mutex> lock(mutex);
-      while (is_queue_active_(q_idx) and queues[q_idx].full()) {
-        nof_threads_waiting++;
-        queues[q_idx].cv_full.wait(lock);
-        nof_threads_waiting--;
-      }
-      if (not is_queue_active_(q_idx)) {
-        cv_exit.notify_one();
-        return;
-      }
-      queues[q_idx].push(std::forward<FwdRef>(value));
-    }
-    cv_empty.notify_one();
-  }
-
-  bool try_push(int q_idx, const myobj& value)
-  {
-    {
-      std::lock_guard<std::mutex> lock(mutex);
-      if (not is_queue_active_(q_idx) or queues[q_idx].full()) {
-        return false;
-      }
-      queues[q_idx].push(value);
-    }
-    cv_empty.notify_one();
-    return true;
-  }
-
-  std::pair<bool, myobj> try_push(int q_idx, myobj&& value)
-  {
-    {
-      std::lock_guard<std::mutex> lck(mutex);
-      if (not is_queue_active_(q_idx) or queues[q_idx].full()) {
-        return {false, std::move(value)};
-      }
-      queues[q_idx].push(std::move(value));
-    }
-    cv_empty.notify_one();
-    return {true, std::move(value)};
-  }
-
-  int wait_pop(myobj* value)
+  bool wait_pop(myobj* value)
   {
     std::unique_lock<std::mutex> lock(mutex);
     while (running) {
       if (round_robin_pop_(value)) {
-        if (nof_threads_waiting > 0) {
-          lock.unlock();
-          queues[spin_idx].cv_full.notify_one();
-        }
-        return spin_idx;
+        return true;
       }
-      nof_threads_waiting++;
-      cv_empty.wait(lock);
-      nof_threads_waiting--;
+      pushed_data = false;
+      wait_state  = true;
+      while (not pushed_data) {
+        cv_empty.wait(lock);
+      }
+      wait_state = false;
     }
     cv_exit.notify_one();
-    return -1;
+    return false;
   }
 
-  int try_pop(myobj* value)
+  bool try_pop(myobj* value)
   {
     std::unique_lock<std::mutex> lock(mutex);
-    if (running) {
-      if (round_robin_pop_(value)) {
-        if (nof_threads_waiting > 0) {
-          lock.unlock();
-          queues[spin_idx].cv_full.notify_one();
-        }
-        return spin_idx;
-      }
-      // didn't find any task
-      return -1;
-    }
-    cv_exit.notify_one();
-    return -1;
+    return running and round_robin_pop_(value);
   }
 
-  bool empty(int qidx)
-  {
-    std::lock_guard<std::mutex> lck(mutex);
-    return queues[qidx].empty();
-  }
-
-  size_t size(int qidx)
-  {
-    std::lock_guard<std::mutex> lck(mutex);
-    return queues[qidx].size();
-  }
-
-  size_t max_size(int qidx)
-  {
-    std::lock_guard<std::mutex> lck(mutex);
-    return queues[qidx].capacity();
-  }
-
-  const myobj& front(int qidx)
-  {
-    std::lock_guard<std::mutex> lck(mutex);
-    return queues[qidx].front();
-  }
-
-  void erase_queue(int qidx)
-  {
-    std::lock_guard<std::mutex> lck(mutex);
-    if (is_queue_active_(qidx)) {
-      queues[qidx].active = false;
-      while (not queues[qidx].empty()) {
-        queues[qidx].pop();
-      }
-    }
-  }
-
-  bool is_queue_active(int qidx)
-  {
-    std::lock_guard<std::mutex> lck(mutex);
-    return is_queue_active_(qidx);
-  }
-
-  queue_handle get_queue_handler() { return {this, add_queue()}; }
-  queue_handle get_queue_handler(uint32_t size) { return {this, add_queue(size)}; }
-
 private:
-  bool is_queue_active_(int qidx) const { return running and queues[qidx].active; }
-
   bool round_robin_pop_(myobj* value)
   {
     // Round-robin for all queues
-    for (const circular_buffer& q : queues) {
-      spin_idx = (spin_idx + 1) % queues.size();
-      if (is_queue_active_(spin_idx) and not queues[spin_idx].empty()) {
-        if (value) {
-          *value = std::move(queues[spin_idx].front());
-        }
-        queues[spin_idx].pop();
+    auto     it    = queues.begin() + spin_idx;
+    uint32_t count = 0;
+    for (; count < queues.size(); ++count, ++it) {
+      if (it == queues.end()) {
+        it = queues.begin(); // wrap-around
+      }
+      if (it->try_pop(*value)) {
+        spin_idx = (spin_idx + count + 1) % queues.size();
         return true;
       }
     }
     return false;
   }
+  /// Called by the producer threads to signal the consumer to unlock in wait_pop
+  void signal_pushed_data()
+  {
+    {
+      std::lock_guard<std::mutex> lock(mutex);
+      if (pushed_data) {
+        return;
+      }
+      pushed_data = true;
+    }
+    cv_empty.notify_one();
+  }
 
-  std::mutex                   mutex;
-  std::condition_variable      cv_empty, cv_exit;
-  uint32_t                     spin_idx = 0;
-  bool                         running  = true;
-  std::vector<circular_buffer> queues;
-  uint32_t                     capacity            = 0;
-  uint32_t                     nof_threads_waiting = 0;
+  mutable std::mutex          mutex;
+  std::condition_variable     cv_empty, cv_exit;
+  uint32_t                    spin_idx = 0;
+  bool                        running = true, pushed_data = false, wait_state = false;
+  std::deque<input_port_impl> queues;
+  uint32_t                    default_capacity = 0;
 };
 
+template <typename T>
+using queue_handle = typename multiqueue_handler<T>::queue_handle;
+
 //! Specialization for tasks
 using task_multiqueue   = multiqueue_handler<move_task_t>;
 using task_queue_handle = task_multiqueue::queue_handle;
diff --git a/lib/include/srsran/common/task_scheduler.h b/lib/include/srsran/common/task_scheduler.h
index 936098deb..1a957927f 100644
--- a/lib/include/srsran/common/task_scheduler.h
+++ b/lib/include/srsran/common/task_scheduler.h
@@ -35,20 +35,20 @@ public:
   explicit task_scheduler(uint32_t default_extern_tasks_size = 512, uint32_t nof_timers_prealloc = 100) :
     external_tasks{default_extern_tasks_size}, timers{nof_timers_prealloc}
   {
-    background_queue_id = external_tasks.add_queue();
+    background_queue = external_tasks.add_queue();
   }
   task_scheduler(const task_scheduler&) = delete;
   task_scheduler(task_scheduler&&)      = delete;
   task_scheduler& operator=(const task_scheduler&) = delete;
   task_scheduler& operator=(task_scheduler&&) = delete;
 
-  void stop() { external_tasks.reset(); }
+  void stop() { external_tasks.stop(); }
 
   srsran::unique_timer get_unique_timer() { return timers.get_unique_timer(); }
 
   //! Creates new queue for tasks coming from external thread
-  srsran::task_queue_handle make_task_queue() { return external_tasks.get_queue_handler(); }
-  srsran::task_queue_handle make_task_queue(uint32_t qsize) { return external_tasks.get_queue_handler(qsize); }
+  srsran::task_queue_handle make_task_queue() { return external_tasks.add_queue(); }
+  srsran::task_queue_handle make_task_queue(uint32_t qsize) { return external_tasks.add_queue(qsize); }
 
   //! Delays a task processing by duration_ms
   template <typename F>
@@ -64,7 +64,7 @@ public:
   void notify_background_task_result(srsran::move_task_t task)
   {
     // run the notification in next tic
-    external_tasks.push(background_queue_id, std::move(task));
+    background_queue.push(std::move(task));
   }
 
   //! Updates timers, and run any pending internal tasks.
@@ -76,7 +76,7 @@ public:
   bool run_next_task()
   {
     srsran::move_task_t task{};
-    if (external_tasks.wait_pop(&task) >= 0) {
+    if (external_tasks.wait_pop(&task)) {
       task();
       run_all_internal_tasks();
       return true;
@@ -90,7 +90,7 @@ public:
   {
     run_all_internal_tasks();
     srsran::move_task_t task{};
-    while (external_tasks.try_pop(&task) >= 0) {
+    while (external_tasks.try_pop(&task)) {
       task();
       run_all_internal_tasks();
     }
@@ -110,9 +110,9 @@ private:
     }
   }
 
-  int                     background_queue_id = -1; ///< Queue for handling the outcomes of tasks run in the background
-  srsran::task_multiqueue external_tasks;
-  srsran::timer_handler   timers;
+  srsran::task_multiqueue         external_tasks;
+  srsran::task_queue_handle       background_queue; ///< Queue for handling the outcomes of tasks run in the background
+  srsran::timer_handler           timers;
   std::deque<srsran::move_task_t> internal_tasks; ///< enqueues stack tasks from within main thread. Avoids locking
 };
 
diff --git a/lib/include/srsran/phy/ch_estimation/csi_rs.h b/lib/include/srsran/phy/ch_estimation/csi_rs.h
index a58d21e7e..0bc114b8b 100644
--- a/lib/include/srsran/phy/ch_estimation/csi_rs.h
+++ b/lib/include/srsran/phy/ch_estimation/csi_rs.h
@@ -48,26 +48,6 @@
  */
 #define SRSRAN_CSI_RS_NOF_FREQ_DOMAIN_ALLOC_OTHER 6
 
-/**
- * @brief Describes a measurement for NZP-CSI-RS
- * @note Used for fine tracking RSRP, SNR, CFO, SFO, and so on
- * @note srsran_csi_channel_measurements_t is used for CSI report generation
- */
-typedef struct SRSRAN_API {
-  float    rsrp;       ///< Linear scale RSRP
-  float    rsrp_dB;    ///< Logarithm scale RSRP relative to full-scale
-  float    epre;       ///< Linear scale EPRE
-  float    epre_dB;    ///< Logarithm scale EPRE relative to full-scale
-  float    n0;         ///< Linear noise level
-  float    n0_dB;      ///< Logarithm scale noise level relative to full-scale
-  float    snr_dB;     ///< Signal to noise ratio in decibels
-  float    cfo_hz;     ///< Carrier frequency offset in Hz. Only set if more than 2 symbols are available in a TRS set
-  float    cfo_hz_max; ///< Maximum CFO in Hz that can be measured. It is set to 0 if CFO cannot be estimated
-  float    delay_us;   ///< Average measured delay in microseconds
-  uint32_t nof_re;     ///< Number of available RE for the measurement, it can be used for weighting among different
-                       ///< measurements
-} srsran_csi_trs_measurements_t;
-
 /**
  * @brief Calculates if the given periodicity implies a CSI-RS transmission in the given slot
  * @remark Described in TS 36.211 section 7.4.1.5.3 Mapping to physical resources
diff --git a/lib/include/srsran/phy/common/phy_common_nr.h b/lib/include/srsran/phy/common/phy_common_nr.h
index b8ad1406a..8fd45d81f 100644
--- a/lib/include/srsran/phy/common/phy_common_nr.h
+++ b/lib/include/srsran/phy/common/phy_common_nr.h
@@ -149,6 +149,53 @@ extern "C" {
  */
 #define SRSRAN_MAX_HARQ_PROC_UL_NR 16 // 3GPP TS 38.214 version 15.3.0 Sec. 6.1
 
+/**
+ * @brief SSB bandwidth in subcarriers, described in TS 38.211 section 7.4.3.1 Time-frequency structure of an SS/PBCH
+ * block
+ */
+#define SRSRAN_SSB_BW_SUBC 240
+
+/**
+ * @brief SSB duration in symbols, described in TS 38.211 section 7.4.3.1 Time-frequency structure of an SS/PBCH block
+ */
+#define SRSRAN_SSB_DURATION_NSYMB 4
+
+/**
+ * @brief Number of NR N_id_1 Physical Cell Identifier (PCI) as described in TS 38.211 section 7.4.2.1 Physical-layer
+ * cell identities
+ */
+#define SRSRAN_NOF_NID_1_NR 336
+
+/**
+ * @brief Number of NR N_id_2 Physical Cell Identifier (PCI) as described in TS 38.211 section 7.4.2.1 Physical-layer
+ * cell identities
+ */
+#define SRSRAN_NOF_NID_2_NR 3
+
+/**
+ * @brief Number of NR N_id Physical Cell Identifier (PCI) as described in TS 38.211 section 7.4.2.1 Physical-layer
+ * cell identities
+ */
+#define SRSRAN_NOF_NID_NR (SRSRAN_NOF_NID_1_NR * SRSRAN_NOF_NID_2_NR)
+
+/**
+ * @brief Compute N_id_1 from the Physical Cell Identifier (PCI) as described in TS 38.211 section 7.4.2.1
+ * Physical-layer cell identities
+ */
+#define SRSRAN_NID_1_NR(N_ID) ((N_ID) / SRSRAN_NOF_NID_2_NR)
+
+/**
+ * @brief Compute N_id_2 from the Physical Cell Identifier (PCI) as described in TS 38.211 section 7.4.2.1
+ * Physical-layer cell identities
+ */
+#define SRSRAN_NID_2_NR(N_ID) ((N_ID) % SRSRAN_NOF_NID_2_NR)
+
+/**
+ * @brief SSB number of resource elements, described in TS 38.211 section 7.4.3.1 Time-frequency structure of an SS/PBCH
+ * block
+ */
+#define SRSRAN_SSB_NOF_RE (SRSRAN_SSB_BW_SUBC * SRSRAN_SSB_DURATION_NSYMB)
+
 typedef enum SRSRAN_API {
   srsran_coreset_mapping_type_non_interleaved = 0,
   srsran_coreset_mapping_type_interleaved,
@@ -385,6 +432,26 @@ typedef struct SRSRAN_API {
   srsran_tdd_pattern_t pattern2;
 } srsran_tdd_config_nr_t;
 
+/**
+ * @brief Describes a measurement based on NZP-CSI-RS or SSB-CSI
+ * @note Used for tracking RSRP, SNR, CFO, SFO, and so on
+ * @note srsran_csi_channel_measurements_t is used for CSI report generation
+ */
+typedef struct SRSRAN_API {
+  float    rsrp;       ///< Linear scale RSRP
+  float    rsrp_dB;    ///< Logarithm scale RSRP relative to full-scale
+  float    epre;       ///< Linear scale EPRE
+  float    epre_dB;    ///< Logarithm scale EPRE relative to full-scale
+  float    n0;         ///< Linear noise level
+  float    n0_dB;      ///< Logarithm scale noise level relative to full-scale
+  float    snr_dB;     ///< Signal to noise ratio in decibels
+  float    cfo_hz;     ///< Carrier frequency offset in Hz. Only set if more than 2 symbols are available in a TRS set
+  float    cfo_hz_max; ///< Maximum CFO in Hz that can be measured. It is set to 0 if CFO cannot be estimated
+  float    delay_us;   ///< Average measured delay in microseconds
+  uint32_t nof_re;     ///< Number of available RE for the measurement, it can be used for weighting among different
+  ///< measurements
+} srsran_csi_trs_measurements_t;
+
 /**
  * @brief Get the RNTI type name for NR
  * @param rnti_type RNTI type name
@@ -458,10 +525,10 @@ SRSRAN_API uint32_t srsran_min_symbol_sz_rb(uint32_t nof_prb);
  * @remark All symbol size reference and values are taken from TS 38.211 section 5.3 OFDM baseband signal generation
  * @param l0 First symbol index within the slot
  * @param l1 Second symbol index within the slot
- * @param numerology NR Carrier numerology
+ * @param scs Subcarrier spacing
  * @return Returns the time in seconds between the two symbols if the condition above is satisfied, 0 seconds otherwise
  */
-SRSRAN_API float srsran_symbol_distance_s(uint32_t l0, uint32_t l1, uint32_t numerology);
+SRSRAN_API float srsran_symbol_distance_s(uint32_t l0, uint32_t l1, srsran_subcarrier_spacing_t scs);
 
 /**
  * @brief Decides whether a given slot is configured as Downlink
@@ -483,6 +550,15 @@ SRSRAN_API bool srsran_tdd_nr_is_ul(const srsran_tdd_config_nr_t* cfg, uint32_t
 
 SRSRAN_API int srsran_carrier_to_cell(const srsran_carrier_nr_t* carrier, srsran_cell_t* cell);
 
+/**
+ * @brief Writes Channel State Information measurement into a string
+ * @param meas Provides the measurement
+ * @param str Provides string
+ * @param str_len Maximum string length
+ * @return The number of writen characters
+ */
+SRSRAN_API uint32_t srsran_csi_meas_info(const srsran_csi_trs_measurements_t* meas, char* str, uint32_t str_len);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/lib/include/srsran/phy/phch/pbch_nr.h b/lib/include/srsran/phy/phch/pbch_nr.h
new file mode 100644
index 000000000..cc31fa3b3
--- /dev/null
+++ b/lib/include/srsran/phy/phch/pbch_nr.h
@@ -0,0 +1,25 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2021 Software Radio Systems Limited
+ *
+ * By using this file, you agree to the terms and conditions set
+ * forth in the LICENSE file which can be found at the top level of
+ * the distribution.
+ *
+ */
+
+#ifndef SRSRAN_PBCH_NR_H
+#define SRSRAN_PBCH_NR_H
+
+#include "srsran/config.h"
+
+/**
+ * @brief Descibes the NR PBCH message
+ */
+typedef struct SRSRAN_API {
+  // TBD
+} srsran_pbch_msg_nr_t;
+
+#endif // SRSRAN_PBCH_NR_H
diff --git a/lib/include/srsran/phy/resampling/resampler.h b/lib/include/srsran/phy/resampling/resampler.h
index 571e5be76..da28ff735 100644
--- a/lib/include/srsran/phy/resampling/resampler.h
+++ b/lib/include/srsran/phy/resampling/resampler.h
@@ -49,19 +49,20 @@ typedef enum {
 } srsran_resampler_mode_t;
 
 /**
- * Resampler internal buffers and subcomponents
+ * @brief Resampler internal buffers and subcomponents
  */
 typedef struct {
-  srsran_resampler_mode_t mode;
-  uint32_t                ratio;
-  uint32_t                window_sz;
-  srsran_dft_plan_t       fft;
-  srsran_dft_plan_t       ifft;
-  uint32_t                state_len;
-  cf_t*                   in_buffer;
-  cf_t*                   out_buffer;
-  cf_t*                   state;
-  cf_t*                   filter;
+  srsran_resampler_mode_t mode;       ///< Interpolate or decimate mode
+  uint32_t                ratio;      ///< Decimation/Interpolation ratio
+  uint32_t                window_sz;  ///< Maximum number of processed samples
+  uint32_t                delay;      ///< Filter delay in samples
+  srsran_dft_plan_t       fft;        ///< Forward DFT
+  srsran_dft_plan_t       ifft;       ///< Backward DFT
+  uint32_t                state_len;  ///< Number of acccumulated samples in the internal state
+  cf_t*                   in_buffer;  ///< DFT input buffer
+  cf_t*                   out_buffer; ///< DFT output buffer
+  cf_t*                   state;      ///< Filter state
+  cf_t*                   filter;     ///< Frequency domain filter
 } srsran_resampler_fft_t;
 
 /**
diff --git a/lib/include/srsran/phy/sync/pss_nr.h b/lib/include/srsran/phy/sync/pss_nr.h
new file mode 100644
index 000000000..e56a10dfa
--- /dev/null
+++ b/lib/include/srsran/phy/sync/pss_nr.h
@@ -0,0 +1,49 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2021 Software Radio Systems Limited
+ *
+ * By using this file, you agree to the terms and conditions set
+ * forth in the LICENSE file which can be found at the top level of
+ * the distribution.
+ *
+ */
+
+#ifndef SRSRAN_PSS_NR_H
+#define SRSRAN_PSS_NR_H
+
+#include "srsran/config.h"
+#include "srsran/phy/common/phy_common_nr.h"
+#include <inttypes.h>
+
+/**
+ * @brief NR PSS sequence length in frequency domain
+ */
+#define SRSRAN_PSS_NR_LEN 127
+
+/**
+ * @brief NR PSS Symbol number
+ */
+#define SRSRAN_PSS_NR_SYMBOL_IDX 0
+
+/**
+ * @brief Put NR PSS sequence in the SSB grid
+ * @remark Described in TS 38.211 section 7.4.2.2 Primary synchronization signal
+ * @param ssb_grid SSB resource grid
+ * @param N_id_2 Physical cell ID 2
+ * @param beta PSS power allocation
+ * @return SRSLTE_SUCCESS if the parameters are valid, SRSLTE_ERROR code otherwise
+ */
+SRSRAN_API int srsran_pss_nr_put(cf_t ssb_grid[SRSRAN_SSB_NOF_RE], uint32_t N_id_2, float beta);
+
+/**
+ * @brief Extracts the NR PSS Least Square Estimates (LSE) from the SSB grid
+ * @param ssb_grid received SSB resource grid
+ * @param N_id_2 Physical cell ID 2
+ * @param lse Provides LSE pointer
+ * @return SRSLTE_SUCCESS if the parameters are valid, SRSLTE_ERROR code otherwise
+ */
+SRSRAN_API int srsran_pss_nr_extract_lse(const cf_t* ssb_grid, uint32_t N_id_2, cf_t lse[SRSRAN_PSS_NR_LEN]);
+
+#endif // SRSRAN_PSS_NR_H
diff --git a/lib/include/srsran/phy/sync/ssb.h b/lib/include/srsran/phy/sync/ssb.h
new file mode 100644
index 000000000..66c23f8c5
--- /dev/null
+++ b/lib/include/srsran/phy/sync/ssb.h
@@ -0,0 +1,135 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2021 Software Radio Systems Limited
+ *
+ * By using this file, you agree to the terms and conditions set
+ * forth in the LICENSE file which can be found at the top level of
+ * the distribution.
+ *
+ */
+
+#ifndef SRSRAN_SSB_H
+#define SRSRAN_SSB_H
+
+#include "srsran/config.h"
+#include "srsran/phy/common/phy_common_nr.h"
+#include "srsran/phy/dft/dft.h"
+#include "srsran/phy/phch/pbch_nr.h"
+#include <inttypes.h>
+
+/**
+ * @brief Default SSB maximum sampling rate
+ */
+#define SRSRAN_SSB_DEFAULT_MAX_SRATE_HZ 61.44e6
+
+/**
+ * @brief Default SSB minimum subcarrier spacing
+ */
+#define SRSRAN_SSB_DEFAULT_MIN_SCS srsran_subcarrier_spacing_15kHz
+
+/**
+ * @brief Default beta value, used in case they are set to zero
+ */
+#define SRSRAN_SSB_DEFAULT_BETA 1.0f
+
+/**
+ * @brief Describes SSB object initialization arguments
+ */
+typedef struct SRSRAN_API {
+  double max_srate_hz; ///< Maximum sampling rate in Hz (common for gNb and UE), set to zero to use default
+  srsran_subcarrier_spacing_t min_scs;          ///< Minimum subcarrier spacing
+  bool                        enable_correlate; ///< Enables PSS/SSS correlation and peak search (UE cell search)
+  bool                        enable_encode;    ///< Enables PBCH Encoder (intended for gNb)
+  bool                        enable_decode;    ///< Enables PBCH Decoder (intented for UE)
+  bool                        enable_measure;   ///< Enables PSS/SSS CSI measurements
+} srsran_ssb_args_t;
+
+/**
+ * @brief Describes SSB configuration arguments
+ */
+typedef struct SRSRAN_API {
+  double                      srate_hz;       ///< Current sampling rate in Hz
+  double                      freq_offset_hz; ///< SSB base-band frequency offset
+  srsran_subcarrier_spacing_t scs;            ///< SSB configured Subcarrier spacing
+  float                       beta_pss;       ////< PSS power allocation
+  float                       beta_sss;       ////< SSS power allocation
+  float                       beta_pbch;      ////< PBCH power allocation
+  float                       beta_pbch_dmrs; ////< PBCH DMRS power allocation
+} srsran_ssb_cfg_t;
+
+/**
+ * @brief Describes SSB object
+ */
+typedef struct SRSRAN_API {
+  srsran_ssb_args_t args; ///< Stores initialization arguments
+  srsran_ssb_cfg_t  cfg;  ///< Stores last configuration
+
+  /// Sampling rate dependent parameters
+  float    scs_hz;        ///< Subcarrier spacing in Hz
+  uint32_t max_symbol_sz; ///< Maximum symbol size given the minimum supported SCS and sampling rate
+  uint32_t symbol_sz;     ///< Current SSB symbol size (for the given base-band sampling rate)
+  int32_t  offset;        ///< Current SSB integer offset (multiple of SCS)
+  uint32_t cp0_sz;        ///< First symbol cyclic prefix size
+  uint32_t cp_sz;         ///< Other symbol cyclic prefix size
+
+  /// Internal Objects
+  srsran_dft_plan_t ifft; ///< IFFT object for modulating the SSB
+  srsran_dft_plan_t fft;  ///< FFT object for demodulate the SSB.
+
+  /// Frequency/Time domain temporal data
+  cf_t* tmp_freq;
+  cf_t* tmp_time;
+} srsran_ssb_t;
+
+/**
+ * @brief Initialises configures NR SSB with the given arguments
+ * @param q SSB object
+ * @param args NR PSS initialization arguments
+ * @return SRSLTE_SUCCESS if the parameters are valid, SRSLTE_ERROR code otherwise
+ */
+SRSRAN_API int srsran_ssb_init(srsran_ssb_t* q, const srsran_ssb_args_t* args);
+
+/**
+ * @brief Frees NR SSB object
+ * @param q SSB object
+ */
+SRSRAN_API void srsran_ssb_free(srsran_ssb_t* q);
+
+/**
+ * @brief Sets SSB configuration with the current SSB configuration
+ * @param q SSB object
+ * @param cfg Current SSB configuration
+ * @return SRSLTE_SUCCESS if the parameters are valid, SRSLTE_ERROR code otherwise
+ */
+SRSRAN_API int srsran_ssb_set_cfg(srsran_ssb_t* q, const srsran_ssb_cfg_t* cfg);
+/**
+ * @brief Decodes PBCH in the given time domain signal
+ * @param q SSB object
+ * @return SRSLTE_SUCCESS if the parameters are valid, SRSLTE_ERROR code otherwise
+ */
+SRSRAN_API int srsran_ssb_decode_pbch(srsran_ssb_t* q, const cf_t* in, srsran_pbch_msg_nr_t* msg);
+
+/**
+ * @brief Adds SSB to a given signal in time domain
+ * @param q SSB object
+ * @param N_id Physical Cell Identifier
+ * @param msg NR PBCH message to transmit
+ * @return SRSLTE_SUCCESS if the parameters are valid, SRSLTE_ERROR code otherwise
+ */
+SRSRAN_API int
+srsran_ssb_add(srsran_ssb_t* q, uint32_t N_id, const srsran_pbch_msg_nr_t* msg, const cf_t* in, cf_t* out);
+
+/**
+ * @brief Perform Channel State Information (CSI) measurement from the SSB
+ * @param q NR PSS object
+ * @param N_id Physical Cell Identifier
+ * @param in Base-band signal
+ * @param meas SSB-based CSI measurement
+ * @return SRSLTE_SUCCESS if the parameters are valid, SRSLTE_ERROR code otherwise
+ */
+SRSRAN_API int
+srsran_ssb_csi_measure(srsran_ssb_t* q, uint32_t N_id, const cf_t* in, srsran_csi_trs_measurements_t* meas);
+
+#endif // SRSRAN_SSB_H
diff --git a/lib/include/srsran/phy/sync/sss_nr.h b/lib/include/srsran/phy/sync/sss_nr.h
new file mode 100644
index 000000000..87ec5092c
--- /dev/null
+++ b/lib/include/srsran/phy/sync/sss_nr.h
@@ -0,0 +1,64 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2021 Software Radio Systems Limited
+ *
+ * By using this file, you agree to the terms and conditions set
+ * forth in the LICENSE file which can be found at the top level of
+ * the distribution.
+ *
+ */
+
+#ifndef SRSRAN_SSS_NR_H
+#define SRSRAN_SSS_NR_H
+
+#include "srsran/config.h"
+#include "srsran/phy/common/phy_common_nr.h"
+#include <inttypes.h>
+
+/**
+ * @brief NR SSS sequence length in frequency domain
+ */
+#define SRSRAN_SSS_NR_LEN 127
+
+/**
+ * @brief NR SSS Symbol number
+ */
+#define SRSRAN_SSS_NR_SYMBOL_IDX 2
+
+/**
+ * @brief Put NR SSS sequence in the SSB grid
+ * @remark Described in TS 38.211 section 7.4.2.3 Secondary synchronization signal
+ * @param ssb_grid SSB resource grid
+ * @param N_id_1 Physical cell ID 1
+ * @param N_id_2 Physical cell ID 2
+ * @param beta SSS power allocation
+ * @return SRSLTE_SUCCESS if the parameters are valid, SRSLTE_ERROR code otherwise
+ */
+SRSRAN_API int srsran_sss_nr_put(cf_t ssb_grid[SRSRAN_SSB_NOF_RE], uint32_t N_id_1, uint32_t N_id_2, float beta);
+
+/**
+ * @brief Extracts the NR SSS Least Square Estimates (LSE) from the SSB grid
+ * @param ssb_grid received SSB resource grid
+ * @param N_id_1 Physical cell ID 1
+ * @param N_id_2 Physical cell ID 2
+ * @param lse Provides LSE pointer
+ * @return SRSLTE_SUCCESS if the parameters are valid, SRSLTE_ERROR code otherwise
+ */
+SRSRAN_API int
+srsran_sss_nr_extract_lse(const cf_t* ssb_grid, uint32_t N_id_1, uint32_t N_id_2, cf_t lse[SRSRAN_SSS_NR_LEN]);
+
+/**
+ * @brief Find the best SSS sequence given the N_id_2 and the SSB resource grid
+ * @attention Assumes the SSB is synchronized and the average delay is pre-compensated
+ * @param ssb_grid The SSB resource grid to search
+ * @param N_id_2 Fix N_id_2 to search, it reduces the search space 1/3
+ * @param norm_corr Normalised correlation of the best found sequence
+ * @param found_N_id_1 The N_id_1 of the best sequence
+ * @return SRSLTE_SUCCESS if the parameters are valid, SRSLTE_ERROR code otherwise
+ */
+SRSRAN_API int
+srsran_sss_nr_find(const cf_t ssb_grid[SRSRAN_SSB_NOF_RE], uint32_t N_id_2, float* norm_corr, uint32_t* found_N_id_1);
+
+#endif // SRSRAN_SSS_NR_H
diff --git a/lib/src/phy/common/phy_common_nr.c b/lib/src/phy/common/phy_common_nr.c
index eb89ade9a..3c85e3708 100644
--- a/lib/src/phy/common/phy_common_nr.c
+++ b/lib/src/phy/common/phy_common_nr.c
@@ -183,8 +183,10 @@ float srsran_symbol_distance_s(uint32_t l0, uint32_t l1, srsran_subcarrier_spaci
   // Compute reference FFT size
   uint32_t N = (2048 + 144) * count + extra_cp;
 
+  float TS = SRSRAN_LTE_TS / (float)(1U << (uint32_t)scs);
+
   // Return symbol distance in microseconds
-  return (N << (uint32_t)scs) * SRSRAN_LTE_TS;
+  return (float)N * TS;
 }
 
 bool srsran_tdd_nr_is_dl(const srsran_tdd_config_nr_t* cfg, uint32_t numerology, uint32_t slot_idx)
@@ -276,3 +278,21 @@ int srsran_carrier_to_cell(const srsran_carrier_nr_t* carrier, srsran_cell_t* ce
 
   return SRSRAN_SUCCESS;
 }
+
+uint32_t srsran_csi_meas_info(const srsran_csi_trs_measurements_t* meas, char* str, uint32_t str_len)
+{
+  if (meas == NULL || str == NULL || str_len == 0) {
+    return 0;
+  }
+
+  return srsran_print_check(str,
+                            str_len,
+                            0,
+                            "rsrp=%+.1f epre=%+.1f n0=%+.1f snr=%+.1f cfo=%+.1f delay=%+.1f",
+                            meas->rsrp_dB,
+                            meas->epre_dB,
+                            meas->n0_dB,
+                            meas->snr_dB,
+                            meas->cfo_hz,
+                            meas->delay_us);
+}
diff --git a/lib/src/phy/resampling/resampler.c b/lib/src/phy/resampling/resampler.c
index 1a3eac01d..cdc376f23 100644
--- a/lib/src/phy/resampling/resampler.c
+++ b/lib/src/phy/resampling/resampler.c
@@ -34,6 +34,11 @@
  */
 #define RESAMPLER_BETA 0.45
 
+/**
+ * Filter delay in multiples of ratio
+ */
+#define RESAMPLER_DELAY 7
+
 /**
  * The FFT size power is determined from the ratio logarithm in base 2 plus the following parameter
  */
@@ -70,21 +75,26 @@ int srsran_resampler_fft_init(srsran_resampler_fft_t* q, srsran_resampler_mode_t
   uint32_t output_fft_size = 0;
   uint32_t high_size       = base_size * ratio;
 
+  // Select FFT/IFFT sizes filter delay and window size. For best performance and avoid aliasing, the window size shall
+  // be as big as the input DFT subtracting the filter length at the input rate
   switch (mode) {
     case SRSRAN_RESAMPLER_MODE_INTERPOLATE:
       input_fft_size  = base_size;
       output_fft_size = high_size;
+      q->delay        = RESAMPLER_DELAY * ratio;
+      q->window_sz    = input_fft_size - 2 * RESAMPLER_DELAY;
       break;
     case SRSRAN_RESAMPLER_MODE_DECIMATE:
     default:
       input_fft_size  = high_size;
       output_fft_size = base_size;
+      q->delay        = RESAMPLER_DELAY * ratio;
+      q->window_sz    = input_fft_size - 2 * q->delay;
       break;
   }
 
-  q->mode      = mode;
-  q->ratio     = ratio;
-  q->window_sz = input_fft_size / 4;
+  q->mode  = mode;
+  q->ratio = ratio;
 
   q->in_buffer = srsran_vec_cf_malloc(high_size);
   if (q->in_buffer == NULL) {
@@ -120,11 +130,20 @@ int srsran_resampler_fft_init(srsran_resampler_fft_t* q, srsran_resampler_mode_t
     return SRSRAN_ERROR;
   }
 
+  // Calculate absolute filter delay
+  double delay = (double)q->delay;
+  if (mode == SRSRAN_RESAMPLER_MODE_INTERPOLATE) {
+    delay = (double)(high_size - q->delay);
+  }
+
   // Compute time domain filter coefficients, see raised cosine formula in section "1.2 Impulse Response" of
   // https://dspguru.com/dsp/reference/raised-cosine-and-root-raised-cosine-formulas/
   double T = (double)1.0;
   for (int32_t i = 0; i < high_size; i++) {
-    double t = ((double)i - (double)high_size / 2.0) / (double)ratio;
+    // Convert to time
+    double t = ((double)i - delay) / (double)ratio;
+
+    // Compute coefficient
     double h = 1.0 / T;
     if (isnormal(t)) {
       h = sin(M_PI * t / T);
@@ -135,6 +154,11 @@ int srsran_resampler_fft_init(srsran_resampler_fft_t* q, srsran_resampler_mode_t
     q->in_buffer[i] = (float)h;
   }
 
+  if (srsran_verbose >= SRSRAN_VERBOSE_INFO && !handler_registered) {
+    printf("h_%s=", q->mode == SRSRAN_RESAMPLER_MODE_INTERPOLATE ? "interp" : "decimate");
+    srsran_vec_fprint_c(stdout, q->in_buffer, high_size);
+  }
+
   // Compute frequency domain coefficients, since the filter is symmetrical, it does not matter whether FFT or iFFT
   if (mode == SRSRAN_RESAMPLER_MODE_INTERPOLATE) {
     srsran_dft_run_guru_c(&q->ifft);
@@ -179,14 +203,11 @@ static void resampler_fft_interpolate(srsran_resampler_fft_t* q, const cf_t* inp
       // Execute FFT
       srsran_dft_run_guru_c(&q->fft);
 
-      // Replicate input spectrum
-      for (uint32_t i = 1; i < q->ratio; i++) {
-        srsran_vec_cf_copy(&q->out_buffer[q->fft.size * i], q->out_buffer, q->fft.size);
+      // Replicate input spectrum and filter at same time
+      for (uint32_t i = 0; i < q->ratio; i++) {
+        srsran_vec_prod_ccc(q->out_buffer, &q->filter[q->fft.size * i], &q->in_buffer[q->fft.size * i], q->fft.size);
       }
 
-      // Apply filtering
-      srsran_vec_prod_ccc(q->out_buffer, q->filter, q->in_buffer, q->ifft.size);
-
       // Execute iFFT
       srsran_dft_run_guru_c(&q->ifft);
     } else {
@@ -224,7 +245,7 @@ static void resampler_fft_decimate(srsran_resampler_fft_t* q, const cf_t* input,
 
     if (input) {
       // Copy input samples
-      srsran_vec_cf_copy(q->in_buffer, &input[count], q->window_sz);
+      srsran_vec_cf_copy(q->in_buffer, &input[count], n);
 
       // Pad zeroes
       srsran_vec_cf_zero(&q->in_buffer[n], q->fft.size - n);
@@ -232,12 +253,14 @@ static void resampler_fft_decimate(srsran_resampler_fft_t* q, const cf_t* input,
       // Execute FFT
       srsran_dft_run_guru_c(&q->fft);
 
-      // Apply filtering and cut
-      srsran_vec_prod_ccc(q->out_buffer, q->filter, q->in_buffer, q->ifft.size / 2);
-      srsran_vec_prod_ccc(&q->out_buffer[q->fft.size - q->ifft.size / 2],
-                          &q->filter[q->fft.size - q->ifft.size / 2],
-                          &q->in_buffer[q->ifft.size / 2],
-                          q->ifft.size / 2);
+      // Apply filter
+      srsran_vec_prod_ccc(q->out_buffer, q->filter, q->out_buffer, q->fft.size);
+
+      // Decimate
+      srsran_vec_cf_copy(q->in_buffer, q->out_buffer, q->ifft.size);
+      for (uint32_t i = 1; i < q->ratio; i++) {
+        srsran_vec_sum_ccc(&q->out_buffer[q->ifft.size * i], q->in_buffer, q->in_buffer, q->ifft.size);
+      }
 
       // Execute iFFT
       srsran_dft_run_guru_c(&q->ifft);
@@ -316,5 +339,5 @@ uint32_t srsran_resampler_fft_get_delay(srsran_resampler_fft_t* q)
     return UINT32_MAX;
   }
 
-  return q->ifft.size / 2;
-}
\ No newline at end of file
+  return q->delay;
+}
diff --git a/lib/src/phy/resampling/test/resampler_test.c b/lib/src/phy/resampling/test/resampler_test.c
index 08ace27ad..42aa18e43 100644
--- a/lib/src/phy/resampling/test/resampler_test.c
+++ b/lib/src/phy/resampling/test/resampler_test.c
@@ -19,6 +19,7 @@
  *
  */
 
+#include "srsran/phy/channel/ch_awgn.h"
 #include "srsran/phy/resampling/resampler.h"
 #include "srsran/phy/utils/debug.h"
 #include "srsran/phy/utils/vector.h"
@@ -29,12 +30,19 @@
 static uint32_t buffer_size = 1920;
 static uint32_t factor      = 2;
 static uint32_t repetitions = 2;
+static enum {
+  WAVE_SINE = 0,
+  WAVE_DELTA,
+  WAVE_STEP,
+  WAVE_GAUSS,
+} wave = WAVE_SINE;
 
 static void usage(char* prog)
 {
   printf("Usage: %s [sfr]\n", prog);
   printf("\t-s Buffer size [Default %d]\n", buffer_size);
-  printf("\t-f Buffer size [Default %d]\n", factor);
+  printf("\t-f Interpolation/Decimation factor [Default %d]\n", factor);
+  printf("\t-w Wave type: sine, step, delta [Default sine]\n");
   printf("\t-f r [Default %d]\n", repetitions);
 }
 
@@ -42,7 +50,7 @@ static void parse_args(int argc, char** argv)
 {
   int opt;
 
-  while ((opt = getopt(argc, argv, "sfr")) != -1) {
+  while ((opt = getopt(argc, argv, "sfrvw")) != -1) {
     switch (opt) {
       case 's':
         buffer_size = (uint32_t)strtol(argv[optind], NULL, 10);
@@ -53,6 +61,33 @@ static void parse_args(int argc, char** argv)
       case 'r':
         repetitions = (uint32_t)strtol(argv[optind], NULL, 10);
         break;
+      case 'v':
+        srsran_verbose++;
+        break;
+      case 'w':
+        if (strcmp(argv[optind], "sine") == 0) {
+          wave = WAVE_SINE;
+          break;
+        }
+
+        if (strcmp(argv[optind], "delta") == 0) {
+          wave = WAVE_DELTA;
+          break;
+        }
+
+        if (strcmp(argv[optind], "step") == 0) {
+          wave = WAVE_STEP;
+          break;
+        }
+
+        if (strcmp(argv[optind], "gauss") == 0) {
+          wave = WAVE_GAUSS;
+          break;
+        }
+
+        printf("Invalid wave '%s'\n", argv[optind]);
+        usage(argv[0]);
+        break;
       default:
         usage(argv[0]);
         exit(-1);
@@ -65,6 +100,7 @@ int main(int argc, char** argv)
   struct timeval         t[3]   = {};
   srsran_resampler_fft_t interp = {};
   srsran_resampler_fft_t decim  = {};
+  srsran_channel_awgn_t  awgn   = {};
 
   parse_args(argc, argv);
 
@@ -81,7 +117,31 @@ int main(int argc, char** argv)
   }
 
   srsran_vec_cf_zero(src, buffer_size);
-  srsran_vec_gen_sine(1.0f, 0.01f, src, buffer_size / 10);
+
+  switch (wave) {
+    case WAVE_SINE:
+      srsran_vec_gen_sine(1.0f, 0.01f, src, buffer_size / 2);
+      break;
+    case WAVE_DELTA:
+      src[0] = 1.0f;
+      break;
+    case WAVE_STEP:
+      for (uint32_t i = 0; i < buffer_size; i++) {
+        src[i] = 1.0f;
+      }
+      break;
+    case WAVE_GAUSS:
+      srsran_channel_awgn_init(&awgn, 0);
+      srsran_channel_awgn_set_n0(&awgn, 0);
+      srsran_channel_awgn_run_c(&awgn, src, src, buffer_size);
+      srsran_channel_awgn_free(&awgn);
+      break;
+  }
+
+  if (srsran_verbose >= SRSRAN_VERBOSE_INFO && !handler_registered) {
+    printf("signal=");
+    srsran_vec_fprint_c(stdout, src, buffer_size);
+  }
 
   gettimeofday(&t[1], NULL);
   for (uint32_t r = 0; r < repetitions; r++) {
@@ -91,22 +151,26 @@ int main(int argc, char** argv)
   gettimeofday(&t[2], NULL);
   get_time_interval(t);
   uint64_t duration_us = (uint64_t)(t[0].tv_sec * 1000000UL + t[0].tv_usec);
-  printf("Done %.1f Msps\n", factor * buffer_size * repetitions / (double)duration_us);
 
-  //  printf("interp=");
-  //  srsran_vec_fprint_c(stdout, interpolated, buffer_size * factor);
+  if (srsran_verbose >= SRSRAN_VERBOSE_INFO && !handler_registered) {
+    printf("interp=");
+    srsran_vec_fprint_c(stdout, interpolated, buffer_size * factor);
+    printf("decim=");
+    srsran_vec_fprint_c(stdout, decimated, buffer_size);
+  }
 
   // Check error
-  uint32_t delay    = srsran_resampler_fft_get_delay(&decim) * 2;
+  uint32_t delay    = (srsran_resampler_fft_get_delay(&decim) + srsran_resampler_fft_get_delay(&interp)) / factor;
   uint32_t nsamples = buffer_size - delay;
   srsran_vec_sub_ccc(src, &decimated[delay], interpolated, nsamples);
   float mse = sqrtf(srsran_vec_avg_power_cf(interpolated, nsamples));
-  printf("MSE: %f\n", mse);
 
-  //  printf("src=");
-  //  srsran_vec_fprint_c(stdout, src, nsamples);
-  //  printf("decim=");
-  //  srsran_vec_fprint_c(stdout, &decimated[delay], nsamples);
+  if (srsran_verbose >= SRSRAN_VERBOSE_INFO && !handler_registered) {
+    printf("recovered=");
+    srsran_vec_fprint_c(stdout, &decimated[delay], nsamples);
+  }
+
+  printf("Done %.1f Msps; MSE: %.6f\n", factor * buffer_size * repetitions / (double)duration_us, mse);
 
   srsran_resampler_fft_free(&interp);
   srsran_resampler_fft_free(&decim);
diff --git a/lib/src/phy/sync/pss_nr.c b/lib/src/phy/sync/pss_nr.c
new file mode 100644
index 000000000..488ee0f19
--- /dev/null
+++ b/lib/src/phy/sync/pss_nr.c
@@ -0,0 +1,96 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2021 Software Radio Systems Limited
+ *
+ * By using this file, you agree to the terms and conditions set
+ * forth in the LICENSE file which can be found at the top level of
+ * the distribution.
+ *
+ */
+
+#include "srsran/phy/sync/pss_nr.h"
+#include "srsran/phy/utils/vector.h"
+
+/**
+ * NR PSS First Subcarrier index
+ */
+#define PSS_NR_SUBC_BEGIN 56
+
+/**
+ * Calculates Sequence circular offset
+ */
+#define PSS_NR_SEQUENCE_M(N_id_2) ((43U * (N_id_2)) % SRSRAN_PSS_NR_LEN)
+
+/**
+ * Pregenerated modulated sequence
+ */
+static cf_t pss_nr_d[SRSRAN_PSS_NR_LEN] = {};
+
+/**
+ * Sequence generation as described in TS 38.211 clause 7.4.2.2.1
+ */
+__attribute__((constructor)) __attribute__((unused)) static void pss_nr_pregen()
+{
+  // Initialise M sequence x
+  uint32_t x[SRSRAN_PSS_NR_LEN + 7];
+  x[6] = 1;
+  x[5] = 1;
+  x[4] = 1;
+  x[3] = 0;
+  x[2] = 1;
+  x[1] = 1;
+  x[0] = 0;
+
+  // Generate M sequence x
+  for (uint32_t i = 0; i < SRSRAN_PSS_NR_LEN; i++) {
+    x[i + 7] = (x[i + 4] + x[i]) % 2;
+  }
+
+  // Modulate M sequence d
+  for (uint32_t i = 0; i < SRSRAN_PSS_NR_LEN; i++) {
+    pss_nr_d[i] = 1.0f - 2.0f * (float)x[i];
+  }
+}
+
+int srsran_pss_nr_put(cf_t ssb_grid[SRSRAN_SSB_NOF_RE], uint32_t N_id_2, float beta)
+{
+  // Verify inputs
+  if (ssb_grid == NULL || N_id_2 >= SRSRAN_NOF_NID_2_NR) {
+    return SRSRAN_ERROR;
+  }
+
+  // Calculate generation parameters
+  uint32_t m          = PSS_NR_SEQUENCE_M(N_id_2);
+  uint32_t copy_sz_1  = SRSRAN_PSS_NR_LEN - m;
+  uint32_t grid_idx_1 = SRSRAN_PSS_NR_SYMBOL_IDX * SRSRAN_SSB_BW_SUBC + PSS_NR_SUBC_BEGIN;
+  uint32_t grid_idx_2 = grid_idx_1 + copy_sz_1;
+
+  // Copy sequence from offset to the end
+  srsran_vec_sc_prod_cfc(&pss_nr_d[m], beta, &ssb_grid[grid_idx_1], copy_sz_1);
+
+  // Copy sequence from 0 to offset
+  srsran_vec_sc_prod_cfc(&pss_nr_d[0], beta, &ssb_grid[grid_idx_2], m);
+
+  return SRSRAN_SUCCESS;
+}
+
+int srsran_pss_nr_extract_lse(const cf_t* ssb_grid, uint32_t N_id_2, cf_t lse[SRSRAN_PSS_NR_LEN])
+{
+  // Verify inputs
+  if (ssb_grid == NULL || N_id_2 >= SRSRAN_NOF_NID_2_NR || lse == NULL) {
+    return SRSRAN_ERROR_INVALID_INPUTS;
+  }
+
+  // Extract PSS
+  srsran_vec_cf_copy(
+      lse, &ssb_grid[SRSRAN_PSS_NR_SYMBOL_IDX * SRSRAN_SSB_BW_SUBC + PSS_NR_SUBC_BEGIN], SRSRAN_PSS_NR_LEN);
+
+  // Estimate
+  uint32_t m = PSS_NR_SEQUENCE_M(N_id_2);
+  srsran_vec_prod_ccc(&pss_nr_d[m], lse, lse, SRSRAN_PSS_NR_LEN - m);
+  srsran_vec_prod_ccc(&pss_nr_d[0], &lse[SRSRAN_PSS_NR_LEN - m], &lse[SRSRAN_PSS_NR_LEN - m], m);
+
+  return SRSRAN_SUCCESS;
+}
diff --git a/lib/src/phy/sync/ssb.c b/lib/src/phy/sync/ssb.c
new file mode 100644
index 000000000..324b6b308
--- /dev/null
+++ b/lib/src/phy/sync/ssb.c
@@ -0,0 +1,365 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2021 Software Radio Systems Limited
+ *
+ * By using this file, you agree to the terms and conditions set
+ * forth in the LICENSE file which can be found at the top level of
+ * the distribution.
+ *
+ */
+
+#include "srsran/phy/sync/ssb.h"
+#include "srsran/phy/sync/pss_nr.h"
+#include "srsran/phy/sync/sss_nr.h"
+#include "srsran/phy/utils/debug.h"
+#include "srsran/phy/utils/vector.h"
+#include <complex.h>
+
+/*
+ * Maximum allowed maximum sampling rate error in Hz
+ */
+#define SSB_SRATE_MAX_ERROR_HZ 0.01
+
+/*
+ * Maximum allowed maximum frequency error offset in Hz
+ */
+#define SSB_FREQ_OFFSET_MAX_ERROR_HZ 0.01
+
+int srsran_ssb_init(srsran_ssb_t* q, const srsran_ssb_args_t* args)
+{
+  // Verify input parameters
+  if (q == NULL || args == NULL) {
+    return SRSRAN_ERROR_INVALID_INPUTS;
+  }
+
+  // Copy arguments
+  q->args = *args;
+
+  // Check if the maximum sampling rate is in range, force default otherwise
+  if (!isnormal(q->args.max_srate_hz) || q->args.max_srate_hz < 0.0) {
+    q->args.max_srate_hz = SRSRAN_SSB_DEFAULT_MAX_SRATE_HZ;
+  }
+
+  q->scs_hz        = (float)SRSRAN_SUBC_SPACING_NR(q->args.min_scs);
+  q->max_symbol_sz = (uint32_t)round(q->args.max_srate_hz / q->scs_hz);
+
+  // Allocate temporal data
+  q->tmp_time = srsran_vec_cf_malloc(q->max_symbol_sz);
+  q->tmp_freq = srsran_vec_cf_malloc(q->max_symbol_sz);
+  if (q->tmp_time == NULL || q->tmp_time == NULL) {
+    ERROR("Malloc");
+    return SRSRAN_ERROR;
+  }
+
+  return SRSRAN_SUCCESS;
+}
+
+void srsran_ssb_free(srsran_ssb_t* q)
+{
+  if (q == NULL) {
+    return;
+  }
+
+  if (q->tmp_time != NULL) {
+    free(q->tmp_time);
+  }
+
+  if (q->tmp_freq != NULL) {
+    free(q->tmp_freq);
+  }
+
+  srsran_dft_plan_free(&q->ifft);
+  srsran_dft_plan_free(&q->fft);
+
+  SRSRAN_MEM_ZERO(q, srsran_ssb_t, 1);
+}
+
+int srsran_ssb_set_cfg(srsran_ssb_t* q, const srsran_ssb_cfg_t* cfg)
+{
+  // Verify input parameters
+  if (q == NULL || cfg == NULL) {
+    return SRSRAN_ERROR_INVALID_INPUTS;
+  }
+
+  // Calculate subcarrier spacing in Hz
+  q->scs_hz = (double)SRSRAN_SUBC_SPACING_NR(cfg->scs);
+
+  // Calculate SSB symbol size and integer offset
+  uint32_t symbol_sz = (uint32_t)round(cfg->srate_hz / q->scs_hz);
+  q->offset          = (uint32_t)(cfg->freq_offset_hz / q->scs_hz);
+  q->cp0_sz          = (160U * symbol_sz) / 2048U;
+  q->cp_sz           = (144U * symbol_sz) / 2048U;
+
+  // Calculate SSB sampling error and check
+  double ssb_srate_error_Hz = ((double)symbol_sz * q->scs_hz) - cfg->srate_hz;
+  if (fabs(ssb_srate_error_Hz) > SSB_SRATE_MAX_ERROR_HZ) {
+    ERROR("Invalid sampling rate (%.2f MHz)", cfg->srate_hz / 1e6);
+    return SRSRAN_ERROR;
+  }
+
+  // Calculate SSB offset error and check
+  double ssb_offset_error_Hz = ((double)q->offset * q->scs_hz) - cfg->freq_offset_hz;
+  if (fabs(ssb_offset_error_Hz) > SSB_FREQ_OFFSET_MAX_ERROR_HZ) {
+    ERROR("SSB Offset error exceeds maximum allowed");
+    return SRSRAN_ERROR;
+  }
+
+  // Verify symbol size
+  if (q->max_symbol_sz < symbol_sz) {
+    ERROR("New symbol size (%d) exceeds maximum symbol size (%d)", symbol_sz, q->max_symbol_sz);
+  }
+
+  // Replan iFFT
+  if ((q->args.enable_encode) && q->symbol_sz != symbol_sz) {
+    // free the current IFFT, it internally checks if the plan was created
+    srsran_dft_plan_free(&q->ifft);
+
+    // Creates DFT plan
+    if (srsran_dft_plan_guru_c(&q->ifft, (int)symbol_sz, SRSRAN_DFT_BACKWARD, q->tmp_freq, q->tmp_time, 1, 1, 1, 1, 1) <
+        SRSRAN_SUCCESS) {
+      ERROR("Error creating iDFT");
+      return SRSRAN_ERROR;
+    }
+  }
+
+  // Replan FFT
+  if ((q->args.enable_measure || q->args.enable_decode) && q->symbol_sz != symbol_sz) {
+    // free the current FFT, it internally checks if the plan was created
+    srsran_dft_plan_free(&q->fft);
+
+    // Creates DFT plan
+    if (srsran_dft_plan_guru_c(&q->fft, (int)symbol_sz, SRSRAN_DFT_FORWARD, q->tmp_time, q->tmp_freq, 1, 1, 1, 1, 1) <
+        SRSRAN_SUCCESS) {
+      ERROR("Error creating iDFT");
+      return SRSRAN_ERROR;
+    }
+  }
+
+  // Finally, copy configuration
+  q->cfg       = *cfg;
+  q->symbol_sz = symbol_sz;
+
+  if (!isnormal(q->cfg.beta_pss)) {
+    q->cfg.beta_pss = SRSRAN_SSB_DEFAULT_BETA;
+  }
+
+  if (!isnormal(q->cfg.beta_sss)) {
+    q->cfg.beta_sss = SRSRAN_SSB_DEFAULT_BETA;
+  }
+
+  if (!isnormal(q->cfg.beta_pbch)) {
+    q->cfg.beta_pbch = SRSRAN_SSB_DEFAULT_BETA;
+  }
+
+  if (!isnormal(q->cfg.beta_pbch_dmrs)) {
+    q->cfg.beta_pbch = SRSRAN_SSB_DEFAULT_BETA;
+  }
+
+  return SRSRAN_SUCCESS;
+}
+
+int srsran_ssb_add(srsran_ssb_t* q, uint32_t N_id, const srsran_pbch_msg_nr_t* msg, const cf_t* in, cf_t* out)
+{
+  // Verify input parameters
+  if (q == NULL || N_id >= SRSRAN_NOF_NID_NR || msg == NULL || in == NULL || out == NULL) {
+    return SRSRAN_ERROR_INVALID_INPUTS;
+  }
+
+  if (!q->args.enable_encode) {
+    ERROR("SSB is not configured for encode");
+    return SRSRAN_ERROR;
+  }
+
+  uint32_t N_id_1                      = SRSRAN_NID_1_NR(N_id);
+  uint32_t N_id_2                      = SRSRAN_NID_2_NR(N_id);
+  cf_t     ssb_grid[SRSRAN_SSB_NOF_RE] = {};
+
+  // Put PSS
+  if (srsran_pss_nr_put(ssb_grid, N_id_2, q->cfg.beta_pss) < SRSRAN_SUCCESS) {
+    ERROR("Error putting PSS");
+    return SRSRAN_ERROR;
+  }
+
+  // Put SSS
+  if (srsran_sss_nr_put(ssb_grid, N_id_1, N_id_2, q->cfg.beta_sss) < SRSRAN_SUCCESS) {
+    ERROR("Error putting PSS");
+    return SRSRAN_ERROR;
+  }
+
+  // Put PBCH DMRS
+  // ...
+
+  // Put PBCH payload
+  // ...
+
+  // Initialise frequency domain
+  srsran_vec_cf_zero(q->tmp_freq, q->symbol_sz);
+
+  // Modulate
+  const cf_t* in_ptr  = in;
+  cf_t*       out_ptr = out;
+  for (uint32_t l = 0; l < SRSRAN_SSB_DURATION_NSYMB; l++) {
+    // Get CP length
+    uint32_t cp_len = (l == 0) ? q->cp0_sz : q->cp_sz;
+
+    // Select symbol in grid
+    cf_t* ptr = &ssb_grid[l * SRSRAN_SSB_BW_SUBC];
+
+    // Map grid into frequency domain symbol
+    if (q->offset >= SRSRAN_SSB_BW_SUBC / 2) {
+      srsran_vec_cf_copy(&q->tmp_freq[q->offset - SRSRAN_SSB_BW_SUBC / 2], ptr, SRSRAN_SSB_BW_SUBC);
+    } else if (q->offset <= -SRSRAN_SSB_BW_SUBC / 2) {
+      srsran_vec_cf_copy(&q->tmp_freq[q->symbol_sz + q->offset - SRSRAN_SSB_BW_SUBC / 2], ptr, SRSRAN_SSB_BW_SUBC);
+    } else {
+      srsran_vec_cf_copy(&q->tmp_freq[0], &ptr[SRSRAN_SSB_BW_SUBC / 2 - q->offset], SRSRAN_SSB_BW_SUBC / 2 + q->offset);
+      srsran_vec_cf_copy(
+          &q->tmp_freq[q->symbol_sz - SRSRAN_SSB_BW_SUBC / 2 + q->offset], &ptr[0], SRSRAN_SSB_BW_SUBC / 2 - q->offset);
+    }
+
+    // Convert to time domain
+    srsran_dft_run_guru_c(&q->ifft);
+
+    // Normalise output
+    float norm = sqrtf((float)q->symbol_sz);
+    if (isnormal(norm)) {
+      srsran_vec_sc_prod_cfc(q->tmp_time, 1.0f / norm, q->tmp_time, q->symbol_sz);
+    }
+
+    // Add cyclic prefix to input;
+    srsran_vec_sum_ccc(in_ptr, &q->tmp_time[q->symbol_sz - cp_len], out_ptr, cp_len);
+    in_ptr += cp_len;
+    out_ptr += cp_len;
+
+    // Add symbol to the input baseband
+    srsran_vec_sum_ccc(in_ptr, q->tmp_time, out_ptr, q->symbol_sz);
+    in_ptr += q->symbol_sz;
+    out_ptr += q->symbol_sz;
+  }
+
+  return SRSRAN_SUCCESS;
+}
+
+int srsran_ssb_csi_measure(srsran_ssb_t* q, uint32_t N_id, const cf_t* in, srsran_csi_trs_measurements_t* meas)
+{
+  // Verify inputs
+  if (q == NULL || N_id >= SRSRAN_NOF_NID_NR || in == NULL || meas == NULL || !isnormal(q->scs_hz)) {
+    return SRSRAN_ERROR_INVALID_INPUTS;
+  }
+
+  if (!q->args.enable_measure) {
+    ERROR("SSB is not configured for measure");
+    return SRSRAN_ERROR;
+  }
+
+  uint32_t N_id_1                      = SRSRAN_NID_1_NR(N_id);
+  uint32_t N_id_2                      = SRSRAN_NID_2_NR(N_id);
+  cf_t     ssb_grid[SRSRAN_SSB_NOF_RE] = {};
+
+  // Demodulate
+  const cf_t* in_ptr = in;
+  for (uint32_t l = 0; l < SRSRAN_SSB_DURATION_NSYMB; l++) {
+    // Get CP length
+    uint32_t cp_len = (l == 0) ? q->cp0_sz : q->cp_sz;
+
+    // Advance half CP, to avoid inter symbol interference
+    in_ptr += SRSRAN_FLOOR(cp_len, 2);
+
+    // Copy FFT window in temporal time domain buffer
+    srsran_vec_cf_copy(q->tmp_time, in_ptr, q->symbol_sz);
+    in_ptr += q->symbol_sz + SRSRAN_CEIL(cp_len, 2);
+
+    // Convert to frequency domain
+    srsran_dft_run_guru_c(&q->fft);
+
+    // Compensate half CP delay
+    srsran_vec_apply_cfo(q->tmp_freq, SRSRAN_CEIL(cp_len, 2) / (float)(q->symbol_sz), q->tmp_freq, q->symbol_sz);
+
+    // Select symbol in grid
+    cf_t* ptr = &ssb_grid[l * SRSRAN_SSB_BW_SUBC];
+
+    // Map frequency domain symbol into the SSB grid
+    if (q->offset >= SRSRAN_SSB_BW_SUBC / 2) {
+      srsran_vec_cf_copy(ptr, &q->tmp_freq[q->offset - SRSRAN_SSB_BW_SUBC / 2], SRSRAN_SSB_BW_SUBC);
+    } else if (q->offset <= -SRSRAN_SSB_BW_SUBC / 2) {
+      srsran_vec_cf_copy(ptr, &q->tmp_freq[q->symbol_sz + q->offset - SRSRAN_SSB_BW_SUBC / 2], SRSRAN_SSB_BW_SUBC);
+    } else {
+      srsran_vec_cf_copy(&ptr[SRSRAN_SSB_BW_SUBC / 2 - q->offset], &q->tmp_freq[0], SRSRAN_SSB_BW_SUBC / 2 + q->offset);
+      srsran_vec_cf_copy(
+          &ptr[0], &q->tmp_freq[q->symbol_sz - SRSRAN_SSB_BW_SUBC / 2 + q->offset], SRSRAN_SSB_BW_SUBC / 2 - q->offset);
+    }
+
+    // Normalize
+    float norm = sqrtf((float)q->symbol_sz);
+    if (isnormal(norm)) {
+      srsran_vec_sc_prod_cfc(ptr, 1.0f / norm, ptr, SRSRAN_SSB_BW_SUBC);
+    }
+  }
+
+  // Extract PSS LSE
+  cf_t pss_lse[SRSRAN_PSS_NR_LEN];
+  cf_t sss_lse[SRSRAN_SSS_NR_LEN];
+  if (srsran_pss_nr_extract_lse(ssb_grid, N_id_2, pss_lse) < SRSRAN_SUCCESS ||
+      srsran_sss_nr_extract_lse(ssb_grid, N_id_1, N_id_2, sss_lse) < SRSRAN_SUCCESS) {
+    ERROR("Error extracting LSE");
+    return SRSRAN_ERROR;
+  }
+
+  // Estimate average delay
+  float delay_pss_norm = srsran_vec_estimate_frequency(pss_lse, SRSRAN_PSS_NR_LEN);
+  float delay_sss_norm = srsran_vec_estimate_frequency(sss_lse, SRSRAN_SSS_NR_LEN);
+  float delay_avg_norm = (delay_pss_norm + delay_sss_norm) / 2.0f;
+  float delay_avg_us   = 1e6f * delay_avg_norm / q->scs_hz;
+
+  // Pre-compensate delay
+  for (uint32_t l = 0; l < SRSRAN_SSB_DURATION_NSYMB; l++) {
+    srsran_vec_apply_cfo(
+        &ssb_grid[SRSRAN_SSB_BW_SUBC * l], delay_avg_norm, &ssb_grid[SRSRAN_SSB_BW_SUBC * l], SRSRAN_SSB_BW_SUBC);
+  }
+
+  // Extract LSE again
+  if (srsran_pss_nr_extract_lse(ssb_grid, N_id_2, pss_lse) < SRSRAN_SUCCESS ||
+      srsran_sss_nr_extract_lse(ssb_grid, N_id_1, N_id_2, sss_lse) < SRSRAN_SUCCESS) {
+    ERROR("Error extracting LSE");
+    return SRSRAN_ERROR;
+  }
+
+  // Estimate average EPRE
+  float epre_pss = srsran_vec_avg_power_cf(pss_lse, SRSRAN_PSS_NR_LEN);
+  float epre_sss = srsran_vec_avg_power_cf(sss_lse, SRSRAN_SSS_NR_LEN);
+  float epre     = (epre_pss + epre_sss) / 2.0f;
+
+  // Compute correlation
+  cf_t corr_pss = srsran_vec_acc_cc(pss_lse, SRSRAN_PSS_NR_LEN) / SRSRAN_PSS_NR_LEN;
+  cf_t corr_sss = srsran_vec_acc_cc(sss_lse, SRSRAN_SSS_NR_LEN) / SRSRAN_SSS_NR_LEN;
+
+  // Compute CFO in Hz
+  float distance_s = srsran_symbol_distance_s(SRSRAN_PSS_NR_SYMBOL_IDX, SRSRAN_SSS_NR_SYMBOL_IDX, q->cfg.scs);
+  float cfo_hz_max = 1.0f / distance_s;
+  float cfo_hz     = cargf(corr_pss * conjf(corr_sss)) / (2.0f * M_PI) * cfo_hz_max;
+
+  // Compute average RSRP
+  float rsrp = (SRSRAN_CSQABS(corr_pss) + SRSRAN_CSQABS(corr_sss)) / 2.0f;
+
+  // Compute Noise
+  float n0 = 1e-9; // Almost 0
+  if (epre > rsrp) {
+    n0 = epre - rsrp;
+  }
+
+  // Put measurements together
+  meas->epre       = epre;
+  meas->epre_dB    = srsran_convert_power_to_dB(epre);
+  meas->rsrp       = rsrp;
+  meas->epre_dB    = srsran_convert_power_to_dB(rsrp);
+  meas->n0         = n0;
+  meas->n0_dB      = srsran_convert_power_to_dB(n0);
+  meas->snr_dB     = meas->rsrp_dB - meas->n0_dB;
+  meas->cfo_hz     = cfo_hz;
+  meas->cfo_hz_max = cfo_hz_max;
+  meas->delay_us   = delay_avg_us; // Convert the delay to microseconds
+  meas->nof_re     = SRSRAN_PSS_NR_LEN + SRSRAN_SSS_NR_LEN;
+
+  return SRSRAN_SUCCESS;
+}
\ No newline at end of file
diff --git a/lib/src/phy/sync/sss_nr.c b/lib/src/phy/sync/sss_nr.c
new file mode 100644
index 000000000..0ef3fa0bc
--- /dev/null
+++ b/lib/src/phy/sync/sss_nr.c
@@ -0,0 +1,208 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2021 Software Radio Systems Limited
+ *
+ * By using this file, you agree to the terms and conditions set
+ * forth in the LICENSE file which can be found at the top level of
+ * the distribution.
+ *
+ */
+
+#include "srsran/phy/sync/sss_nr.h"
+#include "srsran/phy/utils/vector.h"
+
+/**
+ * NR SSS First Subcarrier index
+ */
+#define SSS_NR_SUBC_BEGIN 56
+
+/**
+ * Number of possible M1 shifts
+ */
+#define SSS_NR_NOF_M1 112U
+
+/**
+ * Number of possible M0 shifts
+ */
+#define SSS_NR_NOF_M0 SRSRAN_FLOOR(SRSRAN_NOF_NID_1_NR, SSS_NR_NOF_M1)
+
+/**
+ * Calculates Sequence circular offset M0 value
+ */
+#define SSS_NR_SEQUENCE_M0(N_id_1, N_id_2)                                                                             \
+  ((15U * SRSRAN_FLOOR(N_id_1, SSS_NR_NOF_M1) + 5 * (N_id_2)) % SRSRAN_SSS_NR_LEN)
+
+/**
+ * Calculates Sequence circular offset M1 value
+ */
+#define SSS_NR_SEQUENCE_M1(N_id_1) (N_id_1 % SSS_NR_NOF_M1)
+
+/**
+ * Pregenerated modulated sequences
+ */
+static cf_t sss_nr_d0[SRSRAN_SSS_NR_LEN] = {};
+static cf_t sss_nr_d1[SRSRAN_SSS_NR_LEN] = {};
+
+/**
+ * Sequence generation as described in TS 38.211 clause 7.4.2.2.1
+ */
+__attribute__((constructor)) __attribute__((unused)) static void sss_nr_pregen()
+{
+  // Initialise M sequence x0
+  uint32_t x0[SRSRAN_SSS_NR_LEN + 7];
+  x0[6] = 0;
+  x0[5] = 0;
+  x0[4] = 0;
+  x0[3] = 0;
+  x0[2] = 0;
+  x0[1] = 0;
+  x0[0] = 1;
+
+  // Initialise M sequence x1
+  uint32_t x1[SRSRAN_SSS_NR_LEN + 7];
+  x1[6] = 0;
+  x1[5] = 0;
+  x1[4] = 0;
+  x1[3] = 0;
+  x1[2] = 0;
+  x1[1] = 0;
+  x1[0] = 1;
+
+  // Generate M sequence x
+  for (uint32_t i = 0; i < SRSRAN_SSS_NR_LEN; i++) {
+    x0[i + 7] = (x0[i + 4] + x0[i]) % 2;
+    x1[i + 7] = (x1[i + 1] + x1[i]) % 2;
+  }
+
+  // Modulate M sequence d
+  for (uint32_t i = 0; i < SRSRAN_SSS_NR_LEN; i++) {
+    sss_nr_d0[i] = 1.0f - 2.0f * (float)x0[i];
+    sss_nr_d1[i] = 1.0f - 2.0f * (float)x1[i];
+  }
+}
+
+int srsran_sss_nr_put(cf_t ssb_grid[SRSRAN_SSB_NOF_RE], uint32_t N_id_1, uint32_t N_id_2, float beta)
+{
+  // Verify inputs
+  if (ssb_grid == NULL || N_id_1 >= SRSRAN_NOF_NID_1_NR || N_id_2 >= SRSRAN_NOF_NID_2_NR) {
+    return SRSRAN_ERROR;
+  }
+
+  // Calculate generation parameters
+  uint32_t m0            = SSS_NR_SEQUENCE_M0(N_id_1, N_id_2);
+  uint32_t m1            = SSS_NR_SEQUENCE_M1(N_id_1);
+  uint32_t grid_idx_m0_1 = SRSRAN_SSS_NR_SYMBOL_IDX * SRSRAN_SSB_BW_SUBC + SSS_NR_SUBC_BEGIN;
+  uint32_t grid_idx_m0_2 = grid_idx_m0_1 + (SRSRAN_SSS_NR_LEN - m0);
+  uint32_t grid_idx_m1_1 = SRSRAN_SSS_NR_SYMBOL_IDX * SRSRAN_SSB_BW_SUBC + SSS_NR_SUBC_BEGIN;
+  uint32_t grid_idx_m1_2 = grid_idx_m1_1 + (SRSRAN_SSS_NR_LEN - m1);
+
+  // Copy d0 sequence first part from m0 to the end
+  srsran_vec_sc_prod_cfc(&sss_nr_d0[m0], beta, &ssb_grid[grid_idx_m0_1], SRSRAN_SSS_NR_LEN - m0);
+
+  // Copy d0 sequence second part from 0 to m0
+  srsran_vec_sc_prod_cfc(&sss_nr_d0[0], beta, &ssb_grid[grid_idx_m0_2], m0);
+
+  // Multiply d1 sequence first part from m1 to the end
+  srsran_vec_prod_ccc(&ssb_grid[grid_idx_m1_1], &sss_nr_d1[m1], &ssb_grid[grid_idx_m1_1], SRSRAN_SSS_NR_LEN - m1);
+
+  // Multiply d1 sequence second part from 0 to m1
+  srsran_vec_prod_ccc(&ssb_grid[grid_idx_m1_2], &sss_nr_d1[0], &ssb_grid[grid_idx_m1_2], m1);
+
+  return SRSRAN_SUCCESS;
+}
+
+int srsran_sss_nr_extract_lse(const cf_t* ssb_grid, uint32_t N_id_1, uint32_t N_id_2, cf_t lse[SRSRAN_SSS_NR_LEN])
+{
+  // Verify inputs
+  if (ssb_grid == NULL || N_id_2 >= SRSRAN_NOF_NID_2_NR || lse == NULL) {
+    return SRSRAN_ERROR_INVALID_INPUTS;
+  }
+
+  // Extract SSS
+  srsran_vec_cf_copy(
+      lse, &ssb_grid[SRSRAN_SSS_NR_SYMBOL_IDX * SRSRAN_SSB_BW_SUBC + SSS_NR_SUBC_BEGIN], SRSRAN_SSS_NR_LEN);
+
+  // Estimate
+  uint32_t m0 = SSS_NR_SEQUENCE_M0(N_id_1, N_id_2);
+  srsran_vec_prod_ccc(&sss_nr_d0[m0], lse, lse, SRSRAN_SSS_NR_LEN - m0);
+  srsran_vec_prod_ccc(&sss_nr_d0[0], &lse[SRSRAN_SSS_NR_LEN - m0], &lse[SRSRAN_SSS_NR_LEN - m0], m0);
+
+  uint32_t m1 = SSS_NR_SEQUENCE_M1(N_id_1);
+  srsran_vec_prod_ccc(&sss_nr_d1[m1], lse, lse, SRSRAN_SSS_NR_LEN - m1);
+  srsran_vec_prod_ccc(&sss_nr_d1[0], &lse[SRSRAN_SSS_NR_LEN - m1], &lse[SRSRAN_SSS_NR_LEN - m1], m1);
+
+  return SRSRAN_SUCCESS;
+}
+
+int srsran_sss_nr_find(const cf_t ssb_grid[SRSRAN_SSB_NOF_RE],
+                       uint32_t   N_id_2,
+                       float*     norm_corr,
+                       uint32_t*  found_N_id_1)
+{
+  // Verify inputs
+  if (ssb_grid == NULL || N_id_2 >= SRSRAN_NOF_NID_2_NR) {
+    return SRSRAN_ERROR_INVALID_INPUTS;
+  }
+
+  // Extract SSS ptr
+  const cf_t* sss_ptr = &ssb_grid[SRSRAN_SSS_NR_SYMBOL_IDX * SRSRAN_SSB_BW_SUBC + SSS_NR_SUBC_BEGIN];
+
+  // Measure SSS average power
+  float avg_power = srsran_vec_avg_power_cf(sss_ptr, SRSRAN_SSS_NR_LEN);
+
+  // If the measured power is invalid or zero, consider no SSS signal
+  if (!isnormal(avg_power)) {
+    if (norm_corr) {
+      *norm_corr = 0.0f;
+    }
+    return SRSRAN_SUCCESS;
+  }
+
+  // Search state
+  float    max_corr = -INFINITY; //< Stores best correlation
+  uint32_t N_id_1   = 0;         //< Best N_id_1
+
+  // Iterate over all M1 shifts
+  for (uint32_t m1 = 0; m1 < SSS_NR_NOF_M1; m1++) {
+    // Temporal storage of SSS after applying d1 sequence
+    cf_t sss_seq_m1[SRSRAN_SSS_NR_LEN];
+
+    // Apply d1 sequence fist part
+    srsran_vec_prod_ccc(&sss_ptr[0], &sss_nr_d1[m1], &sss_seq_m1[0], SRSRAN_SSS_NR_LEN - m1);
+
+    // Apply d1 sequence second part
+    srsran_vec_prod_ccc(&sss_ptr[SRSRAN_SSS_NR_LEN - m1], &sss_nr_d1[0], &sss_seq_m1[SRSRAN_SSS_NR_LEN - m1], m1);
+
+    // Iterate over all N_id_1 with the given m1 sequence
+    for (uint32_t N_id_1_blind = m1; N_id_1_blind < SRSRAN_NOF_NID_1; N_id_1_blind += SSS_NR_NOF_M1) {
+      uint32_t m0 = SSS_NR_SEQUENCE_M0(N_id_1_blind, N_id_2);
+
+      cf_t sss_seq_m0[SRSRAN_SSS_NR_LEN];
+
+      // Apply d0 sequence fist part
+      srsran_vec_prod_ccc(&sss_seq_m1[0], &sss_nr_d0[m0], &sss_seq_m0[0], SRSRAN_SSS_NR_LEN - m0);
+
+      // Apply d0 sequence second part
+      srsran_vec_prod_ccc(&sss_seq_m1[SRSRAN_SSS_NR_LEN - m0], &sss_nr_d0[0], &sss_seq_m0[SRSRAN_SSS_NR_LEN - m0], m0);
+
+      // Correlate
+      float corr = SRSRAN_CSQABS(srsran_vec_acc_cc(sss_seq_m0, SRSRAN_SSS_NR_LEN)) / avg_power;
+      if (corr > max_corr) {
+        N_id_1   = N_id_1_blind;
+        max_corr = corr;
+      }
+    }
+  }
+
+  if (norm_corr) {
+    *norm_corr = max_corr;
+  }
+
+  if (found_N_id_1) {
+    *found_N_id_1 = N_id_1;
+  }
+
+  return SRSRAN_SUCCESS;
+}
diff --git a/lib/src/phy/sync/test/CMakeLists.txt b/lib/src/phy/sync/test/CMakeLists.txt
index 7ef124c70..aeef97fd3 100644
--- a/lib/src/phy/sync/test/CMakeLists.txt
+++ b/lib/src/phy/sync/test/CMakeLists.txt
@@ -133,3 +133,13 @@ target_link_libraries(cfo_test srsran_phy)
 
 add_test(cfo_test_1 cfo_test -f 0.12345 -n 1000)
 add_test(cfo_test_2 cfo_test -f 0.99849 -n 1000)
+
+
+########################################################################
+# NE TEST
+########################################################################
+
+add_executable(ssb_measure_test ssb_measure_test.c)
+target_link_libraries(ssb_measure_test srsran_phy)
+
+add_test(ssb_measure_test ssb_measure_test)
diff --git a/lib/src/phy/sync/test/ssb_measure_test.c b/lib/src/phy/sync/test/ssb_measure_test.c
new file mode 100644
index 000000000..46530d739
--- /dev/null
+++ b/lib/src/phy/sync/test/ssb_measure_test.c
@@ -0,0 +1,183 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2021 Software Radio Systems Limited
+ *
+ * By using this file, you agree to the terms and conditions set
+ * forth in the LICENSE file which can be found at the top level of
+ * the distribution.
+ *
+ */
+
+#include "srsran/common/test_common.h"
+#include "srsran/phy/channel/ch_awgn.h"
+#include "srsran/phy/sync/ssb.h"
+#include "srsran/phy/utils/debug.h"
+#include "srsran/phy/utils/vector.h"
+#include <getopt.h>
+#include <stdlib.h>
+
+// NR parameters
+static uint32_t                    carrier_nof_prb = 52;
+static srsran_subcarrier_spacing_t carrier_scs     = srsran_subcarrier_spacing_15kHz;
+static srsran_subcarrier_spacing_t ssb_scs         = srsran_subcarrier_spacing_30kHz;
+
+// Channel parameters
+static int32_t delay_n = 1;
+static float   cfo_hz  = 100.0f;
+static float   n0_dB   = -30.0f;
+
+// Test context
+static srsran_channel_awgn_t awgn     = {};
+static double                srate_hz = 0.0f; // Base-band sampling rate
+static float                 delay_us = 0.0f; // Base-band sampling rate
+static uint32_t              sf_len   = 0;    // Subframe length
+static cf_t*                 buffer   = NULL; // Base-band buffer
+
+#define RSRP_MAX_ERROR 1.0f
+#define EPRE_MAX_ERROR 1.0f
+#define N0_MAX_ERROR 2.0f
+#define SNR_MAX_ERROR 2.0f
+#define CFO_MAX_ERROR (cfo_hz * 0.3f)
+#define DELAY_MAX_ERROR (delay_us * 0.1f)
+
+static void usage(char* prog)
+{
+  printf("Usage: %s [v]\n", prog);
+  printf("\t-v [set srsran_verbose to debug, default none]\n");
+}
+
+static void parse_args(int argc, char** argv)
+{
+  int opt;
+  while ((opt = getopt(argc, argv, "v")) != -1) {
+    switch (opt) {
+      case 'v':
+        srsran_verbose++;
+        break;
+      default:
+        usage(argv[0]);
+        exit(-1);
+    }
+  }
+}
+
+static void run_channel()
+{
+  // Delay
+  for (uint32_t i = 0; i < sf_len; i++) {
+    buffer[i] = buffer[(i + delay_n) % sf_len];
+  }
+
+  // CFO
+  srsran_vec_apply_cfo(buffer, -cfo_hz / srate_hz, buffer, sf_len);
+
+  // AWGN
+  srsran_channel_awgn_run_c(&awgn, buffer, buffer, sf_len);
+}
+
+static int test_case_1(srsran_ssb_t* ssb)
+{
+  uint64_t         t_usec  = 0;
+  srsran_ssb_cfg_t ssb_cfg = {};
+  ssb_cfg.srate_hz         = srate_hz;
+  ssb_cfg.freq_offset_hz   = 0.0;
+  ssb_cfg.scs              = ssb_scs;
+
+  TESTASSERT(srsran_ssb_set_cfg(ssb, &ssb_cfg) == SRSRAN_SUCCESS);
+
+  // Build PBCH message
+  srsran_pbch_msg_nr_t pbch_msg = {};
+
+  for (uint32_t pci = 0; pci < SRSRAN_NOF_NID_NR; pci++) {
+    struct timeval t[3] = {};
+
+    // Initialise baseband
+    srsran_vec_cf_zero(buffer, sf_len);
+
+    // Add the SSB base-band
+    TESTASSERT(srsran_ssb_add(ssb, pci, &pbch_msg, buffer, buffer) == SRSRAN_SUCCESS);
+
+    // Run channel
+    run_channel();
+
+    // Measure
+    gettimeofday(&t[1], NULL);
+    srsran_csi_trs_measurements_t meas = {};
+    TESTASSERT(srsran_ssb_csi_measure(ssb, pci, buffer, &meas) == SRSRAN_SUCCESS);
+    gettimeofday(&t[2], NULL);
+    get_time_interval(t);
+    t_usec += t[0].tv_usec + t[0].tv_sec * 1000000UL;
+
+    // Print measurement
+    char str[512];
+    srsran_csi_meas_info(&meas, str, sizeof(str));
+    INFO("test_case_1 - pci=%d %s", pci, str);
+
+    // Assert measurements
+    TESTASSERT(fabsf(meas.rsrp_dB - 0.0f) < RSRP_MAX_ERROR);
+    TESTASSERT(fabsf(meas.epre_dB - 0.0f) < EPRE_MAX_ERROR);
+    TESTASSERT(fabsf(meas.n0_dB - n0_dB) < N0_MAX_ERROR);
+    TESTASSERT(fabsf(meas.snr_dB + n0_dB) < SNR_MAX_ERROR);
+    TESTASSERT(fabsf(meas.cfo_hz - cfo_hz) < CFO_MAX_ERROR);
+    TESTASSERT(fabsf(meas.delay_us + delay_us) < DELAY_MAX_ERROR);
+  }
+
+  INFO("test_case_1 - %.1f usec/measurement", (double)t_usec / (double)SRSRAN_NOF_NID_NR);
+
+  return SRSRAN_SUCCESS;
+}
+
+int main(int argc, char** argv)
+{
+  int ret = SRSRAN_ERROR;
+  parse_args(argc, argv);
+
+  srate_hz = (double)SRSRAN_SUBC_SPACING_NR(carrier_scs) * srsran_min_symbol_sz_rb(carrier_nof_prb);
+  delay_us = 1e6f * delay_n / (float)srate_hz;
+  sf_len   = (uint32_t)ceil(srate_hz / 1000.0);
+  buffer   = srsran_vec_cf_malloc(sf_len);
+
+  srsran_ssb_t      ssb      = {};
+  srsran_ssb_args_t ssb_args = {};
+  ssb_args.enable_encode     = true;
+  ssb_args.enable_measure    = true;
+
+  if (buffer == NULL) {
+    ERROR("Malloc");
+    goto clean_exit;
+  }
+
+  if (srsran_channel_awgn_init(&awgn, 0x0) < SRSRAN_SUCCESS) {
+    ERROR("AWGN");
+    goto clean_exit;
+  }
+
+  if (srsran_channel_awgn_set_n0(&awgn, n0_dB) < SRSRAN_SUCCESS) {
+    ERROR("AWGN");
+    goto clean_exit;
+  }
+
+  if (srsran_ssb_init(&ssb, &ssb_args) < SRSRAN_SUCCESS) {
+    ERROR("Init");
+    goto clean_exit;
+  }
+
+  if (test_case_1(&ssb) != SRSRAN_SUCCESS) {
+    ERROR("test case failed");
+  }
+
+  ret = SRSRAN_SUCCESS;
+
+clean_exit:
+  srsran_ssb_free(&ssb);
+
+  srsran_channel_awgn_free(&awgn);
+
+  if (buffer) {
+    free(buffer);
+  }
+
+  return ret;
+}
\ No newline at end of file
diff --git a/lib/test/common/multiqueue_test.cc b/lib/test/common/multiqueue_test.cc
index 95ad8fae0..d2564c26b 100644
--- a/lib/test/common/multiqueue_test.cc
+++ b/lib/test/common/multiqueue_test.cc
@@ -21,20 +21,14 @@
 
 #include "srsran/adt/move_callback.h"
 #include "srsran/common/multiqueue.h"
+#include "srsran/common/test_common.h"
 #include "srsran/common/thread_pool.h"
 #include <iostream>
 #include <map>
+#include <random>
 #include <thread>
 #include <unistd.h>
 
-#define TESTASSERT(cond)                                                                                               \
-  {                                                                                                                    \
-    if (!(cond)) {                                                                                                     \
-      std::cout << "[" << __FUNCTION__ << "][Line " << __LINE__ << "]: FAIL at " << (#cond) << std::endl;              \
-      return -1;                                                                                                       \
-    }                                                                                                                  \
-  }
-
 using namespace srsran;
 
 int test_multiqueue()
@@ -44,79 +38,80 @@ int test_multiqueue()
   int number = 2;
 
   multiqueue_handler<int> multiqueue;
-  TESTASSERT(multiqueue.nof_queues() == 0)
+  TESTASSERT(multiqueue.nof_queues() == 0);
 
   // test push/pop and size for one queue
-  int qid1 = multiqueue.add_queue();
-  TESTASSERT(qid1 == 0 and multiqueue.is_queue_active(qid1))
-  TESTASSERT(multiqueue.size(qid1) == 0 and multiqueue.empty(qid1))
-  TESTASSERT(multiqueue.nof_queues() == 1)
-  TESTASSERT(multiqueue.try_push(qid1, 5).first)
-  TESTASSERT(multiqueue.try_push(qid1, number))
-  TESTASSERT(multiqueue.size(qid1) == 2 and not multiqueue.empty(qid1))
-  TESTASSERT(multiqueue.wait_pop(&number) == qid1)
-  TESTASSERT(number == 5)
-  TESTASSERT(multiqueue.wait_pop(&number) == qid1)
-  TESTASSERT(number == 2 and multiqueue.empty(qid1) and multiqueue.size(qid1) == 0)
+  queue_handle<int> qid1 = multiqueue.add_queue();
+  TESTASSERT(qid1.active());
+  TESTASSERT(qid1.size() == 0 and qid1.empty());
+  TESTASSERT(multiqueue.nof_queues() == 1);
+  TESTASSERT(qid1.try_push(5).has_value());
+  TESTASSERT(qid1.try_push(number));
+  TESTASSERT(qid1.size() == 2 and not qid1.empty());
+  TESTASSERT(multiqueue.wait_pop(&number));
+  TESTASSERT(number == 5);
+  TESTASSERT(multiqueue.wait_pop(&number));
+  TESTASSERT(number == 2 and qid1.empty());
 
   // test push/pop and size for two queues
-  int qid2 = multiqueue.add_queue();
-  TESTASSERT(qid2 == 1)
-  TESTASSERT(multiqueue.nof_queues() == 2 and multiqueue.is_queue_active(qid1))
-  TESTASSERT(multiqueue.try_push(qid2, 3).first)
-  TESTASSERT(multiqueue.size(qid2) == 1 and not multiqueue.empty(qid2))
-  TESTASSERT(multiqueue.empty(qid1) and multiqueue.size(qid1) == 0)
+  queue_handle<int> qid2 = multiqueue.add_queue();
+  TESTASSERT(qid2.active());
+  TESTASSERT(multiqueue.nof_queues() == 2 and qid1.active());
+  TESTASSERT(qid2.try_push(3).has_value());
+  TESTASSERT(qid2.size() == 1 and not qid2.empty());
+  TESTASSERT(qid1.empty());
 
   // check if erasing a queue breaks anything
-  multiqueue.erase_queue(qid1);
-  TESTASSERT(multiqueue.nof_queues() == 1 and not multiqueue.is_queue_active(qid1))
+  qid1.reset();
+  TESTASSERT(multiqueue.nof_queues() == 1 and not qid1.active());
   qid1 = multiqueue.add_queue();
-  TESTASSERT(qid1 == 0)
-  TESTASSERT(multiqueue.empty(qid1) and multiqueue.is_queue_active(qid1))
+  TESTASSERT(qid1.empty() and qid1.active());
+  TESTASSERT(qid2.size() == 1 and not qid2.empty());
   multiqueue.wait_pop(&number);
 
   // check round-robin
   for (int i = 0; i < 10; ++i) {
-    TESTASSERT(multiqueue.try_push(qid1, i))
+    TESTASSERT(qid1.try_push(i));
   }
   for (int i = 20; i < 35; ++i) {
-    TESTASSERT(multiqueue.try_push(qid2, i))
+    TESTASSERT(qid2.try_push(i));
   }
-  TESTASSERT(multiqueue.size(qid1) == 10)
-  TESTASSERT(multiqueue.size(qid2) == 15)
-  TESTASSERT(multiqueue.wait_pop(&number) == qid1 and number == 0)
-  TESTASSERT(multiqueue.wait_pop(&number) == qid2 and number == 20)
-  TESTASSERT(multiqueue.wait_pop(&number) == qid1 and number == 1)
-  TESTASSERT(multiqueue.wait_pop(&number) == qid2 and number == 21)
-  TESTASSERT(multiqueue.size(qid1) == 8)
-  TESTASSERT(multiqueue.size(qid2) == 13)
+  TESTASSERT(qid1.size() == 10);
+  TESTASSERT(qid2.size() == 15);
+  TESTASSERT(multiqueue.wait_pop(&number) and number == 0);
+  TESTASSERT(multiqueue.wait_pop(&number) and number == 20);
+  TESTASSERT(multiqueue.wait_pop(&number) and number == 1);
+  TESTASSERT(multiqueue.wait_pop(&number) and number == 21);
+  TESTASSERT(qid1.size() == 8);
+  TESTASSERT(qid2.size() == 13);
   for (int i = 0; i < 8 * 2; ++i) {
     multiqueue.wait_pop(&number);
   }
-  TESTASSERT(multiqueue.size(qid1) == 0)
-  TESTASSERT(multiqueue.size(qid2) == 5)
-  TESTASSERT(multiqueue.wait_pop(&number) == qid2 and number == 30)
+  TESTASSERT(qid1.size() == 0);
+  TESTASSERT(qid2.size() == 5);
+  TESTASSERT(multiqueue.wait_pop(&number) and number == 30);
 
   // remove existing queues
-  multiqueue.erase_queue(qid1);
-  multiqueue.erase_queue(qid2);
-  TESTASSERT(multiqueue.nof_queues() == 0)
+  qid1.reset();
+  qid2.reset();
+  TESTASSERT(multiqueue.nof_queues() == 0);
 
   // check that adding a queue of different capacity works
   {
-    int qid1 = multiqueue.add_queue();
-    int qid2 = multiqueue.add_queue();
+    qid1 = multiqueue.add_queue();
+    qid2 = multiqueue.add_queue();
 
     // remove first queue again
-    multiqueue.erase_queue(qid1);
-    TESTASSERT(multiqueue.nof_queues() == 1)
+    qid1.reset();
+    TESTASSERT(multiqueue.nof_queues() == 1);
 
     // add queue with non-default capacity
-    int qid3 = multiqueue.add_queue(10);
+    auto qid3 = multiqueue.add_queue(10);
+    TESTASSERT(qid3.capacity() == 10);
 
     // make sure neither a new queue index is returned
-    TESTASSERT(qid1 != qid3)
-    TESTASSERT(qid2 != qid3)
+    TESTASSERT(qid1 != qid3);
+    TESTASSERT(qid2 != qid3);
   }
 
   std::cout << "outcome: Success\n";
@@ -131,10 +126,10 @@ int test_multiqueue_threading()
 
   int                     capacity = 4, number = 0, start_number = 2, nof_pushes = capacity + 1;
   multiqueue_handler<int> multiqueue(capacity);
-  int                     qid1 = multiqueue.add_queue();
-  auto push_blocking_func      = [&multiqueue](int qid, int start_value, int nof_pushes, bool* is_running) {
+  auto                    qid1 = multiqueue.add_queue();
+  auto push_blocking_func      = [](queue_handle<int>* qid, int start_value, int nof_pushes, bool* is_running) {
     for (int i = 0; i < nof_pushes; ++i) {
-      multiqueue.push(qid, start_value + i);
+      qid->push(start_value + i);
       std::cout << "t1: pushed item " << i << std::endl;
     }
     std::cout << "t1: pushed all items\n";
@@ -142,17 +137,17 @@ int test_multiqueue_threading()
   };
 
   bool        t1_running = true;
-  std::thread t1(push_blocking_func, qid1, start_number, nof_pushes, &t1_running);
+  std::thread t1(push_blocking_func, &qid1, start_number, nof_pushes, &t1_running);
 
   // Wait for queue to fill
-  while ((int)multiqueue.size(qid1) != capacity) {
+  while ((int)qid1.size() != capacity) {
     usleep(1000);
-    TESTASSERT(t1_running)
+    TESTASSERT(t1_running);
   }
 
   for (int i = 0; i < nof_pushes; ++i) {
-    TESTASSERT(multiqueue.wait_pop(&number) == qid1)
-    TESTASSERT(number == start_number + i)
+    TESTASSERT(multiqueue.wait_pop(&number));
+    TESTASSERT(number == start_number + i);
     std::cout << "main: popped item " << i << "\n";
   }
   std::cout << "main: popped all items\n";
@@ -161,9 +156,9 @@ int test_multiqueue_threading()
   while (t1_running) {
     usleep(1000);
   }
-  TESTASSERT(multiqueue.size(qid1) == 0)
+  TESTASSERT(qid1.size() == 0);
 
-  multiqueue.reset();
+  multiqueue.stop();
   t1.join();
 
   std::cout << "outcome: Success\n";
@@ -179,25 +174,25 @@ int test_multiqueue_threading2()
 
   int                     capacity = 4, start_number = 2, nof_pushes = capacity + 1;
   multiqueue_handler<int> multiqueue(capacity);
-  int                     qid1 = multiqueue.add_queue();
-  auto push_blocking_func      = [&multiqueue](int qid, int start_value, int nof_pushes, bool* is_running) {
+  auto                    qid1 = multiqueue.add_queue();
+  auto push_blocking_func      = [](queue_handle<int>* qid, int start_value, int nof_pushes, bool* is_running) {
     for (int i = 0; i < nof_pushes; ++i) {
-      multiqueue.push(qid, start_value + i);
+      qid->push(start_value + i);
     }
     std::cout << "t1: pushed all items\n";
     *is_running = false;
   };
 
   bool        t1_running = true;
-  std::thread t1(push_blocking_func, qid1, start_number, nof_pushes, &t1_running);
+  std::thread t1(push_blocking_func, &qid1, start_number, nof_pushes, &t1_running);
 
   // Wait for queue to fill
-  while ((int)multiqueue.size(qid1) != capacity) {
+  while ((int)qid1.size() != capacity) {
     usleep(1000);
-    TESTASSERT(t1_running)
+    TESTASSERT(t1_running);
   }
 
-  multiqueue.reset();
+  multiqueue.stop();
   t1.join();
 
   std::cout << "outcome: Success\n";
@@ -213,23 +208,95 @@ int test_multiqueue_threading3()
 
   int                     capacity = 4;
   multiqueue_handler<int> multiqueue(capacity);
-  int                     qid1              = multiqueue.add_queue();
-  auto                    pop_blocking_func = [&multiqueue](int qid, bool* success) {
-    int number = 0;
-    int id     = multiqueue.wait_pop(&number);
-    *success   = id < 0;
+  auto                    qid1              = multiqueue.add_queue();
+  auto                    pop_blocking_func = [&multiqueue](bool* success) {
+    int  number = 0;
+    bool ret    = multiqueue.wait_pop(&number);
+    *success    = not ret;
   };
 
   bool        t1_success = false;
-  std::thread t1(pop_blocking_func, qid1, &t1_success);
+  std::thread t1(pop_blocking_func, &t1_success);
 
-  TESTASSERT(not t1_success)
+  TESTASSERT(not t1_success);
   usleep(1000);
-  TESTASSERT(not t1_success)
-  TESTASSERT((int)multiqueue.size(qid1) == 0)
+  TESTASSERT(not t1_success);
+  TESTASSERT((int)qid1.size() == 0);
 
   // Should be able to unlock all
-  multiqueue.reset();
+  multiqueue.stop();
+  TESTASSERT(multiqueue.nof_queues() == 0);
+  TESTASSERT(not qid1.active());
+  t1.join();
+  TESTASSERT(t1_success);
+
+  std::cout << "outcome: Success\n";
+  std::cout << "===================================================\n";
+
+  return 0;
+}
+
+int test_multiqueue_threading4()
+{
+  std::cout << "\n===== TEST multiqueue threading test 4: start =====\n";
+  // Description: the consumer will block on popping, but the pushing from different producers
+  //              should be sufficient to awake it when necessary
+
+  int                     capacity = 4;
+  multiqueue_handler<int> multiqueue(capacity);
+  auto                    qid1 = multiqueue.add_queue();
+  auto                    qid2 = multiqueue.add_queue();
+  auto                    qid3 = multiqueue.add_queue();
+  auto                    qid4 = multiqueue.add_queue();
+  std::mutex              mutex;
+  int                     last_number       = -1;
+  auto                    pop_blocking_func = [&multiqueue, &last_number, &mutex](bool* success) {
+    int number = 0;
+    while (multiqueue.wait_pop(&number)) {
+      std::lock_guard<std::mutex> lock(mutex);
+      last_number = std::max(last_number, number);
+    }
+    *success = true;
+  };
+
+  bool        t1_success = false;
+  std::thread t1(pop_blocking_func, &t1_success);
+
+  std::random_device              rd;
+  std::mt19937                    gen(rd());
+  std::uniform_int_distribution<> dist{0, 2};
+  for (int i = 0; i < 10000; ++i) {
+    int qidx = dist(gen);
+    switch (qidx) {
+      case 0:
+        qid1.push(i);
+        break;
+      case 1:
+        qid2.push(i);
+        break;
+      case 2:
+        qid4.push(i);
+        break;
+      default:
+        break;
+    }
+    if (i % 20 == 0) {
+      int                          count = 0;
+      std::unique_lock<std::mutex> lock(mutex);
+      while (last_number != i) {
+        lock.unlock();
+        usleep(100);
+        count++;
+        TESTASSERT(count < 100000);
+        lock.lock();
+      }
+    }
+  }
+
+  // Should be able to unlock all
+  multiqueue.stop();
+  TESTASSERT(multiqueue.nof_queues() == 0);
+  TESTASSERT(not qid1.active());
   t1.join();
   TESTASSERT(t1_success);
 
@@ -430,6 +497,7 @@ int main()
   TESTASSERT(test_multiqueue_threading() == 0);
   TESTASSERT(test_multiqueue_threading2() == 0);
   TESTASSERT(test_multiqueue_threading3() == 0);
+  TESTASSERT(test_multiqueue_threading4() == 0);
 
   TESTASSERT(test_task_thread_pool() == 0);
   TESTASSERT(test_task_thread_pool2() == 0);
diff --git a/srsenb/hdr/stack/enb_stack_lte.h b/srsenb/hdr/stack/enb_stack_lte.h
index f7bd635e3..25ea20f36 100644
--- a/srsenb/hdr/stack/enb_stack_lte.h
+++ b/srsenb/hdr/stack/enb_stack_lte.h
@@ -130,7 +130,7 @@ private:
 
   // task handling
   srsran::task_scheduler    task_sched;
-  srsran::task_queue_handle enb_task_queue, gtpu_task_queue, mme_task_queue, sync_task_queue;
+  srsran::task_queue_handle enb_task_queue, sync_task_queue;
 
   srsenb::mac  mac;
   srsenb::rlc  rlc;
diff --git a/srsenb/hdr/stack/rrc/rrc.h b/srsenb/hdr/stack/rrc/rrc.h
index 9bf78ffb7..47aadb295 100644
--- a/srsenb/hdr/stack/rrc/rrc.h
+++ b/srsenb/hdr/stack/rrc/rrc.h
@@ -203,7 +203,7 @@ private:
   const static uint32_t LCID_REM_USER   = 0xffff0001;
   const static uint32_t LCID_REL_USER   = 0xffff0002;
   const static uint32_t LCID_ACT_USER   = 0xffff0004;
-  const static uint32_t LCID_RTX_USER   = 0xffff0005;
+  const static uint32_t LCID_RLC_RTX    = 0xffff0005;
   const static uint32_t LCID_RADLINK_DL = 0xffff0006;
   const static uint32_t LCID_RADLINK_UL = 0xffff0007;
 
diff --git a/srsenb/hdr/stack/rrc/rrc_paging.h b/srsenb/hdr/stack/rrc/rrc_paging.h
index 19a0e0228..c4ebe7cf8 100644
--- a/srsenb/hdr/stack/rrc/rrc_paging.h
+++ b/srsenb/hdr/stack/rrc/rrc_paging.h
@@ -85,13 +85,21 @@ private:
   const static size_t nof_paging_subframes = 4;
 
   bool       add_paging_record(uint32_t ueid, const asn1::rrc::paging_record_s& paging_record);
-  pcch_info& get_pcch_info(tti_point tti_tx_dl)
+  pcch_info* get_pcch_info(tti_point tti_tx_dl)
   {
-    return pending_paging[tti_tx_dl.sfn() % T][get_sf_idx_key(tti_tx_dl.sf_idx())];
+    int sf_key = get_sf_idx_key(tti_tx_dl.sf_idx());
+    if (sf_key < 0) {
+      return nullptr;
+    }
+    return &pending_paging[tti_tx_dl.sfn() % T][sf_key];
   }
-  const pcch_info& get_pcch_info(tti_point tti_tx_dl) const
+  const pcch_info* get_pcch_info(tti_point tti_tx_dl) const
   {
-    return pending_paging[tti_tx_dl.sfn() % T][get_sf_idx_key(tti_tx_dl.sf_idx())];
+    int sf_key = get_sf_idx_key(tti_tx_dl.sf_idx());
+    if (sf_key < 0) {
+      return nullptr;
+    }
+    return &pending_paging[tti_tx_dl.sfn() % T][sf_key];
   }
   static int get_sf_idx_key(uint32_t sf_idx)
   {
@@ -156,12 +164,13 @@ bool paging_manager::add_paging_record(uint32_t ueid, const asn1::rrc::paging_re
     logger.error("SF pattern is N/A for Ns=%d, i_s=%d, imsi_decimal=%d", Ns, i_s, ueid);
     return false;
   }
+  size_t sf_key = static_cast<size_t>(get_sf_idx_key(sf_idx));
 
   size_t     sfn_cycle_idx = (T / N) * (ueid % N);
-  pcch_info& pending_pcch  = pending_paging[sfn_cycle_idx][get_sf_idx_key(sf_idx)];
+  pcch_info& pending_pcch  = pending_paging[sfn_cycle_idx][sf_key];
   auto&      record_list   = pending_pcch.pcch_msg.msg.c1().paging().paging_record_list;
 
-  std::lock_guard<std::mutex> lock(sf_idx_mutex[get_sf_idx_key(sf_idx)]);
+  std::lock_guard<std::mutex> lock(sf_idx_mutex[sf_key]);
 
   if (record_list.size() >= ASN1_RRC_MAX_PAGE_REC) {
     logger.warning("Failed to add new paging record for ueid=%d. Cause: no paging record space left.", ueid);
@@ -197,19 +206,19 @@ size_t paging_manager::pending_pcch_bytes(tti_point tti_tx_dl)
     return 0;
   }
 
-  std::lock_guard<std::mutex> lock(sf_idx_mutex[sf_key]);
+  std::lock_guard<std::mutex> lock(sf_idx_mutex[static_cast<size_t>(sf_key)]);
 
   // clear old PCCH that has been transmitted at this point
-  pcch_info& old_pcch = get_pcch_info(tti_tx_dl - SRSRAN_NOF_SF_X_FRAME);
-  if (not old_pcch.empty()) {
-    old_pcch.clear();
+  pcch_info* old_pcch = get_pcch_info(tti_tx_dl - SRSRAN_NOF_SF_X_FRAME);
+  if (old_pcch != nullptr and not old_pcch->empty()) {
+    old_pcch->clear();
   }
 
-  const pcch_info& pending_pcch = get_pcch_info(tti_tx_dl);
-  if (pending_pcch.empty()) {
+  const pcch_info* pending_pcch = get_pcch_info(tti_tx_dl);
+  if (pending_pcch->empty()) {
     return 0;
   }
-  return pending_pcch.pdu->size();
+  return pending_pcch->pdu->size();
 }
 
 template <typename Callable>
@@ -221,18 +230,18 @@ bool paging_manager::read_pdu_pcch(tti_point tti_tx_dl, const Callable& func)
     return false;
   }
 
-  pcch_info& pending_pcch = get_pcch_info(tti_tx_dl);
+  std::lock_guard<std::mutex> lock(sf_idx_mutex[static_cast<size_t>(sf_key)]);
 
-  std::lock_guard<std::mutex> lock(sf_idx_mutex[get_sf_idx_key(tti_tx_dl.sf_idx())]);
+  pcch_info* pending_pcch = get_pcch_info(tti_tx_dl);
 
-  if (pending_pcch.empty()) {
+  if (pending_pcch->empty()) {
     logger.warning("read_pdu_pdcch(...) called for tti=%d, but there is no pending pcch message", tti_tx_dl.to_uint());
     return false;
   }
 
   // Call callable for existing PCCH pdu
-  if (func(*pending_pcch.pdu, pending_pcch.pcch_msg, pending_pcch.tti_tx_dl.is_valid())) {
-    pending_pcch.tti_tx_dl = tti_tx_dl;
+  if (func(*pending_pcch->pdu, pending_pcch->pcch_msg, pending_pcch->tti_tx_dl.is_valid())) {
+    pending_pcch->tti_tx_dl = tti_tx_dl;
     return true;
   }
   return false;
diff --git a/srsenb/hdr/stack/rrc/rrc_ue.h b/srsenb/hdr/stack/rrc/rrc_ue.h
index 7b246c2bd..bb78d7702 100644
--- a/srsenb/hdr/stack/rrc/rrc_ue.h
+++ b/srsenb/hdr/stack/rrc/rrc_ue.h
@@ -50,13 +50,11 @@ public:
   std::string to_string(const activity_timeout_type_t& type);
   void        set_activity_timeout(const activity_timeout_type_t type);
   void        set_activity();
-  void        start_rlf_timer();
-  void        stop_rlf_timer();
   void        set_radiolink_dl_state(bool crc_res);
   void        set_radiolink_ul_state(bool crc_res);
   void        activity_timer_expired(const activity_timeout_type_t type);
-  void        rlf_timer_expired();
-  void        max_retx_reached();
+  void        rlf_timer_expired(uint32_t timeout_id);
+  void        max_rlc_retx_reached();
 
   rrc_state_t get_state();
   void        get_metrics(rrc_ue_metrics_t& ue_metrics) const;
@@ -122,7 +120,6 @@ public:
   bool has_erab(uint32_t erab_id) const { return bearer_list.get_erabs().count(erab_id) > 0; }
   int  get_erab_addr_in(uint16_t erab_id, transp_addr_t& addr_in, uint32_t& teid_in) const;
 
-  bool setup_erabs(const asn1::s1ap::erab_to_be_setup_list_ctxt_su_req_l& e);
   bool release_erabs();
   int  release_erab(uint32_t erab_id);
   int  setup_erab(uint16_t                                           erab_id,
@@ -167,9 +164,12 @@ public:
   bool is_csfb = false;
 
 private:
-  // args
-  srsran::unique_timer activity_timer;
-  srsran::unique_timer rlf_release_timer;
+  srsran::unique_timer activity_timer; // for basic DL/UL activity timeout
+
+  /// Radio link failure handling uses distinct timers for PHY (DL and UL) and RLC signaled RLF
+  srsran::unique_timer phy_dl_rlf_timer; // can be stopped through recovered DL activity
+  srsran::unique_timer phy_ul_rlf_timer; // can be stopped through recovered UL activity
+  srsran::unique_timer rlc_rlf_timer;    // can only be stoped through UE reestablishment
 
   /// cached ASN1 fields for RRC config update checking, and ease of context transfer during HO
   ue_var_cfg_t current_ue_cfg;
diff --git a/srsenb/hdr/stack/upper/ngap.h b/srsenb/hdr/stack/upper/ngap.h
index 41600b49b..eeb65471a 100644
--- a/srsenb/hdr/stack/upper/ngap.h
+++ b/srsenb/hdr/stack/upper/ngap.h
@@ -44,7 +44,7 @@ public:
   ngap(srsran::task_sched_handle   task_sched_,
        srslog::basic_logger&       logger,
        srsran::socket_manager_itf* rx_socket_handler);
-  int  init(ngap_args_t args_, rrc_interface_ngap_nr* rrc_);
+  int  init(const ngap_args_t& args_, rrc_interface_ngap_nr* rrc_);
   void stop();
 
   // RRC NR interface
diff --git a/srsenb/src/stack/enb_stack_lte.cc b/srsenb/src/stack/enb_stack_lte.cc
index 596193607..694764921 100644
--- a/srsenb/src/stack/enb_stack_lte.cc
+++ b/srsenb/src/stack/enb_stack_lte.cc
@@ -50,9 +50,7 @@ enb_stack_lte::enb_stack_lte(srslog::sink& log_sink) :
   pending_stack_metrics(64)
 {
   get_background_workers().set_nof_workers(2);
-  enb_task_queue  = task_sched.make_task_queue();
-  mme_task_queue  = task_sched.make_task_queue();
-  gtpu_task_queue = task_sched.make_task_queue();
+  enb_task_queue = task_sched.make_task_queue();
   // sync_queue is added in init()
 }
 
@@ -227,7 +225,7 @@ bool enb_stack_lte::get_metrics(stack_metrics_t* metrics)
     }
   });
 
-  if (ret.first) {
+  if (ret.has_value()) {
     // wait for result
     *metrics = pending_stack_metrics.pop_blocking();
     return true;
diff --git a/srsenb/src/stack/rrc/mac_controller.cc b/srsenb/src/stack/rrc/mac_controller.cc
index ea3f73fec..e4ed7d066 100644
--- a/srsenb/src/stack/rrc/mac_controller.cc
+++ b/srsenb/src/stack/rrc/mac_controller.cc
@@ -308,8 +308,10 @@ void mac_controller::handle_ho_prep(const asn1::rrc::ho_prep_info_r8_ies_s& ho_p
 
 void mac_controller::handle_max_retx()
 {
-  set_drb_activation(false);
-  update_mac(other);
+  for (auto& ue_bearer : current_sched_ue_cfg.ue_bearers) {
+    ue_bearer.direction = sched_interface::ue_bearer_cfg_t::IDLE;
+  }
+  update_mac(config_tx);
 }
 
 void mac_controller::set_scell_activation(const std::bitset<SRSRAN_MAX_CARRIERS>& scell_mask)
diff --git a/srsenb/src/stack/rrc/rrc.cc b/srsenb/src/stack/rrc/rrc.cc
index e5044d138..af8775d8a 100644
--- a/srsenb/src/stack/rrc/rrc.cc
+++ b/srsenb/src/stack/rrc/rrc.cc
@@ -183,7 +183,7 @@ uint32_t rrc::get_nof_users()
 
 void rrc::max_retx_attempted(uint16_t rnti)
 {
-  rrc_pdu p = {rnti, LCID_RTX_USER, false, nullptr};
+  rrc_pdu p = {rnti, LCID_RLC_RTX, false, nullptr};
   if (not rx_pdu_queue.try_push(std::move(p))) {
     logger.error("Failed to push max Retx event to RRC queue");
   }
@@ -952,8 +952,8 @@ void rrc::tti_clock()
       case LCID_RADLINK_UL:
         user_it->second->set_radiolink_ul_state(p.arg);
         break;
-      case LCID_RTX_USER:
-        user_it->second->max_retx_reached();
+      case LCID_RLC_RTX:
+        user_it->second->max_rlc_retx_reached();
         break;
       case LCID_EXIT:
         logger.info("Exiting thread");
diff --git a/srsenb/src/stack/rrc/rrc_ue.cc b/srsenb/src/stack/rrc/rrc_ue.cc
index 275621ed3..a9152d7a5 100644
--- a/srsenb/src/stack/rrc/rrc_ue.cc
+++ b/srsenb/src/stack/rrc/rrc_ue.cc
@@ -65,13 +65,18 @@ int rrc::ue::init()
   // Configure
   apply_setup_phy_common(parent->cfg.sibs[1].sib2().rr_cfg_common, true);
 
-  rlf_release_timer = parent->task_sched.get_unique_timer();
-  activity_timer    = parent->task_sched.get_unique_timer();
+  phy_dl_rlf_timer = parent->task_sched.get_unique_timer();
+  phy_ul_rlf_timer = parent->task_sched.get_unique_timer();
+  rlc_rlf_timer    = parent->task_sched.get_unique_timer();
+  activity_timer   = parent->task_sched.get_unique_timer();
   set_activity_timeout(MSG3_RX_TIMEOUT); // next UE response is Msg3
 
   // Set timeout to release UE context after RLF detection
-  uint32_t deadline_ms = parent->cfg.rlf_release_timer_ms;
-  rlf_release_timer.set(deadline_ms, [this](uint32_t tid) { rlf_timer_expired(); });
+  uint32_t deadline_ms       = parent->cfg.rlf_release_timer_ms;
+  auto     timer_expire_func = [this](uint32_t tid) { rlf_timer_expired(tid); };
+  phy_dl_rlf_timer.set(deadline_ms, timer_expire_func);
+  phy_ul_rlf_timer.set(deadline_ms, timer_expire_func);
+  rlc_rlf_timer.set(deadline_ms, timer_expire_func);
   parent->logger.info("Setting RLF timer for rnti=0x%x to %dms", rnti, deadline_ms);
 
   mobility_handler = make_rnti_obj<rrc_mobility>(rnti, this);
@@ -106,44 +111,34 @@ void rrc::ue::set_activity()
   }
 }
 
-void rrc::ue::start_rlf_timer()
-{
-  rlf_release_timer.run();
-  parent->logger.info("RLF timer started for rnti=0x%x (duration=%dms)", rnti, rlf_release_timer.duration());
-}
-
-void rrc::ue::stop_rlf_timer()
-{
-  if (rlf_release_timer.is_running()) {
-    parent->logger.info("RLF timer stopped for rnti=0x%x (time elapsed=%dms)", rnti, rlf_release_timer.time_elapsed());
-  }
-  rlf_release_timer.stop();
-}
-
 void rrc::ue::set_radiolink_dl_state(bool crc_res)
 {
   parent->logger.debug(
       "Radio-Link downlink state for rnti=0x%x: crc_res=%d, consecutive_ko=%d", rnti, crc_res, consecutive_kos_dl);
 
-  // If received OK, restart counter and stop RLF timer
+  // If received OK, restart DL counter and stop RLF timer
   if (crc_res) {
     consecutive_kos_dl = 0;
-    consecutive_kos_ul = 0;
-    stop_rlf_timer();
+    if (phy_dl_rlf_timer.is_running()) {
+      parent->logger.info(
+          "DL RLF timer stopped for rnti=0x%x (time elapsed=%dms)", rnti, phy_dl_rlf_timer.time_elapsed());
+      phy_dl_rlf_timer.stop();
+    }
     return;
   }
 
   // Count KOs in MAC and trigger release if it goes above a certain value.
   // This is done to detect out-of-coverage UEs
-  if (rlf_release_timer.is_running()) {
+  if (phy_dl_rlf_timer.is_running()) {
     // RLF timer already running, no need to count KOs
     return;
   }
 
   consecutive_kos_dl++;
   if (consecutive_kos_dl > parent->cfg.max_mac_dl_kos) {
-    parent->logger.info("Max KOs in DL reached, triggering release rnti=0x%x", rnti);
-    max_retx_reached();
+    parent->logger.info("Max KOs in DL reached, starting RLF timer rnti=0x%x", rnti);
+    mac_ctrl.handle_max_retx();
+    phy_dl_rlf_timer.run();
   }
 }
 
@@ -152,31 +147,34 @@ void rrc::ue::set_radiolink_ul_state(bool crc_res)
   parent->logger.debug(
       "Radio-Link uplink state for rnti=0x%x: crc_res=%d, consecutive_ko=%d", rnti, crc_res, consecutive_kos_ul);
 
-  // If received OK, restart counter and stop RLF timer
+  // If received OK, restart UL counter and stop RLF timer
   if (crc_res) {
-    consecutive_kos_dl = 0;
     consecutive_kos_ul = 0;
-    stop_rlf_timer();
+    if (phy_ul_rlf_timer.is_running()) {
+      parent->logger.info(
+          "UL RLF timer stopped for rnti=0x%x (time elapsed=%dms)", rnti, phy_ul_rlf_timer.time_elapsed());
+      phy_ul_rlf_timer.stop();
+    }
     return;
   }
 
   // Count KOs in MAC and trigger release if it goes above a certain value.
   // This is done to detect out-of-coverage UEs
-  if (rlf_release_timer.is_running()) {
+  if (phy_ul_rlf_timer.is_running()) {
     // RLF timer already running, no need to count KOs
     return;
   }
 
   consecutive_kos_ul++;
   if (consecutive_kos_ul > parent->cfg.max_mac_ul_kos) {
-    parent->logger.info("Max KOs in UL reached, triggering release rnti=0x%x", rnti);
-    max_retx_reached();
+    parent->logger.info("Max KOs in UL reached, starting RLF timer rnti=0x%x", rnti);
+    mac_ctrl.handle_max_retx();
+    phy_ul_rlf_timer.run();
   }
 }
 
 void rrc::ue::activity_timer_expired(const activity_timeout_type_t type)
 {
-  stop_rlf_timer();
   if (parent) {
     parent->logger.info("Activity timer for rnti=0x%x expired after %d ms", rnti, activity_timer.time_elapsed());
 
@@ -205,11 +203,17 @@ void rrc::ue::activity_timer_expired(const activity_timeout_type_t type)
   state = RRC_STATE_RELEASE_REQUEST;
 }
 
-void rrc::ue::rlf_timer_expired()
+void rrc::ue::rlf_timer_expired(uint32_t timeout_id)
 {
   activity_timer.stop();
   if (parent) {
-    parent->logger.info("RLF timer for rnti=0x%x expired after %d ms", rnti, rlf_release_timer.time_elapsed());
+    if (timeout_id == phy_dl_rlf_timer.id()) {
+      parent->logger.info("DL RLF timer for rnti=0x%x expired after %d ms", rnti, phy_dl_rlf_timer.time_elapsed());
+    } else if (timeout_id == phy_ul_rlf_timer.id()) {
+      parent->logger.info("UL RLF timer for rnti=0x%x expired after %d ms", rnti, phy_ul_rlf_timer.time_elapsed());
+    } else if (timeout_id == rlc_rlf_timer.id()) {
+      parent->logger.info("RLC RLF timer for rnti=0x%x expired after %d ms", rnti, rlc_rlf_timer.time_elapsed());
+    }
 
     if (parent->s1ap->user_exists(rnti)) {
       parent->s1ap->user_release(rnti, asn1::s1ap::cause_radio_network_opts::radio_conn_with_ue_lost);
@@ -224,14 +228,13 @@ void rrc::ue::rlf_timer_expired()
   state = RRC_STATE_RELEASE_REQUEST;
 }
 
-void rrc::ue::max_retx_reached()
+void rrc::ue::max_rlc_retx_reached()
 {
   if (parent) {
-    parent->logger.info("Max retx reached for rnti=0x%x", rnti);
-
-    // Give UE time to start re-establishment
-    start_rlf_timer();
+    parent->logger.info("Max RLC retx reached for rnti=0x%x", rnti);
 
+    // Turn off DRB scheduling but give UE chance to start re-establishment
+    rlc_rlf_timer.run();
     mac_ctrl.handle_max_retx();
   }
 }
@@ -397,10 +400,8 @@ void rrc::ue::handle_rrc_con_req(rrc_conn_request_s* msg)
     for (auto& user : parent->users) {
       if (user.first != rnti && user.second->has_tmsi && user.second->mmec == mmec && user.second->m_tmsi == m_tmsi) {
         parent->logger.info("RRC connection request: UE context already exists. M-TMSI=%d", m_tmsi);
-        if (parent->s1ap->user_release(rnti, asn1::s1ap::cause_radio_network_opts::radio_conn_with_ue_lost)) {
-          // Do not wait for MME response
-          parent->rem_user_thread(user.first);
-        }
+        user.second->state = RRC_STATE_IDLE; // Set old rnti to IDLE so that enb doesn't send RRC Connection Release
+        parent->s1ap->user_release(user.first, asn1::s1ap::cause_radio_network_opts::radio_conn_with_ue_lost);
         break;
       }
     }
@@ -528,9 +529,6 @@ void rrc::ue::handle_rrc_con_reest_req(rrc_conn_reest_request_s* msg)
   const rrc_conn_reest_request_r8_ies_s& req_r8   = msg->crit_exts.rrc_conn_reest_request_r8();
   uint16_t                               old_rnti = req_r8.ue_id.c_rnti.to_number();
 
-  srsran::console(
-      "User 0x%x requesting RRC Reestablishment as 0x%x. Cause: %s\n", rnti, old_rnti, req_r8.reest_cause.to_string());
-
   if (not parent->s1ap->is_mme_connected()) {
     parent->logger.error("MME isn't connected. Sending Connection Reject");
     send_connection_reest_rej(procedure_result_code::error_mme_not_connected);
@@ -542,69 +540,84 @@ void rrc::ue::handle_rrc_con_reest_req(rrc_conn_reest_request_s* msg)
                        msg->crit_exts.rrc_conn_reest_request_r8().ue_id.pci,
                        (uint32_t)msg->crit_exts.rrc_conn_reest_request_r8().ue_id.short_mac_i.to_number(),
                        msg->crit_exts.rrc_conn_reest_request_r8().reest_cause.to_string());
-  if (is_idle()) {
-    uint16_t               old_pci  = msg->crit_exts.rrc_conn_reest_request_r8().ue_id.pci;
-    const enb_cell_common* old_cell = parent->cell_common_list->get_pci(old_pci);
-    auto                   ue_it    = parent->users.find(old_rnti);
-    // Reject unrecognized rntis, and PCIs that do not belong to eNB
-    if (ue_it != parent->users.end() and old_cell != nullptr and
-        ue_it->second->ue_cell_list.get_enb_cc_idx(old_cell->enb_cc_idx) != nullptr) {
-      parent->logger.info("ConnectionReestablishmentRequest for rnti=0x%x. Sending Connection Reestablishment",
-                          old_rnti);
 
-      // Cancel Handover in Target eNB if on-going
-      asn1::s1ap::cause_c cause;
-      cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::interaction_with_other_proc;
-      parent->users.at(old_rnti)->mobility_handler->trigger(rrc_mobility::ho_cancel_ev{cause});
-
-      // Recover security setup
-      const enb_cell_common* pcell_cfg = get_ue_cc_cfg(UE_PCELL_CC_IDX);
-      ue_security_cfg                  = parent->users.at(old_rnti)->ue_security_cfg;
-      ue_security_cfg.regenerate_keys_handover(pcell_cfg->cell_cfg.pci, pcell_cfg->cell_cfg.dl_earfcn);
-
-      // send reestablishment and restore bearer configuration
-      send_connection_reest(parent->users.at(old_rnti)->ue_security_cfg.get_ncc());
-
-      // Get PDCP entity state (required when using RLC AM)
-      for (const auto& erab_pair : parent->users.at(old_rnti)->bearer_list.get_erabs()) {
-        uint16_t lcid              = erab_pair.second.id - 2;
-        old_reest_pdcp_state[lcid] = {};
-        parent->pdcp->get_bearer_state(old_rnti, lcid, &old_reest_pdcp_state[lcid]);
-
-        parent->logger.debug("Getting PDCP state for E-RAB with LCID %d", lcid);
-        parent->logger.debug("Got PDCP state: TX HFN %d, NEXT_PDCP_TX_SN %d, RX_HFN %d, NEXT_PDCP_RX_SN %d, "
-                             "LAST_SUBMITTED_PDCP_RX_SN %d",
-                             old_reest_pdcp_state[lcid].tx_hfn,
-                             old_reest_pdcp_state[lcid].next_pdcp_tx_sn,
-                             old_reest_pdcp_state[lcid].rx_hfn,
-                             old_reest_pdcp_state[lcid].next_pdcp_rx_sn,
-                             old_reest_pdcp_state[lcid].last_submitted_pdcp_rx_sn);
-      }
-
-      // Make sure UE capabilities are copied over to new RNTI
-      eutra_capabilities          = parent->users.at(old_rnti)->eutra_capabilities;
-      eutra_capabilities_unpacked = parent->users.at(old_rnti)->eutra_capabilities_unpacked;
-      ue_capabilities             = parent->users.at(old_rnti)->ue_capabilities;
-      if (parent->logger.debug.enabled()) {
-        asn1::json_writer js{};
-        eutra_capabilities.to_json(js);
-        parent->logger.debug("rnti=0x%x EUTRA capabilities: %s", rnti, js.to_string().c_str());
-      }
-
-      old_reest_rnti = old_rnti;
-      state          = RRC_STATE_WAIT_FOR_CON_REEST_COMPLETE;
-      set_activity_timeout(UE_INACTIVITY_TIMEOUT);
-    } else {
-      parent->logger.error("Received ConnectionReestablishment for rnti=0x%x without context", old_rnti);
-      send_connection_reest_rej(procedure_result_code::error_unknown_rnti);
-      srsran::console(
-          "User 0x%x RRC Reestablishment Request rejected. Cause: no rnti=0x%x context available\n", rnti, old_rnti);
-    }
-  } else {
+  if (not is_idle()) {
+    // The created RNTI has to receive ReestablishmentRequest as first message
     parent->logger.error("Received ReestablishmentRequest from an rnti=0x%x not in IDLE", rnti);
     send_connection_reest_rej(procedure_result_code::error_unknown_rnti);
     srsran::console("ERROR: User 0x%x requesting Reestablishment is not in RRC_IDLE\n", rnti);
+    return;
   }
+
+  uint16_t               old_pci   = msg->crit_exts.rrc_conn_reest_request_r8().ue_id.pci;
+  const enb_cell_common* old_cell  = parent->cell_common_list->get_pci(old_pci);
+  auto                   old_ue_it = parent->users.find(old_rnti);
+
+  // Reject unrecognized rntis, and PCIs that do not belong to eNB
+  if (old_ue_it == parent->users.end() or old_cell == nullptr or
+      old_ue_it->second->ue_cell_list.get_enb_cc_idx(old_cell->enb_cc_idx) == nullptr) {
+    parent->logger.error("Received ConnectionReestablishment for rnti=0x%x without context", old_rnti);
+    send_connection_reest_rej(procedure_result_code::error_unknown_rnti);
+    srsran::console(
+        "User 0x%x RRC Reestablishment Request rejected. Cause: no rnti=0x%x context available\n", rnti, old_rnti);
+    return;
+  }
+  ue* old_ue = old_ue_it->second.get();
+
+  // Reestablishment procedure going forward
+  parent->logger.info("ConnectionReestablishmentRequest for rnti=0x%x. Sending Connection Reestablishment", old_rnti);
+  srsran::console(
+      "User 0x%x requesting RRC Reestablishment as 0x%x. Cause: %s\n", rnti, old_rnti, req_r8.reest_cause.to_string());
+
+  // Cancel Handover in Target eNB if on-going
+  asn1::s1ap::cause_c cause;
+  cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::interaction_with_other_proc;
+  old_ue->mobility_handler->trigger(rrc_mobility::ho_cancel_ev{cause});
+
+  // Recover security setup
+  const enb_cell_common* pcell_cfg = get_ue_cc_cfg(UE_PCELL_CC_IDX);
+  ue_security_cfg                  = old_ue->ue_security_cfg;
+  ue_security_cfg.regenerate_keys_handover(pcell_cfg->cell_cfg.pci, pcell_cfg->cell_cfg.dl_earfcn);
+
+  // send RRC Reestablishment message and restore bearer configuration
+  send_connection_reest(old_ue->ue_security_cfg.get_ncc());
+
+  // Get PDCP entity state (required when using RLC AM)
+  for (const auto& erab_pair : old_ue->bearer_list.get_erabs()) {
+    uint16_t lcid              = erab_pair.second.id - 2;
+    old_reest_pdcp_state[lcid] = {};
+    parent->pdcp->get_bearer_state(old_rnti, lcid, &old_reest_pdcp_state[lcid]);
+
+    parent->logger.debug("Getting PDCP state for E-RAB with LCID %d", lcid);
+    parent->logger.debug("Got PDCP state: TX HFN %d, NEXT_PDCP_TX_SN %d, RX_HFN %d, NEXT_PDCP_RX_SN %d, "
+                         "LAST_SUBMITTED_PDCP_RX_SN %d",
+                         old_reest_pdcp_state[lcid].tx_hfn,
+                         old_reest_pdcp_state[lcid].next_pdcp_tx_sn,
+                         old_reest_pdcp_state[lcid].rx_hfn,
+                         old_reest_pdcp_state[lcid].next_pdcp_rx_sn,
+                         old_reest_pdcp_state[lcid].last_submitted_pdcp_rx_sn);
+  }
+
+  // Make sure UE capabilities are copied over to new RNTI
+  eutra_capabilities          = old_ue->eutra_capabilities;
+  eutra_capabilities_unpacked = old_ue->eutra_capabilities_unpacked;
+  ue_capabilities             = old_ue->ue_capabilities;
+  if (parent->logger.debug.enabled()) {
+    asn1::json_writer js{};
+    eutra_capabilities.to_json(js);
+    parent->logger.debug("rnti=0x%x EUTRA capabilities: %s", rnti, js.to_string().c_str());
+  }
+
+  // Stop RLF timers to avoid that old RNTI gets removed during RRC Reestablishment
+  parent->logger.info("Stopped RLF timers for old rnti=0x%x", old_rnti);
+  old_ue->rlc_rlf_timer.stop();
+  old_ue->phy_ul_rlf_timer.stop();
+  old_ue->phy_dl_rlf_timer.stop();
+  old_ue->mac_ctrl.set_drb_activation(false);
+
+  old_reest_rnti = old_rnti;
+  state          = RRC_STATE_WAIT_FOR_CON_REEST_COMPLETE;
+  set_activity_timeout(UE_INACTIVITY_TIMEOUT);
 }
 
 void rrc::ue::send_connection_reest(uint8_t ncc)
@@ -659,6 +672,14 @@ void rrc::ue::handle_rrc_con_reest_complete(rrc_conn_reest_complete_s* msg, srsr
 
   parent->logger.info("RRCConnectionReestablishComplete transaction ID: %d", msg->rrc_transaction_id);
 
+  auto old_ue_it = parent->users.find(old_reest_rnti);
+  if (old_ue_it == parent->users.end()) {
+    parent->logger.error("RRC Reestablishment old rnti=0x%x was erased during the procedure", old_reest_rnti);
+    parent->release_ue(rnti);
+    return;
+  }
+  auto* old_ue = old_ue_it->second.get();
+
   // TODO: msg->selected_plmn_id - used to select PLMN from SIB1 list
   // TODO: if(msg->registered_mme_present) - the indicated MME should be used from a pool
 
@@ -675,10 +696,10 @@ void rrc::ue::handle_rrc_con_reest_complete(rrc_conn_reest_complete_s* msg, srsr
   parent->pdcp->enable_encryption(rnti, srb_to_lcid(lte_srb::srb1));
 
   // Reestablish E-RABs of old rnti during ConnectionReconfiguration
-  bearer_list.reestablish_bearers(std::move(parent->users.at(old_reest_rnti)->bearer_list));
+  bearer_list.reestablish_bearers(std::move(old_ue->bearer_list));
 
   // remove old RNTI
-  parent->rem_user_thread(old_reest_rnti);
+  parent->rem_user(old_reest_rnti);
 
   state = RRC_STATE_REESTABLISHMENT_COMPLETE;
 
@@ -1029,35 +1050,6 @@ void rrc::ue::set_bitrates(const asn1::s1ap::ue_aggregate_maximum_bitrate_s& rat
   bitrates = rates;
 }
 
-bool rrc::ue::setup_erabs(const asn1::s1ap::erab_to_be_setup_list_ctxt_su_req_l& e)
-{
-  for (const auto& item : e) {
-    const auto& erab = item.value.erab_to_be_setup_item_ctxt_su_req();
-    if (erab.ext) {
-      parent->logger.warning("Not handling E-RABToBeSetupListCtxtSURequest extensions");
-    }
-    if (erab.ie_exts_present) {
-      parent->logger.warning("Not handling E-RABToBeSetupListCtxtSURequest extensions");
-    }
-    if (erab.transport_layer_address.length() > 32) {
-      parent->logger.error("IPv6 addresses not currently supported");
-      return false;
-    }
-
-    uint32_t teid_out = 0;
-    srsran::uint8_to_uint32(erab.gtp_teid.data(), &teid_out);
-    srsran::const_span<uint8_t> nas_pdu;
-    if (erab.nas_pdu_present) {
-      nas_pdu = erab.nas_pdu;
-    }
-    asn1::s1ap::cause_c cause;
-    bearer_list.add_erab(
-        erab.erab_id, erab.erab_level_qos_params, erab.transport_layer_address, teid_out, nas_pdu, cause);
-    bearer_list.add_gtpu_bearer(erab.erab_id);
-  }
-  return true;
-}
-
 bool rrc::ue::release_erabs()
 {
   bearer_list.release_erabs();
diff --git a/srsenb/src/stack/upper/gtpu.cc b/srsenb/src/stack/upper/gtpu.cc
index 1a81b6cec..13a1d45f4 100644
--- a/srsenb/src/stack/upper/gtpu.cc
+++ b/srsenb/src/stack/upper/gtpu.cc
@@ -130,9 +130,11 @@ const gtpu_tunnel* gtpu_tunnel_manager::add_tunnel(uint16_t rnti, uint32_t lcid,
 
 bool gtpu_tunnel_manager::update_rnti(uint16_t old_rnti, uint16_t new_rnti)
 {
-  srsran_assert(find_rnti_tunnels(new_rnti) == nullptr, "New rnti=0x%x already exists", new_rnti);
-
   auto* old_rnti_ptr = find_rnti_tunnels(old_rnti);
+  if (old_rnti_ptr == nullptr or find_rnti_tunnels(new_rnti) != nullptr) {
+    logger.error("Modifying bearer rnti. Old rnti=0x%x, new rnti=0x%x", old_rnti, new_rnti);
+    return false;
+  }
   logger.info("Modifying bearer rnti. Old rnti: 0x%x, new rnti: 0x%x", old_rnti, new_rnti);
 
   // create new RNTI and update TEIDs of old rnti to reflect new rnti
@@ -518,11 +520,6 @@ void gtpu::rem_bearer(uint16_t rnti, uint32_t lcid)
 
 void gtpu::mod_bearer_rnti(uint16_t old_rnti, uint16_t new_rnti)
 {
-  auto* old_rnti_ptr = tunnels.find_rnti_tunnels(old_rnti);
-  if (old_rnti_ptr == nullptr or tunnels.find_rnti_tunnels(new_rnti) != nullptr) {
-    logger.error("Modifying bearer rnti. Old rnti=0x%x, new rnti=0x%x", old_rnti, new_rnti);
-    return;
-  }
   tunnels.update_rnti(old_rnti, new_rnti);
 }
 
diff --git a/srsenb/src/stack/upper/ngap.cc b/srsenb/src/stack/upper/ngap.cc
index 25875cb5b..534d88fff 100644
--- a/srsenb/src/stack/upper/ngap.cc
+++ b/srsenb/src/stack/upper/ngap.cc
@@ -107,7 +107,7 @@ ngap::ngap(srsran::task_sched_handle   task_sched_,
   amf_task_queue = task_sched.make_task_queue();
 }
 
-int ngap::init(ngap_args_t args_, rrc_interface_ngap_nr* rrc_)
+int ngap::init(const ngap_args_t& args_, rrc_interface_ngap_nr* rrc_)
 {
   rrc  = rrc_;
   args = args_;
diff --git a/srsenb/src/stack/upper/s1ap.cc b/srsenb/src/stack/upper/s1ap.cc
index e2343486c..df294c27f 100644
--- a/srsenb/src/stack/upper/s1ap.cc
+++ b/srsenb/src/stack/upper/s1ap.cc
@@ -420,24 +420,21 @@ void s1ap::write_pdu(uint16_t rnti, srsran::unique_byte_buffer_t pdu)
 
 bool s1ap::user_release(uint16_t rnti, asn1::s1ap::cause_radio_network_e cause_radio)
 {
-  logger.info("User inactivity - RNTI:0x%x", rnti);
-
   ue* u = users.find_ue_rnti(rnti);
   if (u == nullptr) {
-    return false;
-  }
-
-  if (u->was_uectxtrelease_requested() or not u->ctxt.mme_ue_s1ap_id.has_value()) {
-    logger.warning("UE context for RNTI:0x%x is in zombie state. Releasing...", rnti);
-    users.erase(u);
-    rrc->release_ue(rnti);
+    logger.warning("Released UE with rnti=0x%x not found", rnti);
     return false;
   }
 
   cause_c cause;
   cause.set_radio_network().value = cause_radio.value;
 
-  return u->send_uectxtreleaserequest(cause);
+  if (not u->send_uectxtreleaserequest(cause)) {
+    users.erase(u);
+    rrc->release_ue(rnti);
+    return false;
+  }
+  return true;
 }
 
 bool s1ap::user_exists(uint16_t rnti)
@@ -1385,7 +1382,8 @@ bool s1ap::ue::send_initialuemessage(asn1::s1ap::rrc_establishment_cause_e cause
 
 bool s1ap::ue::send_ulnastransport(srsran::unique_byte_buffer_t pdu)
 {
-  if (not s1ap_ptr->mme_connected) {
+  if (not ctxt.mme_ue_s1ap_id.has_value()) {
+    logger.error("Trying to send UL NAS Transport message for rnti=0x%x without MME-S1AP-UE-ID", ctxt.rnti);
     return false;
   }
 
@@ -1410,6 +1408,10 @@ bool s1ap::ue::send_ulnastransport(srsran::unique_byte_buffer_t pdu)
 
 bool s1ap::ue::send_uectxtreleaserequest(const cause_c& cause)
 {
+  if (was_uectxtrelease_requested()) {
+    logger.warning("UE context for RNTI:0x%x is in zombie state. Releasing...", ctxt.rnti);
+    return false;
+  }
   if (not ctxt.mme_ue_s1ap_id.has_value()) {
     logger.error("Cannot send UE context release request without a MME-UE-S1AP-Id allocated.");
     return false;
diff --git a/srsue/src/phy/nr/cc_worker.cc b/srsue/src/phy/nr/cc_worker.cc
index 7530f0564..3ba1f7e9b 100644
--- a/srsue/src/phy/nr/cc_worker.cc
+++ b/srsue/src/phy/nr/cc_worker.cc
@@ -330,13 +330,11 @@ bool cc_worker::measure_csi()
       continue;
     }
 
-    logger.info("NZP-CSI-RS (TRS): id=%d rsrp=%+.1f epre=%+.1f snr=%+.1f cfo=%+.1f delay=%.1f",
-                resource_set_id,
-                trs_measurements.rsrp_dB,
-                trs_measurements.epre_dB,
-                trs_measurements.snr_dB,
-                trs_measurements.cfo_hz,
-                trs_measurements.delay_us);
+    if (logger.info.enabled()) {
+      std::array<char, 512> str = {};
+      srsran_csi_meas_info(&trs_measurements, str.data(), (uint32_t)str.size());
+      logger.info("NZP-CSI-RS (TRS): id=%d %s", resource_set_id, str.data());
+    }
 
     // Compute channel metrics and push it
     ch_metrics_t ch_metrics = {};
diff --git a/srsue/src/stack/mac/CMakeLists.txt b/srsue/src/stack/mac/CMakeLists.txt
index 026bba8f2..731f77cbf 100644
--- a/srsue/src/stack/mac/CMakeLists.txt
+++ b/srsue/src/stack/mac/CMakeLists.txt
@@ -20,4 +20,4 @@
 
 set(SOURCES demux.cc dl_harq.cc mac.cc mux.cc proc_bsr.cc proc_phr.cc proc_ra.cc proc_sr.cc ul_harq.cc)
 add_library(srsue_mac STATIC ${SOURCES})
-target_link_libraries(srsue_mac srsue_mac_common)
\ No newline at end of file
+target_link_libraries(srsue_mac srsue_mac_common ${ATOMIC_LIBS})
\ No newline at end of file
diff --git a/srsue/src/stack/mac/mac.cc b/srsue/src/stack/mac/mac.cc
index f18c61c0e..ebd598810 100644
--- a/srsue/src/stack/mac/mac.cc
+++ b/srsue/src/stack/mac/mac.cc
@@ -347,7 +347,7 @@ void mac::bch_decoded_ok(uint32_t cc_idx, uint8_t* payload, uint32_t len)
     buf->set_timestamp();
     auto p = stack_task_dispatch_queue.try_push(std::bind(
         [this](srsran::unique_byte_buffer_t& buf) { rlc_h->write_pdu_bcch_bch(std::move(buf)); }, std::move(buf)));
-    if (not p.first) {
+    if (p.is_error()) {
       Warning("Failed to dispatch rlc::write_pdu_bcch_bch task to stack");
     }
   } else {
@@ -408,7 +408,7 @@ void mac::tb_decoded(uint32_t cc_idx, mac_grant_dl_t grant, bool ack[SRSRAN_MAX_
 
       auto ret = stack_task_dispatch_queue.try_push(std::bind(
           [this](srsran::unique_byte_buffer_t& pdu) { rlc_h->write_pdu_pcch(std::move(pdu)); }, std::move(pdu)));
-      if (not ret.first) {
+      if (ret.is_error()) {
         Warning("Failed to dispatch rlc::write_pdu_pcch task to stack");
       }
     } else {
@@ -486,7 +486,7 @@ void mac::process_pdus()
       have_data = demux_unit.process_pdus();
     }
   });
-  if (not ret.first) {
+  if (ret.is_error()) {
     Warning("Failed to dispatch mac::%s task to stack thread", __func__);
   }
 }
diff --git a/srsue/src/stack/mac_nr/proc_ra_nr.cc b/srsue/src/stack/mac_nr/proc_ra_nr.cc
index 8aa2699d7..a493e3fce 100644
--- a/srsue/src/stack/mac_nr/proc_ra_nr.cc
+++ b/srsue/src/stack/mac_nr/proc_ra_nr.cc
@@ -63,7 +63,7 @@ void proc_ra_nr::set_config(const srsran::rach_nr_cfg_t& rach_cfg_)
   }
   rach_cfg   = rach_cfg_;
   configured = true;
-  logger.info("Set RACH common config (Config Index %d, preambleTransMax %d, Repsonse Window %d)",
+  logger.info("Set RACH common config (Config Index %d, preambleTransMax %d, Response Window %d)",
               rach_cfg.prach_ConfigurationIndex,
               rach_cfg.preambleTransMax,
               rach_cfg.ra_responseWindow);
@@ -380,4 +380,5 @@ void proc_ra_nr::reset()
   rar_timeout_timer.stop();
   contention_resolution_timer.stop();
 }
+
 } // namespace srsue
diff --git a/srsue/src/stack/rrc/rrc.cc b/srsue/src/stack/rrc/rrc.cc
index 800e3e17c..21225b242 100644
--- a/srsue/src/stack/rrc/rrc.cc
+++ b/srsue/src/stack/rrc/rrc.cc
@@ -2076,18 +2076,26 @@ void rrc::handle_ue_capability_enquiry(const ue_cap_enquiry_s& enquiry)
         irat_params_nr_r15.supported_band_list_en_dc_r15.push_back(supported_band_nr_r15);
         ue_eutra_cap_v1450_ies->non_crit_ext.non_crit_ext.irat_params_nr_r15_present = true;
         ue_eutra_cap_v1450_ies->non_crit_ext.non_crit_ext.irat_params_nr_r15         = irat_params_nr_r15;
+        ue_eutra_cap_v1450_ies->non_crit_ext.non_crit_ext.non_crit_ext_present       = true;
+
+        // 15.10
+        ue_eutra_cap_v1510_ies_s* ue_cap_enquiry_v1510_ies   = &ue_eutra_cap_v1450_ies->non_crit_ext.non_crit_ext;
+        ue_cap_enquiry_v1510_ies->pdcp_params_nr_r15_present = true;
+        ue_cap_enquiry_v1510_ies->pdcp_params_nr_r15.sn_size_lo_r15_present = true;
       }
 
       // Pack caps and copy to cap info
       uint8_t       buf[64] = {};
       asn1::bit_ref bref(buf, sizeof(buf));
-      cap.pack(bref);
+      if (cap.pack(bref) != asn1::SRSASN_SUCCESS) {
+        logger.error("Error packing EUTRA capabilities");
+        return;
+      }
       bref.align_bytes_zero();
       auto cap_len = (uint32_t)bref.distance_bytes(buf);
       info->ue_cap_rat_container_list[rat_idx].ue_cap_rat_container.resize(cap_len);
       memcpy(info->ue_cap_rat_container_list[rat_idx].ue_cap_rat_container.data(), buf, cap_len);
       rat_idx++;
-
     } else if (enquiry.crit_exts.c1().ue_cap_enquiry_r8().ue_cap_request[i] == rat_type_e::eutra_nr && has_nr_dc()) {
       info->ue_cap_rat_container_list[rat_idx] = get_eutra_nr_capabilities();
       logger.info("Including EUTRA-NR capabilities in UE Capability Info (%d B)",
diff --git a/srsue/src/stack/ue_stack_lte.cc b/srsue/src/stack/ue_stack_lte.cc
index ab8e77ea7..21d39b339 100644
--- a/srsue/src/stack/ue_stack_lte.cc
+++ b/srsue/src/stack/ue_stack_lte.cc
@@ -128,7 +128,7 @@ int ue_stack_lte::init(const stack_args_t& args_)
   mac_nr_logger.set_hex_dump_max_size(args.log.mac_hex_limit);
   rrc_nr_logger.set_level(srslog::str_to_basic_level(args.log.rrc_level));
   rrc_nr_logger.set_hex_dump_max_size(args.log.rrc_hex_limit);
-  
+
   // Set up pcap
   // parse pcap trace list
   std::vector<std::string> pcap_list;
@@ -350,7 +350,7 @@ void ue_stack_lte::run_thread()
 void ue_stack_lte::write_sdu(uint32_t lcid, srsran::unique_byte_buffer_t sdu)
 {
   auto task = [this, lcid](srsran::unique_byte_buffer_t& sdu) { pdcp.write_sdu(lcid, std::move(sdu)); };
-  bool ret  = gw_queue_id.try_push(std::bind(task, std::move(sdu))).first;
+  bool ret  = gw_queue_id.try_push(std::bind(task, std::move(sdu))).has_value();
   if (not ret) {
     pdcp_logger.info("GW SDU with lcid=%d was discarded.", lcid);
     ul_dropped_sdus++;
diff --git a/srsue/src/stack/ue_stack_nr.cc b/srsue/src/stack/ue_stack_nr.cc
index f7c452019..edf1b6f75 100644
--- a/srsue/src/stack/ue_stack_nr.cc
+++ b/srsue/src/stack/ue_stack_nr.cc
@@ -163,9 +163,9 @@ void ue_stack_nr::run_thread()
 void ue_stack_nr::write_sdu(uint32_t lcid, srsran::unique_byte_buffer_t sdu)
 {
   if (pdcp != nullptr) {
-    std::pair<bool, move_task_t> ret = gw_task_queue.try_push(std::bind(
+    auto ret = gw_task_queue.try_push(std::bind(
         [this, lcid](srsran::unique_byte_buffer_t& sdu) { pdcp->write_sdu(lcid, std::move(sdu)); }, std::move(sdu)));
-    if (not ret.first) {
+    if (ret.is_error()) {
       pdcp_logger.warning("GW SDU with lcid=%d was discarded.", lcid);
     }
   }