diff --git a/lib/include/srsran/adt/circular_map.h b/lib/include/srsran/adt/circular_map.h
index 1c03e9b43..ce0884d07 100644
--- a/lib/include/srsran/adt/circular_map.h
+++ b/lib/include/srsran/adt/circular_map.h
@@ -242,7 +242,7 @@ public:
 
   iterator       begin() { return iterator(this, 0); }
   iterator       end() { return iterator(this, N); }
-  const_iterator begin() const { return iterator(this, 0); }
+  const_iterator begin() const { return const_iterator(this, 0); }
   const_iterator end() const { return const_iterator(this, N); }
 
   iterator find(K id)
diff --git a/lib/include/srsran/adt/intrusive_list.h b/lib/include/srsran/adt/intrusive_list.h
new file mode 100644
index 000000000..def48b621
--- /dev/null
+++ b/lib/include/srsran/adt/intrusive_list.h
@@ -0,0 +1,246 @@
+/**
+ *
+ * \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_INTRUSIVE_LIST_H
+#define SRSRAN_INTRUSIVE_LIST_H
+
+#include <iterator>
+#include <type_traits>
+
+namespace srsran {
+
+struct default_intrusive_tag;
+
+/// Base class of T, where T is a node of intrusive_forward_list<T>
+template <typename Tag = default_intrusive_tag>
+struct intrusive_forward_list_element {
+  intrusive_forward_list_element<Tag>* next_node = nullptr;
+};
+
+/**
+ * Forward linked list of pointers of type "T" that doesn't rely on allocations.
+ * It leverages each node's internal pointer (thus intrusive) to store the next node of the list.
+ * It supports push_front/pop_front, iteration, clear, etc.
+ * @tparam T node type. It must be a subclass of intrusive_forward_list_element<Tag>
+ * @tparam Tag useful to differentiate multiple intrusive lists in the same node
+ */
+template <typename T, typename Tag = default_intrusive_tag>
+class intrusive_forward_list
+{
+  using node_t = intrusive_forward_list_element<Tag>;
+
+  template <typename U>
+  class iterator_impl
+  {
+    using elem_t = typename std::conditional<std::is_const<U>::value, const node_t, node_t>::type;
+
+  public:
+    using iterator_category = std::forward_iterator_tag;
+    using value_type        = U;
+    using difference_type   = std::ptrdiff_t;
+    using pointer           = U*;
+    using reference         = U&;
+
+    explicit iterator_impl(elem_t* node_ = nullptr) : node(node_) {}
+    iterator_impl<U>& operator++()
+    {
+      node = node->next_node;
+      return *this;
+    }
+    pointer   operator->() { return static_cast<pointer>(node); }
+    reference operator*() { return static_cast<reference>(*node); }
+
+    bool operator==(const iterator_impl<U>& other) const { return node == other.node; }
+    bool operator!=(const iterator_impl<U>& other) const { return node != other.node; }
+
+  private:
+    elem_t* node;
+  };
+
+public:
+  using iterator       = iterator_impl<T>;
+  using const_iterator = iterator_impl<const T>;
+
+  intrusive_forward_list()
+  {
+    static_assert(std::is_base_of<node_t, T>::value,
+                  "Provided template argument T must have intrusive_forward_list_element<Tag> as base class");
+  }
+  intrusive_forward_list(const intrusive_forward_list&) = default;
+  intrusive_forward_list(intrusive_forward_list&& other) noexcept : node(other.node) { other.node = nullptr; }
+  intrusive_forward_list& operator=(const intrusive_forward_list&) = default;
+  intrusive_forward_list& operator                                 =(intrusive_forward_list&& other) noexcept
+  {
+    node       = other.node;
+    other.node = nullptr;
+    return *this;
+  }
+
+  T& front() const { return *static_cast<T*>(node); }
+
+  void push_front(T* t)
+  {
+    node_t* new_head    = static_cast<node_t*>(t);
+    new_head->next_node = node;
+    node                = new_head;
+  }
+  T* pop_front()
+  {
+    node_t* ret = node;
+    node        = node->next_node;
+    return static_cast<T*>(ret);
+  }
+  void clear()
+  {
+    while (node != nullptr) {
+      node_t* torem    = node;
+      node             = node->next_node;
+      torem->next_node = nullptr;
+    }
+  }
+
+  bool empty() const { return node == nullptr; }
+
+  iterator       begin() { return iterator(node); }
+  iterator       end() { return iterator(nullptr); }
+  const_iterator begin() const { return const_iterator(node); }
+  const_iterator end() const { return const_iterator(nullptr); }
+
+private:
+  node_t* node = nullptr;
+};
+
+template <typename Tag = default_intrusive_tag>
+struct intrusive_double_linked_list_element {
+  intrusive_double_linked_list_element<Tag>* next_node = nullptr;
+  intrusive_double_linked_list_element<Tag>* prev_node = nullptr;
+};
+
+/**
+ * Double Linked List of pointers of type "T" that doesn't rely on allocations.
+ * Instead, it leverages T's internal pointers to store the next and previous nodes
+ * @tparam T node type. Must be a subclass of intrusive_double_linked_list_element<Tag>
+ * @tparam Tag tag of nodes. Useful to differentiate separate intrusive lists inside the same T node
+ */
+template <typename T, typename Tag = default_intrusive_tag>
+class intrusive_double_linked_list
+{
+  using node_t = intrusive_double_linked_list_element<Tag>;
+
+  template <typename U>
+  class iterator_impl
+  {
+    using elem_t = typename std::conditional<std::is_const<U>::value, const node_t, node_t>::type;
+
+  public:
+    using iterator_category = std::bidirectional_iterator_tag;
+    using value_type        = U;
+    using difference_type   = std::ptrdiff_t;
+    using pointer           = U*;
+    using reference         = U&;
+
+    explicit iterator_impl(elem_t* node_ = nullptr) : node(node_) {}
+    iterator_impl<U>& operator++()
+    {
+      node = node->next_node;
+      return *this;
+    }
+    iterator_impl<U>& operator--()
+    {
+      node = node->prev_node;
+      return *this;
+    }
+    pointer   operator->() { return static_cast<pointer>(node); }
+    reference operator*() { return static_cast<reference>(*node); }
+
+    bool operator==(const iterator_impl<U>& other) const { return node == other.node; }
+    bool operator!=(const iterator_impl<U>& other) const { return node != other.node; }
+
+  private:
+    elem_t* node;
+  };
+
+public:
+  using iterator       = iterator_impl<T>;
+  using const_iterator = iterator_impl<const T>;
+
+  intrusive_double_linked_list()
+  {
+    static_assert(std::is_base_of<node_t, T>::value,
+                  "Provided template argument T must have intrusive_forward_list_element<Tag> as base class");
+  }
+  intrusive_double_linked_list(const intrusive_double_linked_list&) = default;
+  intrusive_double_linked_list(intrusive_double_linked_list&& other) noexcept : node(other.node)
+  {
+    other.node = nullptr;
+  }
+  intrusive_double_linked_list& operator=(const intrusive_double_linked_list&) = default;
+  intrusive_double_linked_list& operator=(intrusive_double_linked_list&& other) noexcept
+  {
+    node       = other.node;
+    other.node = nullptr;
+    return *this;
+  }
+  ~intrusive_double_linked_list() { clear(); }
+
+  T& front() const { return *static_cast<T*>(node); }
+
+  void push_front(T* t)
+  {
+    node_t* new_head    = static_cast<node_t*>(t);
+    new_head->prev_node = nullptr;
+    new_head->next_node = node;
+    if (node != nullptr) {
+      node->prev_node = new_head;
+    }
+    node = new_head;
+  }
+  void pop(T* t)
+  {
+    node_t* to_rem = static_cast<node_t*>(t);
+    if (to_rem == node) {
+      node = to_rem->next_node;
+    }
+    if (to_rem->prev_node != nullptr) {
+      to_rem->prev_node->next_node = to_rem->next_node;
+    }
+    if (to_rem->next_node != nullptr) {
+      to_rem->next_node->prev_node = to_rem->prev_node;
+    }
+    to_rem->next_node = nullptr;
+    to_rem->prev_node = nullptr;
+  }
+  void pop_front() { pop(static_cast<T*>(node)); }
+  void clear()
+  {
+    while (node != nullptr) {
+      node_t* torem    = node;
+      node             = node->next_node;
+      torem->next_node = nullptr;
+      torem->prev_node = nullptr;
+    }
+  }
+
+  bool empty() const { return node == nullptr; }
+
+  iterator       begin() { return iterator(node); }
+  iterator       end() { return iterator(nullptr); }
+  const_iterator begin() const { return const_iterator(node); }
+  const_iterator end() const { return const_iterator(nullptr); }
+
+private:
+  node_t* node = nullptr;
+};
+
+} // namespace srsran
+
+#endif // SRSRAN_INTRUSIVE_LIST_H
diff --git a/lib/include/srsran/asn1/rrc_nr_utils.h b/lib/include/srsran/asn1/rrc_nr_utils.h
index 6f9cc2046..f9e81f227 100644
--- a/lib/include/srsran/asn1/rrc_nr_utils.h
+++ b/lib/include/srsran/asn1/rrc_nr_utils.h
@@ -40,6 +40,7 @@ struct rlc_cfg_c;
 struct pdcp_cfg_s;
 struct lc_ch_cfg_s;
 struct rach_cfg_common_s;
+struct phr_cfg_s;
 
 // Phy
 struct tdd_ul_dl_cfg_common_s;
@@ -101,6 +102,7 @@ bool make_phy_pusch_scaling(const asn1::rrc_nr::uci_on_pusch_s& uci_on_pusch, fl
  **************************/
 logical_channel_config_t make_mac_logical_channel_cfg_t(uint8_t lcid, const asn1::rrc_nr::lc_ch_cfg_s& asn1_type);
 rach_nr_cfg_t            make_mac_rach_cfg(const asn1::rrc_nr::rach_cfg_common_s& asn1_type);
+bool                     make_mac_phr_cfg_t(const asn1::rrc_nr::phr_cfg_s& asn1_type, phr_cfg_nr_t* phr_cfg_nr);
 /***************************
  *      RLC Config
  **************************/
diff --git a/lib/include/srsran/asn1/s1ap_utils.h b/lib/include/srsran/asn1/s1ap_utils.h
index edec1f552..75f857b55 100644
--- a/lib/include/srsran/asn1/s1ap_utils.h
+++ b/lib/include/srsran/asn1/s1ap_utils.h
@@ -47,6 +47,10 @@ struct bearers_subject_to_status_transfer_item_ies_o;
 struct erab_level_qos_params_s;
 struct ho_cmd_s;
 struct erab_admitted_item_s;
+struct erab_to_be_modified_item_bearer_mod_req_s;
+struct cause_c;
+struct erab_item_s;
+struct ue_aggregate_maximum_bitrate_s;
 
 template <class ies_set_paramT_>
 struct protocol_ie_single_container_s;
@@ -55,7 +59,32 @@ using bearers_subject_to_status_transfer_list_l =
 using rrc_establishment_cause_e = enumerated<rrc_establishment_cause_opts, true, 3>;
 using cause_radio_network_e     = enumerated<cause_radio_network_opts, true, 4>;
 
+/**************************
+ *     S1AP Obj Id
+ *************************/
+
+template <typename T>
+uint32_t get_obj_id(const T& obj);
+
+template <typename T>
+bool lower_obj_id(const T& lhs, const T& rhs)
+{
+  return get_obj_id(lhs) < get_obj_id(rhs);
+}
+
+template <typename T>
+bool equal_obj_id(const T& lhs, const T& rhs)
+{
+  return get_obj_id(lhs) == get_obj_id(rhs);
+}
+
 } // namespace s1ap
 } // namespace asn1
 
+namespace srsenb {
+
+using transp_addr_t = asn1::bounded_bitstring<1, 160, true, true>;
+
+}
+
 #endif // SRSRAN_S1AP_UTILS_H
diff --git a/lib/include/srsran/common/buffer_pool.h b/lib/include/srsran/common/buffer_pool.h
index 328aaa904..e96787867 100644
--- a/lib/include/srsran/common/buffer_pool.h
+++ b/lib/include/srsran/common/buffer_pool.h
@@ -57,7 +57,8 @@ public:
     if (capacity_ > 0) {
       nof_buffers = (uint32_t)capacity_;
     }
-    used.reserve(nof_buffers);
+    pool.reserve(nof_buffers);
+    free_list.reserve(nof_buffers);
     pthread_mutex_init(&mutex, nullptr);
     pthread_cond_init(&cv_not_empty, nullptr);
     for (uint32_t i = 0; i < nof_buffers; i++) {
@@ -66,21 +67,16 @@ public:
         perror("Error allocating memory. Exiting...\n");
         exit(-1);
       }
-      available.push(b);
+      pool.push_back(b);
+      free_list.push_back(b);
     }
     capacity = nof_buffers;
   }
 
   ~buffer_pool()
   {
-    // this destructor assumes all buffers have been properly deallocated
-    while (available.size()) {
-      delete available.top();
-      available.pop();
-    }
-
-    for (uint32_t i = 0; i < used.size(); i++) {
-      delete used[i];
+    for (auto* p : pool) {
+      delete p;
     }
     pthread_cond_destroy(&cv_not_empty);
     pthread_mutex_destroy(&mutex);
@@ -88,11 +84,13 @@ public:
 
   void print_all_buffers()
   {
-    printf("%d buffers in queue\n", (int)used.size());
+    printf("%d buffers in queue\n", static_cast<int>(pool.size() - free_list.size()));
 #ifdef SRSRAN_BUFFER_POOL_LOG_ENABLED
     std::map<std::string, uint32_t> buffer_cnt;
-    for (uint32_t i = 0; i < used.size(); i++) {
-      buffer_cnt[strlen(used[i]->debug_name) ? used[i]->debug_name : "Undefined"]++;
+    for (uint32_t i = 0; i < pool.size(); i++) {
+      if (std::find(free_list.cbegin(), free_list.cend(), pool[i]) == free_list.cend()) {
+        buffer_cnt[strlen(used[i]->debug_name) ? pool[i]->debug_name : "Undefined"]++;
+      }
     }
     std::map<std::string, uint32_t>::iterator it;
     for (it = buffer_cnt.begin(); it != buffer_cnt.end(); it++) {
@@ -101,22 +99,21 @@ public:
 #endif
   }
 
-  uint32_t nof_available_pdus() { return available.size(); }
+  uint32_t nof_available_pdus() { return free_list.size(); }
 
-  bool is_almost_empty() { return available.size() < capacity / 20; }
+  bool is_almost_empty() { return free_list.size() < capacity / 20; }
 
   buffer_t* allocate(const char* debug_name = nullptr, bool blocking = false)
   {
     pthread_mutex_lock(&mutex);
     buffer_t* b = nullptr;
 
-    if (available.size() > 0) {
-      b = available.top();
-      used.push_back(b);
-      available.pop();
+    if (!free_list.empty()) {
+      b = free_list.back();
+      free_list.pop_back();
 
       if (is_almost_empty()) {
-        printf("Warning buffer pool capacity is %f %%\n", (float)100 * available.size() / capacity);
+        printf("Warning buffer pool capacity is %f %%\n", (float)100 * free_list.size() / capacity);
       }
 #ifdef SRSRAN_BUFFER_POOL_LOG_ENABLED
       if (debug_name) {
@@ -126,14 +123,13 @@ public:
 #endif
     } else if (blocking) {
       // blocking allocation
-      while (available.size() == 0) {
+      while (free_list.empty()) {
         pthread_cond_wait(&cv_not_empty, &mutex);
       }
 
       // retrieve the new buffer
-      b = available.top();
-      used.push_back(b);
-      available.pop();
+      b = free_list.back();
+      free_list.pop_back();
 
       // do not print any warning
     } else {
@@ -152,10 +148,8 @@ public:
   {
     bool ret = false;
     pthread_mutex_lock(&mutex);
-    typename std::vector<buffer_t*>::iterator elem = std::find(used.begin(), used.end(), b);
-    if (elem != used.end()) {
-      used.erase(elem);
-      available.push(b);
+    if (std::find(pool.cbegin(), pool.cend(), b) != pool.cend()) {
+      free_list.push_back(b);
       ret = true;
     }
     pthread_cond_signal(&cv_not_empty);
@@ -165,8 +159,8 @@ public:
 
 private:
   static const int       POOL_SIZE = 4096;
-  std::stack<buffer_t*>  available;
-  std::vector<buffer_t*> used;
+  std::vector<buffer_t*> pool;
+  std::vector<buffer_t*> free_list;
   pthread_mutex_t        mutex;
   pthread_cond_t         cv_not_empty;
   uint32_t               capacity;
diff --git a/lib/include/srsran/common/lte_common.h b/lib/include/srsran/common/lte_common.h
new file mode 100644
index 000000000..788c2b1da
--- /dev/null
+++ b/lib/include/srsran/common/lte_common.h
@@ -0,0 +1,91 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2012-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_LTE_COMMON_H
+#define SRSRAN_LTE_COMMON_H
+
+#include <array>
+#include <cstdint>
+
+namespace srsran {
+
+// Cell nof PRBs
+const std::array<uint32_t, 6> lte_cell_nof_prbs = {6, 15, 25, 50, 75, 100};
+inline uint32_t               lte_cell_nof_prb_to_index(uint32_t nof_prb)
+{
+  switch (nof_prb) {
+    case 6:
+      return 0;
+    case 15:
+      return 1;
+    case 25:
+      return 2;
+    case 50:
+      return 3;
+    case 75:
+      return 4;
+    case 100:
+      return 5;
+    default:
+      break;
+  }
+  return -1;
+}
+inline bool is_lte_cell_nof_prb(uint32_t nof_prb)
+{
+  return lte_cell_nof_prb_to_index(nof_prb) < lte_cell_nof_prbs.size();
+}
+
+// Radio Bearers
+enum class lte_srb { srb0, srb1, srb2, count };
+const uint32_t MAX_LTE_SRB_ID = 2;
+enum class lte_drb { drb1 = 1, drb2, drb3, drb4, drb5, drb6, drb7, drb8, drb9, drb10, drb11, invalid };
+const uint32_t MAX_LTE_DRB_ID  = 11;
+const uint32_t MAX_NOF_BEARERS = 14;
+
+constexpr bool is_lte_rb(uint32_t lcid)
+{
+  return lcid < MAX_NOF_BEARERS;
+}
+
+constexpr bool is_lte_srb(uint32_t lcid)
+{
+  return lcid <= MAX_LTE_SRB_ID;
+}
+inline const char* get_srb_name(lte_srb srb_id)
+{
+  static const char* names[] = {"SRB0", "SRB1", "SRB2", "invalid SRB id"};
+  return names[(uint32_t)(srb_id < lte_srb::count ? srb_id : lte_srb::count)];
+}
+constexpr uint32_t srb_to_lcid(lte_srb srb_id)
+{
+  return static_cast<uint32_t>(srb_id);
+}
+constexpr lte_srb lte_lcid_to_srb(uint32_t lcid)
+{
+  return static_cast<lte_srb>(lcid);
+}
+
+constexpr bool is_lte_drb(uint32_t lcid)
+{
+  return lcid > MAX_LTE_SRB_ID and is_lte_rb(lcid);
+}
+inline const char* get_drb_name(lte_drb drb_id)
+{
+  static const char* names[] = {
+      "DRB1", "DRB2", "DRB3", "DRB4", "DRB5", "DRB6", "DRB7", "DRB8", "DRB9", "DRB10", "DRB11", "invalid DRB id"};
+  return names[(uint32_t)(drb_id < lte_drb::invalid ? drb_id : lte_drb::invalid) - 1];
+}
+
+} // namespace srsran
+
+#endif // SRSRAN_LTE_COMMON_H
diff --git a/lib/include/srsran/common/metrics_hub.h b/lib/include/srsran/common/metrics_hub.h
index 27c19eb13..ad262a028 100644
--- a/lib/include/srsran/common/metrics_hub.h
+++ b/lib/include/srsran/common/metrics_hub.h
@@ -90,7 +90,7 @@ private:
         std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - sleep_start);
 
     if (m) {
-      metrics_t metric;
+      metrics_t metric = {};
       m->get_metrics(&metric);
       for (uint32_t i = 0; i < listeners.size(); i++) {
         listeners[i]->set_metrics(metric, period_usec.count());
diff --git a/lib/include/srsran/common/timers.h b/lib/include/srsran/common/timers.h
index f833c5970..7580321e4 100644
--- a/lib/include/srsran/common/timers.h
+++ b/lib/include/srsran/common/timers.h
@@ -29,17 +29,13 @@
 #ifndef SRSRAN_TIMERS_H
 #define SRSRAN_TIMERS_H
 
+#include "srsran/adt/intrusive_list.h"
 #include "srsran/adt/move_callback.h"
-#include "srsran/phy/utils/debug.h"
 #include <algorithm>
-#include <functional>
+#include <cstdint>
+#include <deque>
 #include <limits>
 #include <mutex>
-#include <queue>
-#include <stdint.h>
-#include <stdio.h>
-#include <time.h>
-#include <vector>
 
 namespace srsran {
 
@@ -49,42 +45,60 @@ public:
   virtual void timer_expired(uint32_t timer_id) = 0;
 };
 
+/**
+ * Class that manages stack timers. It allows creation of unique_timers, with different ids. Each unique_timer duration,
+ * and callback can be set via the set(...) method. A timer can be started/stopped via run()/stop() methods.
+ * Internal Data structures:
+ * - timer_list - std::deque that stores timer objects via push_back() to keep pointer/reference validity.
+ *   The timer index in the timer_list matches the timer object id field.
+ *   This deque will only grow in size. Erased timers are just tagged in the deque as empty, and can be reused for the
+ *   creation of new timers. To avoid unnecessary runtime allocations, the user can set an initial capacity.
+ * - free_list - intrusive forward linked list to keep track of the empty timers and speed up new timer creation.
+ * - A large circular vector of size WHEEL_SIZE which works as a time wheel, storing and circularly indexing the
+ *   currently running timers by their respective timeout value.
+ *   For a number of running timers N, and uniform distribution of timeout values, the step_all() complexity
+ *   should be O(N/WHEEL_SIZE). Thus, the performance should improve with a larger WHEEL_SIZE, at the expense of more
+ *   used memory.
+ */
 class timer_handler
 {
-  constexpr static uint32_t MAX_TIMER_DURATION = std::numeric_limits<uint32_t>::max() / 4;
-  constexpr static uint32_t MAX_TIMER_VALUE    = std::numeric_limits<uint32_t>::max() / 2;
-
-  struct timer_impl {
-    timer_handler*                        parent;
-    uint32_t                              duration = 0, timeout = 0;
-    bool                                  running = false;
-    bool                                  active  = false;
+  using tic_diff_t                              = uint32_t;
+  using tic_t                                   = uint32_t;
+  constexpr static uint32_t   INVALID_ID        = std::numeric_limits<uint32_t>::max();
+  constexpr static tic_diff_t INVALID_TIME_DIFF = std::numeric_limits<tic_diff_t>::max();
+  constexpr static size_t     WHEEL_SHIFT       = 16U;
+  constexpr static size_t     WHEEL_SIZE        = 1U << WHEEL_SHIFT;
+  constexpr static size_t     WHEEL_MASK        = WHEEL_SIZE - 1U;
+
+  struct timer_impl : public intrusive_double_linked_list_element<>, public intrusive_forward_list_element<> {
+    timer_handler& parent;
+    const uint32_t id;
+    tic_diff_t     duration                                          = INVALID_TIME_DIFF;
+    tic_t          timeout                                           = 0;
+    enum state_t : int8_t { empty, stopped, running, expired } state = empty;
     srsran::move_callback<void(uint32_t)> callback;
 
-    explicit timer_impl(timer_handler* parent_) : parent(parent_) {}
-
-    uint32_t id() const { return std::distance((const timer_handler::timer_impl*)&parent->timer_list[0], this); }
-
-    bool is_running() const { return active and running and timeout > 0; }
-
-    bool is_expired() const { return active and not running and timeout > 0 and timeout <= parent->cur_time; }
+    explicit timer_impl(timer_handler& parent_, uint32_t id_) : parent(parent_), id(id_) {}
+    timer_impl(const timer_impl&) = delete;
+    timer_impl(timer_impl&&)      = delete;
+    timer_impl& operator=(const timer_impl&) = delete;
+    timer_impl& operator=(timer_impl&&) = delete;
 
-    uint32_t time_elapsed() const { return std::min(duration, parent->cur_time - (timeout - duration)); }
+    bool       is_empty() const { return state == empty; }
+    bool       is_running() const { return state == running; }
+    bool       is_expired() const { return state == expired; }
+    tic_diff_t time_left() const { return is_running() ? timeout - parent.cur_time : (is_expired() ? 0 : duration); }
+    uint32_t   time_elapsed() const { return duration - time_left(); }
 
     bool set(uint32_t duration_)
     {
-      if (duration_ > MAX_TIMER_DURATION) {
-        ERROR("Error: timer durations above %u are not supported", MAX_TIMER_DURATION);
-        return false;
-      }
-      if (not active) {
-        ERROR("Error: setting inactive timer id=%d", id());
-        return false;
-      }
-      duration = duration_;
+      duration = std::max(duration_, 1U); // the next step will be one place ahead of current one
       if (is_running()) {
         // if already running, just extends timer lifetime
         run();
+      } else {
+        state   = stopped;
+        timeout = 0;
       }
       return true;
     }
@@ -100,235 +114,242 @@ class timer_handler
 
     void run()
     {
-      std::unique_lock<std::mutex> lock(parent->mutex);
-      if (not active) {
-        ERROR("Error: calling run() for inactive timer id=%d", id());
-        return;
-      }
-      timeout = parent->cur_time + duration;
-      parent->running_timers.emplace(id(), timeout);
-      running = true;
+      std::lock_guard<std::mutex> lock(parent.mutex);
+      parent.start_run_(*this);
     }
 
     void stop()
     {
-      running = false; // invalidates trigger
-      if (not is_expired()) {
-        timeout = 0; // if it has already expired, then do not alter is_expired() state
-      }
+      std::lock_guard<std::mutex> lock(parent.mutex);
+      // does not call callback
+      parent.stop_timer_(*this, false);
     }
 
-    void clear()
-    {
-      stop();
-      duration = 0;
-      active   = false;
-      callback = srsran::move_callback<void(uint32_t)>();
-      // leave run_id unchanged. Since the timeout was changed, we shall not get spurious triggering
-    }
-
-    void trigger()
-    {
-      if (is_running()) {
-        running = false;
-        if (not callback.is_empty()) {
-          callback(id());
-        }
-      }
-    }
+    void deallocate() { parent.dealloc_timer(*this); }
   };
 
 public:
   class unique_timer
   {
   public:
-    unique_timer() : timer_id(std::numeric_limits<decltype(timer_id)>::max()) {}
-    explicit unique_timer(timer_handler* parent_, uint32_t timer_id_) : parent(parent_), timer_id(timer_id_) {}
-
+    unique_timer() = default;
+    explicit unique_timer(timer_impl* handle_) : handle(handle_) {}
     unique_timer(const unique_timer&) = delete;
-
-    unique_timer(unique_timer&& other) noexcept : parent(other.parent), timer_id(other.timer_id)
-    {
-      other.parent = nullptr;
-    }
-
-    ~unique_timer()
-    {
-      if (parent != nullptr) {
-        // does not call callback
-        impl()->clear();
-      }
-    }
-
+    unique_timer(unique_timer&& other) noexcept : handle(other.handle) { other.handle = nullptr; }
+    ~unique_timer() { release(); }
     unique_timer& operator=(const unique_timer&) = delete;
-
-    unique_timer& operator=(unique_timer&& other) noexcept
+    unique_timer& operator                       =(unique_timer&& other) noexcept
     {
       if (this != &other) {
-        timer_id     = other.timer_id;
-        parent       = other.parent;
-        other.parent = nullptr;
+        handle       = other.handle;
+        other.handle = nullptr;
       }
       return *this;
     }
 
-    bool is_valid() const { return parent != nullptr; }
+    bool is_valid() const { return handle != nullptr; }
 
     void set(uint32_t duration_, move_callback<void(uint32_t)> callback_)
     {
-      impl()->set(duration_, std::move(callback_));
+      srsran_assert(is_valid(), "Trying to setup empty timer handle");
+      handle->set(duration_, std::move(callback_));
+    }
+    void set(uint32_t duration_)
+    {
+      srsran_assert(is_valid(), "Trying to setup empty timer handle");
+      handle->set(duration_);
     }
 
-    void set(uint32_t duration_) { impl()->set(duration_); }
-
-    bool is_set() const { return (impl()->duration != 0); }
+    bool is_set() const { return is_valid() and handle->duration != INVALID_TIME_DIFF; }
 
-    bool is_running() const { return impl()->is_running(); }
+    bool is_running() const { return is_valid() and handle->is_running(); }
 
-    bool is_expired() const { return impl()->is_expired(); }
+    bool is_expired() const { return is_valid() and handle->is_expired(); }
 
-    uint32_t time_elapsed() const { return impl()->time_elapsed(); }
+    tic_diff_t time_elapsed() const { return is_valid() ? handle->time_elapsed() : INVALID_TIME_DIFF; }
 
-    void run() { impl()->run(); }
+    uint32_t id() const { return is_valid() ? handle->id : INVALID_ID; }
 
-    void stop() { impl()->stop(); }
+    tic_diff_t duration() const { return is_valid() ? handle->duration : INVALID_TIME_DIFF; }
 
-    void release()
+    void run()
     {
-      impl()->clear();
-      parent = nullptr;
+      srsran_assert(is_valid(), "Starting invalid timer");
+      handle->run();
     }
 
-    uint32_t id() const { return timer_id; }
+    void stop()
+    {
+      if (is_valid()) {
+        handle->stop();
+      }
+    }
 
-    uint32_t duration() const { return impl()->duration; }
+    void release()
+    {
+      if (is_valid()) {
+        handle->deallocate();
+        handle = nullptr;
+      }
+    }
 
   private:
-    timer_impl* impl() { return &parent->timer_list[timer_id]; }
-
-    const timer_impl* impl() const { return &parent->timer_list[timer_id]; }
-
-    timer_handler* parent = nullptr;
-    uint32_t       timer_id;
+    timer_impl* handle = nullptr;
   };
 
   explicit timer_handler(uint32_t capacity = 64)
   {
-    timer_list.reserve(capacity);
-    // reserve a priority queue using a vector
-    std::vector<timer_run> v;
-    v.reserve(capacity);
-    std::priority_queue<timer_run> q(std::less<timer_run>(), std::move(v));
-    running_timers = std::move(q);
+    time_wheel.resize(WHEEL_SIZE);
+    // Pre-reserve timers
+    while (timer_list.size() < capacity) {
+      timer_list.emplace_back(*this, timer_list.size());
+    }
+    // push to free list in reverse order to keep ascending ids
+    for (auto it = timer_list.rbegin(); it != timer_list.rend(); ++it) {
+      free_list.push_front(&(*it));
+    }
+    nof_free_timers = timer_list.size();
   }
 
   void step_all()
   {
     std::unique_lock<std::mutex> lock(mutex);
     cur_time++;
-    while (not running_timers.empty()) {
-      uint32_t    next_timeout = running_timers.top().timeout;
-      timer_impl* ptr          = &timer_list[running_timers.top().timer_id];
-      if (not ptr->is_running() or next_timeout != ptr->timeout) {
-        // remove timers that were explicitly stopped, or re-run, to avoid unnecessary priority_queue growth
-        running_timers.pop();
-        continue;
-      }
-      if (cur_time < next_timeout) {
-        break;
-      }
-      // if the timer_run and timer_impl timeouts do not match, it means that timer_impl::timeout was overwritten.
-      // in such case, do not trigger
-      uint32_t timeout = running_timers.top().timeout;
-      running_timers.pop();
+    auto& wheel_list = time_wheel[cur_time & WHEEL_MASK];
+
+    for (auto it = wheel_list.begin(); it != wheel_list.end();) {
+      timer_impl& timer = timer_list[it->id];
+      ++it;
+      if (timer.timeout == cur_time) {
+        // stop timer (callback has to see the timer has already expired)
+        stop_timer_(timer, true);
 
-      if (ptr->timeout == timeout) {
-        // unlock mutex, it could be that the callback tries to run a timer too
-        lock.unlock();
+        // Call callback if configured
+        if (not timer.callback.is_empty()) {
+          // unlock mutex. It can happen that the callback tries to run a timer too
+          lock.unlock();
 
-        // Call callback
-        ptr->trigger();
+          timer.callback(timer.id);
 
-        // Lock again to keep protecting the queue
-        lock.lock();
+          // Lock again to keep protecting the wheel
+          lock.lock();
+        }
       }
     }
   }
 
   void stop_all()
   {
+    std::lock_guard<std::mutex> lock(mutex);
     // does not call callback
-    while (not running_timers.empty()) {
-      running_timers.pop();
-    }
-    for (auto& i : timer_list) {
-      i.running = false;
+    for (timer_impl& timer : timer_list) {
+      stop_timer_(timer, false);
     }
   }
 
-  unique_timer get_unique_timer() { return unique_timer(this, alloc_timer()); }
-
-  uint32_t get_cur_time() const { return cur_time; }
+  unique_timer get_unique_timer() { return unique_timer(&alloc_timer()); }
 
   uint32_t nof_timers() const
   {
-    return std::count_if(timer_list.begin(), timer_list.end(), [](const timer_impl& t) { return t.active; });
+    std::lock_guard<std::mutex> lock(mutex);
+    return timer_list.size() - nof_free_timers;
   }
 
   uint32_t nof_running_timers() const
   {
-    return std::count_if(timer_list.begin(), timer_list.end(), [](const timer_impl& t) { return t.is_running(); });
+    std::lock_guard<std::mutex> lock(mutex);
+    return nof_timers_running_;
   }
 
   template <typename F>
   void defer_callback(uint32_t duration, const F& func)
   {
-    uint32_t                              id = alloc_timer();
-    srsran::move_callback<void(uint32_t)> c  = [func, this, id](uint32_t tid) {
+    timer_impl&                           timer = alloc_timer();
+    srsran::move_callback<void(uint32_t)> c     = [func, &timer](uint32_t tid) {
       func();
       // auto-deletes timer
-      timer_list[id].clear();
+      timer.deallocate();
     };
-    timer_list[id].set(duration, std::move(c));
-    timer_list[id].run();
+    timer.set(duration, std::move(c));
+    timer.run();
   }
 
 private:
-  struct timer_run {
-    uint32_t timer_id;
-    uint32_t timeout;
-
-    timer_run(uint32_t timer_id_, uint32_t timeout_) : timer_id(timer_id_), timeout(timeout_) {}
+  timer_impl& alloc_timer()
+  {
+    std::lock_guard<std::mutex> lock(mutex);
+    timer_impl*                 t;
+    if (not free_list.empty()) {
+      t = &free_list.front();
+      srsran_assert(t->is_empty(), "Invalid timer id=%d state", t->id);
+      free_list.pop_front();
+      nof_free_timers--;
+    } else {
+      // Need to increase deque
+      timer_list.emplace_back(*this, timer_list.size());
+      t = &timer_list.back();
+    }
+    t->state = timer_impl::stopped;
+    return *t;
+  }
 
-    bool operator<(const timer_run& other) const
-    {
-      // returns true, if other.timeout is lower than timeout, accounting for wrap around
-      if (timeout > other.timeout) {
-        return (timeout - other.timeout) < MAX_TIMER_VALUE / 2;
-      }
-      return (other.timeout - timeout) > MAX_TIMER_VALUE / 2;
+  void dealloc_timer(timer_impl& timer)
+  {
+    std::lock_guard<std::mutex> lock(mutex);
+    if (timer.is_empty()) {
+      // already deallocated
+      return;
     }
-  };
+    stop_timer_(timer, false);
+    timer.state    = timer_impl::empty;
+    timer.duration = INVALID_TIME_DIFF;
+    timer.timeout  = 0;
+    timer.callback = srsran::move_callback<void(uint32_t)>();
+    free_list.push_front(&timer);
+    nof_free_timers++;
+    // leave id unchanged.
+  }
 
-  uint32_t alloc_timer()
+  void start_run_(timer_impl& timer)
   {
-    uint32_t i = 0;
-    for (; i < timer_list.size(); ++i) {
-      if (not timer_list[i].active) {
-        break;
-      }
+    uint32_t timeout       = cur_time + timer.duration;
+    size_t   new_wheel_pos = timeout & WHEEL_MASK;
+    if (timer.is_running() and (timer.timeout & WHEEL_MASK) == new_wheel_pos) {
+      // If no change in timer wheel position
+      return;
     }
-    if (i == timer_list.size()) {
-      timer_list.emplace_back(this);
+
+    // Stop timer if it was running, removing it from wheel in the process
+    stop_timer_(timer, false);
+
+    // Insert timer in wheel
+    time_wheel[new_wheel_pos].push_front(&timer);
+    timer.timeout = timeout;
+    timer.state   = timer_impl::running;
+    nof_timers_running_++;
+  }
+
+  /// called when user manually stops timer (as an alternative to expiry)
+  void stop_timer_(timer_impl& timer, bool expiry)
+  {
+    if (not timer.is_running()) {
+      return;
     }
-    timer_list[i].active = true;
-    return i;
+
+    // If already running, need to disconnect it from previous wheel
+    time_wheel[timer.timeout & WHEEL_MASK].pop(&timer);
+
+    timer.state = expiry ? timer_impl::expired : timer_impl::stopped;
+    nof_timers_running_--;
   }
 
-  std::vector<timer_impl>        timer_list;
-  std::priority_queue<timer_run> running_timers;
-  uint32_t                       cur_time = 0;
-  std::mutex                     mutex; // Protect priority queue
+  tic_t  cur_time            = 0;
+  size_t nof_timers_running_ = 0, nof_free_timers = 0;
+  // using a deque to maintain reference validity on emplace_back. Also, this deque will only grow.
+  std::deque<timer_impl>                                         timer_list;
+  srsran::intrusive_forward_list<timer_impl>                     free_list;
+  std::vector<srsran::intrusive_double_linked_list<timer_impl> > time_wheel;
+  mutable std::mutex                                             mutex; // Protect priority queue
 };
 
 using unique_timer = timer_handler::unique_timer;
diff --git a/lib/include/srsran/interfaces/enb_rrc_interface_types.h b/lib/include/srsran/interfaces/enb_rrc_interface_types.h
index a02d37622..39eda2213 100644
--- a/lib/include/srsran/interfaces/enb_rrc_interface_types.h
+++ b/lib/include/srsran/interfaces/enb_rrc_interface_types.h
@@ -41,6 +41,8 @@ struct meas_cell_cfg_t {
   uint16_t pci;
   uint32_t eci;
   float    q_offset;
+  uint32_t allowed_meas_bw;
+  bool     direct_forward_path_available;
 };
 
 // neigh measurement Cell info
@@ -49,6 +51,7 @@ struct rrc_meas_cfg_t {
   std::vector<asn1::rrc::report_cfg_eutra_s> meas_reports;
   asn1::rrc::quant_cfg_eutra_s               quant_cfg;
   uint32_t                                   meas_gap_period;
+  uint32_t                                   allowed_meas_bw;
 };
 
 // Cell/Sector configuration
diff --git a/lib/include/srsran/interfaces/enb_rrc_interfaces.h b/lib/include/srsran/interfaces/enb_rrc_interfaces.h
index becf91451..4936d711c 100644
--- a/lib/include/srsran/interfaces/enb_rrc_interfaces.h
+++ b/lib/include/srsran/interfaces/enb_rrc_interfaces.h
@@ -32,21 +32,48 @@ namespace srsenb {
 class rrc_interface_s1ap
 {
 public:
+  using failed_erab_list = std::map<uint32_t, asn1::s1ap::cause_c>;
+
   virtual void write_dl_info(uint16_t rnti, srsran::unique_byte_buffer_t sdu)                    = 0;
   virtual void release_ue(uint16_t rnti)                                                         = 0;
   virtual bool setup_ue_ctxt(uint16_t rnti, const asn1::s1ap::init_context_setup_request_s& msg) = 0;
   virtual bool modify_ue_ctxt(uint16_t rnti, const asn1::s1ap::ue_context_mod_request_s& msg)    = 0;
-  virtual bool setup_ue_erabs(uint16_t rnti, const asn1::s1ap::erab_setup_request_s& msg)        = 0;
-  virtual void modify_erabs(uint16_t                                 rnti,
-                            const asn1::s1ap::erab_modify_request_s& msg,
-                            std::vector<uint16_t>*                   erabs_modified,
-                            std::vector<uint16_t>*                   erabs_failed_to_modify)                       = 0;
+  virtual bool has_erab(uint16_t rnti, uint32_t erab_id) const                                   = 0;
   virtual bool release_erabs(uint32_t rnti)                                                      = 0;
-  virtual void release_erabs(uint32_t                              rnti,
-                             const asn1::s1ap::erab_release_cmd_s& msg,
-                             std::vector<uint16_t>*                erabs_released,
-                             std::vector<uint16_t>*                erabs_failed_to_release)                     = 0;
-  virtual void add_paging_id(uint32_t ueid, const asn1::s1ap::ue_paging_id_c& ue_paging_id)      = 0;
+
+  virtual int  get_erab_addr_in(uint16_t rnti, uint16_t erab_id, transp_addr_t& addr_in, uint32_t& teid_in) const  = 0;
+  virtual void set_aggregate_max_bitrate(uint16_t rnti, const asn1::s1ap::ue_aggregate_maximum_bitrate_s& bitrate) = 0;
+
+  /**
+   * TS 36.413, 8.2.1 and 8.3.1 - Setup E-RAB / Initial Context Setup
+   * @return if error, cause argument is updated with cause
+   */
+  virtual int setup_erab(uint16_t                                   rnti,
+                         uint16_t                                   erab_id,
+                         const asn1::s1ap::erab_level_qos_params_s& qos_params,
+                         srsran::const_span<uint8_t>                nas_pdu,
+                         const transp_addr_t&                       addr,
+                         uint32_t                                   gtpu_teid_out,
+                         asn1::s1ap::cause_c&                       cause) = 0;
+  /**
+   * TS 36.413, 8.2.2 - Modify E-RAB
+   * @return if error, cause argument is updated with cause
+   */
+  virtual int modify_erab(uint16_t                                   rnti,
+                          uint16_t                                   erab_id,
+                          const asn1::s1ap::erab_level_qos_params_s& qos_params,
+                          srsran::const_span<uint8_t>                nas_pdu,
+                          asn1::s1ap::cause_c&                       cause) = 0;
+  /**
+   * TS 36.413, 8.2.3 - Release E-RAB id
+   * @return error if E-RAB id or rnti were not found
+   */
+  virtual int release_erab(uint16_t rnti, uint16_t erab_id) = 0;
+
+  virtual void add_paging_id(uint32_t ueid, const asn1::s1ap::ue_paging_id_c& ue_paging_id) = 0;
+
+  /// TS 36.413, 8.2.1, 8.2.2, 8.2.3 - Notify UE of ERAB updates (done via RRC Reconfiguration Message)
+  virtual int notify_ue_erab_updates(uint16_t rnti, srsran::const_span<uint8_t> nas_pdu) = 0;
 
   /**
    * Reports the reception of S1 HandoverCommand / HandoverPreparationFailure or abnormal conditions during
@@ -56,13 +83,15 @@ public:
    * @param is_success true if ho cmd was received
    * @param container TargeteNB RRCConnectionReconfiguration message with MobilityControlInfo
    */
+  enum class ho_prep_result { success, failure, timeout };
   virtual void ho_preparation_complete(uint16_t                     rnti,
-                                       bool                         is_success,
+                                       ho_prep_result               result,
                                        const asn1::s1ap::ho_cmd_s&  msg,
                                        srsran::unique_byte_buffer_t container) = 0;
   virtual uint16_t
                start_ho_ue_resource_alloc(const asn1::s1ap::ho_request_s&                                   msg,
-                                          const asn1::s1ap::sourceenb_to_targetenb_transparent_container_s& container) = 0;
+                                          const asn1::s1ap::sourceenb_to_targetenb_transparent_container_s& container,
+                                          asn1::s1ap::cause_c&                                              failure_cause)                                     = 0;
   virtual void set_erab_status(uint16_t rnti, const asn1::s1ap::bearers_subject_to_status_transfer_list_l& erabs) = 0;
 };
 
@@ -82,7 +111,9 @@ public:
   /* Radio Link failure */
   virtual int  add_user(uint16_t rnti, const sched_interface::ue_cfg_t& init_ue_cfg) = 0;
   virtual void upd_user(uint16_t new_rnti, uint16_t old_rnti)                        = 0;
-  virtual void set_activity_user(uint16_t rnti, bool ack_info)                       = 0;
+  virtual void set_activity_user(uint16_t rnti)                                      = 0;
+  virtual void set_radiolink_dl_state(uint16_t rnti, bool crc_res)                   = 0;
+  virtual void set_radiolink_ul_state(uint16_t rnti, bool crc_res)                   = 0;
   virtual bool is_paging_opportunity(uint32_t tti, uint32_t* payload_len)            = 0;
 
   ///< Provide packed SIB to MAC (buffer is managed by RRC)
diff --git a/lib/include/srsran/interfaces/enb_s1ap_interfaces.h b/lib/include/srsran/interfaces/enb_s1ap_interfaces.h
index d48a809c7..daebfbf36 100644
--- a/lib/include/srsran/interfaces/enb_s1ap_interfaces.h
+++ b/lib/include/srsran/interfaces/enb_s1ap_interfaces.h
@@ -43,6 +43,7 @@ struct s1ap_args_t {
 class s1ap_interface_rrc
 {
 public:
+  using failed_erab_list = std::map<uint32_t, asn1::s1ap::cause_c>;
   struct bearer_status_info {
     uint8_t  erab_id;
     uint16_t pdcp_dl_sn, pdcp_ul_sn;
@@ -60,13 +61,14 @@ public:
                           uint32_t                              m_tmsi,
                           uint8_t                               mmec)                     = 0;
 
-  virtual void write_pdu(uint16_t rnti, srsran::unique_byte_buffer_t pdu)                              = 0;
-  virtual bool user_exists(uint16_t rnti)                                                              = 0;
-  virtual void user_mod(uint16_t old_rnti, uint16_t new_rnti)                                          = 0;
-  virtual bool user_release(uint16_t rnti, asn1::s1ap::cause_radio_network_e cause_radio)              = 0;
-  virtual void ue_ctxt_setup_complete(uint16_t rnti, const asn1::s1ap::init_context_setup_resp_s& res) = 0;
-  virtual void ue_erab_setup_complete(uint16_t rnti, const asn1::s1ap::erab_setup_resp_s& res)         = 0;
-  virtual bool is_mme_connected()                                                                      = 0;
+  virtual void write_pdu(uint16_t rnti, srsran::unique_byte_buffer_t pdu)                 = 0;
+  virtual bool user_exists(uint16_t rnti)                                                 = 0;
+  virtual void user_mod(uint16_t old_rnti, uint16_t new_rnti)                             = 0;
+  virtual bool user_release(uint16_t rnti, asn1::s1ap::cause_radio_network_e cause_radio) = 0;
+  virtual bool is_mme_connected()                                                         = 0;
+
+  /// TS 36.413, 8.3.1 - Initial Context Setup
+  virtual void ue_ctxt_setup_complete(uint16_t rnti) = 0;
 
   /**
    * Command the s1ap to transmit a HandoverRequired message to MME.
@@ -83,7 +85,8 @@ public:
                                 uint32_t                     target_eci,
                                 srsran::plmn_id_t            target_plmn,
                                 srsran::span<uint32_t>       fwd_erabs,
-                                srsran::unique_byte_buffer_t rrc_container) = 0;
+                                srsran::unique_byte_buffer_t rrc_container,
+                                bool                         has_direct_fwd_path) = 0;
 
   /**
    * Command the s1ap to transmit eNBStatusTransfer message to MME. This message passes the PDCP context of the UE
@@ -95,24 +98,22 @@ public:
    */
   virtual bool send_enb_status_transfer_proc(uint16_t rnti, std::vector<bearer_status_info>& bearer_status_list) = 0;
 
-  /* Acknowledge Handover Request message back to MME.
-   * This message signals the completion of the HandoverPreparation from the TeNB point of view. */
-  virtual bool send_ho_req_ack(const asn1::s1ap::ho_request_s&                msg,
-                               uint16_t                                       rnti,
-                               uint32_t                                       enb_cc_idx,
-                               srsran::unique_byte_buffer_t                   ho_cmd,
-                               srsran::span<asn1::s1ap::erab_admitted_item_s> admitted_bearers) = 0;
-
-  /**
-   * Notify MME that Handover is complete
-   */
-  virtual void send_ho_notify(uint16_t rnti, uint64_t target_eci) = 0;
-
   /**
    * Cancel on-going S1 Handover. MME should release UE context in target eNB
    * SeNB --> MME
    */
-  virtual void send_ho_cancel(uint16_t rnti) = 0;
+  virtual void send_ho_cancel(uint16_t rnti, const asn1::s1ap::cause_c& cause) = 0;
+
+  /*************************
+   *  Target eNB Handover
+   ************************/
+  virtual bool send_ho_req_ack(const asn1::s1ap::ho_request_s&                msg,
+                               uint16_t                                       rnti,
+                               uint32_t                                       enb_cc_idx,
+                               srsran::unique_byte_buffer_t                   ho_cmd,
+                               srsran::span<asn1::s1ap::erab_admitted_item_s> admitted_bearers,
+                               srsran::const_span<asn1::s1ap::erab_item_s>    not_admitted_bearers) = 0;
+  virtual void send_ho_notify(uint16_t rnti, uint64_t target_eci)                                = 0;
 
   /**
    * Called during release of a subset of eNB E-RABs. Send E-RAB RELEASE INDICATION to MME.
diff --git a/lib/include/srsran/interfaces/mac_interface_types.h b/lib/include/srsran/interfaces/mac_interface_types.h
index 557b63ca4..3b4282179 100644
--- a/lib/include/srsran/interfaces/mac_interface_types.h
+++ b/lib/include/srsran/interfaces/mac_interface_types.h
@@ -159,6 +159,26 @@ struct sr_cfg_nr_t {
   sr_cfg_item_nr_t item[SRSRAN_MAX_MAX_NR_OF_SR_CFG_PER_CELL_GROUP];
 };
 
+struct tag_cfg_nr_t {
+  uint8_t  tag_id;
+  uint32_t time_align_timer;
+};
+
+struct phr_cfg_nr_t {
+  int  periodic_timer;
+  int  prohibit_timer;
+  int  tx_pwr_factor_change;
+  bool extended;
+  phr_cfg_nr_t() { reset(); }
+  void reset()
+  {
+    periodic_timer       = -1;
+    prohibit_timer       = -1;
+    tx_pwr_factor_change = -1;
+    extended             = false;
+  }
+};
+
 struct bsr_cfg_nr_t {
   // mandatory BSR config
   int periodic_timer;
diff --git a/lib/include/srsran/interfaces/pdcp_interface_types.h b/lib/include/srsran/interfaces/pdcp_interface_types.h
index 39bae45de..dfea173cb 100644
--- a/lib/include/srsran/interfaces/pdcp_interface_types.h
+++ b/lib/include/srsran/interfaces/pdcp_interface_types.h
@@ -179,8 +179,9 @@ struct pdcp_lte_state_t {
 };
 
 // Custom type for interface between PDCP and RLC to convey SDU delivery status
-#define MAX_SDUS_PER_RLC_PDU (256) // default to RLC SDU queue length
-#define MAX_SDUS_TO_NOTIFY (MAX_SDUS_PER_RLC_PDU) // Arbitrarily chosen limit
+// Arbitrarily chosen limit, optimal value depends on the RLC (pollPDU) and PDCP config, channel BLER,
+// traffic characterisitcs, etc. The chosen value has been tested with 100 PRB bi-dir TCP
+#define MAX_SDUS_TO_NOTIFY (1024)
 typedef srsran::bounded_vector<uint32_t, MAX_SDUS_TO_NOTIFY> pdcp_sn_vector_t;
 
 } // namespace srsran
diff --git a/lib/include/srsran/interfaces/rrc_nr_interface_types.h b/lib/include/srsran/interfaces/rrc_nr_interface_types.h
index ac5f2f651..35818b4b5 100644
--- a/lib/include/srsran/interfaces/rrc_nr_interface_types.h
+++ b/lib/include/srsran/interfaces/rrc_nr_interface_types.h
@@ -44,21 +44,6 @@ struct phy_cfg_nr_t {
 
   phy_cfg_nr_t()
   {
-    // tdd-UL-DL-ConfigurationCommon
-    //    referenceSubcarrierSpacing: kHz15 (0)
-    //    pattern1
-    //        dl-UL-TransmissionPeriodicity: ms10 (7)
-    //        nrofDownlinkSlots: 7
-    //        nrofDownlinkSymbols: 6
-    //        nrofUplinkSlots: 2
-    //        nrofUplinkSymbols: 4
-    tdd.pattern1.period_ms      = 10;
-    tdd.pattern1.nof_dl_slots   = 7;
-    tdd.pattern1.nof_dl_symbols = 6;
-    tdd.pattern1.nof_ul_slots   = 2;
-    tdd.pattern1.nof_ul_symbols = 4;
-    tdd.pattern2.period_ms      = 0;
-
     // physicalCellGroupConfig
     //    pdsch-HARQ-ACK-Codebook: dynamic (1)
     harq_ack.harq_ack_codebook = srsran_pdsch_harq_ack_codebook_dynamic;
diff --git a/lib/include/srsran/interfaces/ue_nr_interfaces.h b/lib/include/srsran/interfaces/ue_nr_interfaces.h
index 3d37c3e36..f26a35524 100644
--- a/lib/include/srsran/interfaces/ue_nr_interfaces.h
+++ b/lib/include/srsran/interfaces/ue_nr_interfaces.h
@@ -49,11 +49,15 @@ public:
     uint32_t                     tti;
   } mac_nr_grant_dl_t;
 
+  // UL grant as conveyed between PHY and MAC
   typedef struct {
-    uint32_t pid;
     uint16_t rnti;
     uint32_t tti;
-    uint32_t tbs; // transport block size in Bytes
+    uint8_t  pid;          // HARQ process ID
+    uint32_t tbs;          // transport block size in Bytes
+    uint8_t  ndi;          // Raw new data indicator extracted from DCI
+    uint8_t  rv;           // Redundancy Version
+    bool     is_rar_grant; // True if grant comes from RAR
   } mac_nr_grant_ul_t;
 
   /// For UL, payload buffer remains in MAC
@@ -120,6 +124,9 @@ public:
   virtual int  set_config(const srsran::bsr_cfg_nr_t& bsr_cfg)            = 0;
   virtual int  set_config(const srsran::sr_cfg_nr_t& sr_cfg)              = 0;
   virtual void set_config(const srsran::rach_nr_cfg_t& rach_cfg)          = 0;
+  virtual int  add_tag_config(const srsran::tag_cfg_nr_t& tag_cfg)        = 0;
+  virtual int  set_config(const srsran::phr_cfg_nr_t& phr_cfg)            = 0;
+  virtual int  remove_tag_config(const uint32_t tag_id)                   = 0;
 
   // RRC triggers MAC ra procedure
   virtual void start_ra_procedure() = 0;
diff --git a/lib/include/srsran/interfaces/ue_rrc_interfaces.h b/lib/include/srsran/interfaces/ue_rrc_interfaces.h
index 2df8579db..ba4f246fb 100644
--- a/lib/include/srsran/interfaces/ue_rrc_interfaces.h
+++ b/lib/include/srsran/interfaces/ue_rrc_interfaces.h
@@ -80,7 +80,7 @@ public:
   virtual void        set_ue_identity(srsran::s_tmsi_t s_tmsi)                          = 0;
   virtual bool        is_connected()                                                    = 0;
   virtual void        paging_completed(bool outcome)                                    = 0;
-  virtual std::string get_rb_name(uint32_t lcid)                                        = 0;
+  virtual const char* get_rb_name(uint32_t lcid)                                        = 0;
   virtual uint32_t    get_lcid_for_eps_bearer(const uint32_t& eps_bearer_id)            = 0;
   virtual bool        has_nr_dc()                                                       = 0;
 };
@@ -93,14 +93,14 @@ public:
   virtual void        write_pdu_bcch_dlsch(srsran::unique_byte_buffer_t pdu)         = 0;
   virtual void        write_pdu_pcch(srsran::unique_byte_buffer_t pdu)               = 0;
   virtual void        write_pdu_mch(uint32_t lcid, srsran::unique_byte_buffer_t pdu) = 0;
-  virtual std::string get_rb_name(uint32_t lcid)                                     = 0;
+  virtual const char* get_rb_name(uint32_t lcid)                                     = 0;
 };
 
 class rrc_interface_rlc
 {
 public:
   virtual void        max_retx_attempted()                                       = 0;
-  virtual std::string get_rb_name(uint32_t lcid)                                 = 0;
+  virtual const char* get_rb_name(uint32_t lcid)                                 = 0;
   virtual void        write_pdu(uint32_t lcid, srsran::unique_byte_buffer_t pdu) = 0;
 };
 
diff --git a/lib/include/srsran/mac/mac_sch_pdu_nr.h b/lib/include/srsran/mac/mac_sch_pdu_nr.h
index b53293de3..29bc160a9 100644
--- a/lib/include/srsran/mac/mac_sch_pdu_nr.h
+++ b/lib/include/srsran/mac/mac_sch_pdu_nr.h
@@ -63,7 +63,7 @@ public:
   // SDUs up to 256 B can use the short 8-bit L field
   static const int32_t MAC_SUBHEADER_LEN_THRESHOLD = 256;
 
-  mac_sch_subpdu_nr(mac_sch_pdu_nr* parent_) : parent(parent_), logger(&srslog::fetch_basic_logger("MAC")){};
+  mac_sch_subpdu_nr(mac_sch_pdu_nr* parent_) : parent(parent_), logger(&srslog::fetch_basic_logger("MAC-NR")){};
 
   nr_lcid_sch_t get_type();
   bool          is_sdu();
@@ -129,10 +129,10 @@ private:
 class mac_sch_pdu_nr
 {
 public:
-  mac_sch_pdu_nr(bool ulsch_ = false) : ulsch(ulsch_), logger(srslog::fetch_basic_logger("MAC")) {}
+  mac_sch_pdu_nr(bool ulsch_ = false) : ulsch(ulsch_), logger(srslog::fetch_basic_logger("MAC-NR")) {}
 
   void                     pack();
-  void                     unpack(const uint8_t* payload, const uint32_t& len);
+  int                      unpack(const uint8_t* payload, const uint32_t& len);
   uint32_t                 get_num_subpdus();
   const mac_sch_subpdu_nr& get_subpdu(const uint32_t& index);
   bool                     is_ulsch();
diff --git a/lib/include/srsran/phy/ch_estimation/dmrs_sch.h b/lib/include/srsran/phy/ch_estimation/dmrs_sch.h
index de3d519e0..3b495980b 100644
--- a/lib/include/srsran/phy/ch_estimation/dmrs_sch.h
+++ b/lib/include/srsran/phy/ch_estimation/dmrs_sch.h
@@ -58,6 +58,7 @@ typedef struct {
   cf_t*    pilot_estimates; /// Pilots least squares estimates
   cf_t*    temp;            /// Temporal data vector of size SRSRAN_NRE * carrier.nof_prb
 
+  float* filter; ///< Smoothing filter
 } srsran_dmrs_sch_t;
 
 /**
diff --git a/lib/include/srsran/phy/common/phy_common_nr.h b/lib/include/srsran/phy/common/phy_common_nr.h
index ea0145af2..b4c0a3473 100644
--- a/lib/include/srsran/phy/common/phy_common_nr.h
+++ b/lib/include/srsran/phy/common/phy_common_nr.h
@@ -129,6 +129,21 @@ extern "C" {
  */
 #define SRSRAN_MAX_NOF_DL_DATA_TO_UL 8
 
+/**
+ * @brief Maximum number of HARQ processes in the DL, signaled through RRC (PDSCH-ServingCellConfig)
+ */
+#define SRSRAN_MAX_HARQ_PROC_DL_NR 16 // 3GPP TS 38.214 version 15.3.0 Sec. 5.1 or nrofHARQ-ProcessesForPDSCH
+
+/**
+ * @brief Default number of HARQ processes in the DL, if config is absent.
+ */
+#define SRSRAN_DEFAULT_HARQ_PROC_DL_NR 8
+
+/**
+ * @brief Maximum number of HARQ processes in the UL, signaled through RRC (ConfiguredGrantConfig)
+ */
+#define SRSRAN_MAX_HARQ_PROC_UL_NR 16 // 3GPP TS 38.214 version 15.3.0 Sec. 6.1
+
 typedef enum SRSRAN_API {
   srsran_coreset_mapping_type_non_interleaved = 0,
   srsran_coreset_mapping_type_interleaved,
diff --git a/lib/include/srsran/phy/fec/crc.h b/lib/include/srsran/phy/fec/crc.h
index 4ce95cbe9..88a474fc8 100644
--- a/lib/include/srsran/phy/fec/crc.h
+++ b/lib/include/srsran/phy/fec/crc.h
@@ -33,6 +33,7 @@
 #define SRSRAN_CRC_H
 
 #include "srsran/config.h"
+#include <stdbool.h>
 #include <stdint.h>
 
 typedef struct SRSRAN_API {
@@ -82,4 +83,6 @@ SRSRAN_API uint32_t srsran_crc_checksum_byte(srsran_crc_t* h, const uint8_t* dat
 
 SRSRAN_API uint32_t srsran_crc_checksum(srsran_crc_t* h, uint8_t* data, int len);
 
+SRSRAN_API bool srsran_crc_match(srsran_crc_t* h, uint8_t* data, int len);
+
 #endif // SRSRAN_CRC_H
diff --git a/lib/include/srsran/phy/fec/ldpc/ldpc_decoder.h b/lib/include/srsran/phy/fec/ldpc/ldpc_decoder.h
index bf7ff438a..ae0799426 100644
--- a/lib/include/srsran/phy/fec/ldpc/ldpc_decoder.h
+++ b/lib/include/srsran/phy/fec/ldpc/ldpc_decoder.h
@@ -32,13 +32,14 @@
 #ifndef SRSRAN_LDPCDECODER_H
 #define SRSRAN_LDPCDECODER_H
 
+#include "srsran/phy/fec/crc.h"
 #include "srsran/phy/fec/ldpc/base_graph.h"
 
 /*!
  * \brief Types of LDPC decoder.
  */
 typedef enum {
-  SRSRAN_LDPC_DECODER_F,            /*!< \brief %Decoder working with real-valued LLRs. */
+  SRSRAN_LDPC_DECODER_F = 0,        /*!< \brief %Decoder working with real-valued LLRs. */
   SRSRAN_LDPC_DECODER_S,            /*!< \brief %Decoder working with 16-bit integer-valued LLRs. */
   SRSRAN_LDPC_DECODER_C,            /*!< \brief %Decoder working with 8-bit integer-valued LLRs. */
   SRSRAN_LDPC_DECODER_C_FLOOD,      /*!< \brief %Decoder working with 8-bit integer-valued LLRs, flooded scheduling. */
@@ -50,20 +51,32 @@ typedef enum {
                                    (AVX512 version). */
 } srsran_ldpc_decoder_type_t;
 
+/*!
+ * \brief Describes the LDPC decoder configuration arguments.
+ */
+typedef struct {
+  srsran_ldpc_decoder_type_t type;         /*!< \brief Type of LDPC decoder. */
+  srsran_basegraph_t         bg;           /*!< \brief The desired base graph (BG1 or BG2). */
+  uint16_t                   ls;           /*!< \brief The desired lifting size. */
+  float                      scaling_fctr; /*!< \brief Scaling factor of the normalized min-sum algorithm.*/
+  uint32_t                   max_nof_iter; /*!< \brief Maximum number of iterations, set to 0 for default value. */
+} srsran_ldpc_decoder_args_t;
+
 /*!
  * \brief Describes an LDPC decoder.
  */
 typedef struct SRSRAN_API {
-  void*              ptr;   /*!< \brief Registers used by the decoder. */
-  srsran_basegraph_t bg;    /*!< \brief Current base graph. */
-  uint16_t           ls;    /*!< \brief Current lifting size. */
-  uint8_t            bgN;   /*!< \brief Number of variable nodes in the BG. */
-  uint16_t           liftN; /*!< \brief Number of variable nodes in the lifted graph. */
-  uint8_t            bgM;   /*!< \brief Number of check nodes in the BG. */
-  uint16_t           liftM; /*!< \brief Number of check nodes in the lifted graph. */
-  uint8_t            bgK;   /*!< \brief Number of "uncoded bits" in the BG. */
-  uint16_t           liftK; /*!< \brief Number of uncoded bits in the lifted graph. */
-  uint16_t*          pcm;   /*!< \brief Pointer to the parity check matrix (compact form). */
+  void*              ptr;          /*!< \brief Registers used by the decoder. */
+  srsran_basegraph_t bg;           /*!< \brief Current base graph. */
+  uint16_t           ls;           /*!< \brief Current lifting size. */
+  uint32_t           max_nof_iter; /*!< \brief Maximum number of iterations. */
+  uint8_t            bgN;          /*!< \brief Number of variable nodes in the BG. */
+  uint16_t           liftN;        /*!< \brief Number of variable nodes in the lifted graph. */
+  uint8_t            bgM;          /*!< \brief Number of check nodes in the BG. */
+  uint16_t           liftM;        /*!< \brief Number of check nodes in the lifted graph. */
+  uint8_t            bgK;          /*!< \brief Number of "uncoded bits" in the BG. */
+  uint16_t           liftK;        /*!< \brief Number of uncoded bits in the lifted graph. */
+  uint16_t*          pcm;          /*!< \brief Pointer to the parity check matrix (compact form). */
 
   int8_t (*var_indices)[MAX_CNCT]; /*!< \brief Pointer to lists of variable indices connected to a given check node. */
 
@@ -74,32 +87,28 @@ typedef struct SRSRAN_API {
   int (*decode_f)(void*,
                   const float*,
                   uint8_t*,
-                  uint32_t); /*!< \brief Pointer to the decoding function (float version). */
+                  uint32_t,
+                  srsran_crc_t*); /*!< \brief Pointer to the decoding function (float version). */
   int (*decode_s)(void*,
                   const int16_t*,
                   uint8_t*,
-                  uint32_t); /*!< \brief Pointer to the decoding function (16-bit version). */
+                  uint32_t,
+                  srsran_crc_t*); /*!< \brief Pointer to the decoding function (16-bit version). */
   int (*decode_c)(void*,
                   const int8_t*,
                   uint8_t*,
-                  uint32_t); /*!< \brief Pointer to the decoding function (16-bit version). */
+                  uint32_t,
+                  srsran_crc_t*); /*!< \brief Pointer to the decoding function (16-bit version). */
 } srsran_ldpc_decoder_t;
 
 /*!
  * Initializes all the LDPC decoder variables according to the given base graph
  * and lifting size.
  * \param[out] q            A pointer to a srsran_ldpc_decoder_t structure.
- * \param[in]  type         Type of LDPC decoder.
- * \param[in]  bg           The desired base graph (BG1 or BG2).
- * \param[in]  ls           The desired lifting size.
- * \param[in]  scaling_fctr Scaling factor of the normalized min-sum algorithm.
+ * \param[in]  args         LDPC configuration arguments.
  * \return An integer: 0 if the function executes correctly, -1 otherwise.
  */
-SRSRAN_API int srsran_ldpc_decoder_init(srsran_ldpc_decoder_t*     q,
-                                        srsran_ldpc_decoder_type_t type,
-                                        srsran_basegraph_t         bg,
-                                        uint16_t                   ls,
-                                        float                      scaling_fctr);
+SRSRAN_API int srsran_ldpc_decoder_init(srsran_ldpc_decoder_t* q, const srsran_ldpc_decoder_args_t* args);
 
 /*!
  * The LDPC decoder "destructor": it frees all the resources allocated to the decoder.
@@ -150,4 +159,24 @@ srsran_ldpc_decoder_decode_s(srsran_ldpc_decoder_t* q, const int16_t* llrs, uint
 SRSRAN_API int
 srsran_ldpc_decoder_decode_c(srsran_ldpc_decoder_t* q, const int8_t* llrs, uint8_t* message, uint32_t cdwd_rm_length);
 
+/*!
+ * Carries out the actual decoding with 8-bit integer-valued LLRs. It is
+ * recommended to use a 7-bit representation for the LLRs, given that all
+ * values exceeding \f$ 2^{7}-1 \f$ (in magnitude) will be considered as infinity.
+ * \param[in] q A pointer to the LDPC decoder (a srsran_ldpc_decoder_t structure
+ *    instance) that carries out the decoding.
+ * \param[in] llrs The LLRs obtained from the channel samples that correspond to
+ *    the codeword to be decoded.
+ * \param[out] message The message (uncoded bits) resulting from the decoding
+ *    operation.
+ * \param[in] cdwd_rm_length The number of bits forming the codeword (after rate matching).
+ * \param[in,out] crc Code-block CRC object for early stop. Set for NULL to disable check
+ * \return -1 if an error occurred, the number of used iterations, and 0 if CRC is provided and did not match
+ */
+SRSRAN_API int srsran_ldpc_decoder_decode_crc_c(srsran_ldpc_decoder_t* q,
+                                                const int8_t*          llrs,
+                                                uint8_t*               message,
+                                                uint32_t               cdwd_rm_length,
+                                                srsran_crc_t*          crc);
+
 #endif // SRSRAN_LDPCDECODER_H
diff --git a/lib/include/srsran/phy/modem/demod_soft.h b/lib/include/srsran/phy/modem/demod_soft.h
index 515dfe087..08aff4e89 100644
--- a/lib/include/srsran/phy/modem/demod_soft.h
+++ b/lib/include/srsran/phy/modem/demod_soft.h
@@ -42,4 +42,14 @@ SRSRAN_API int srsran_demod_soft_demodulate_s(srsran_mod_t modulation, const cf_
 
 SRSRAN_API int srsran_demod_soft_demodulate_b(srsran_mod_t modulation, const cf_t* symbols, int8_t* llr, int nsymbols);
 
+/**
+ * @brief Soft-demodulates complex symbols into 8-bit LLR. It forces zero symbols produce zero LLRs.
+ * @param modulation Modulation
+ * @param symbols Complex symbols
+ * @param llr 8-bit LLRs
+ * @param nsymbols Number of symbols
+ * @return SRSLTE_SUCCESS if the provided pointers are valid, SRSLTE_ERROR code otherwise
+ */
+SRSRAN_API int srsran_demod_soft_demodulate2_b(srsran_mod_t modulation, const cf_t* symbols, int8_t* llr, int nsymbols);
+
 #endif // SRSRAN_DEMOD_SOFT_H
diff --git a/lib/include/srsran/phy/phch/pdsch_nr.h b/lib/include/srsran/phy/phch/pdsch_nr.h
index 084ca892b..670c4c864 100644
--- a/lib/include/srsran/phy/phch/pdsch_nr.h
+++ b/lib/include/srsran/phy/phch/pdsch_nr.h
@@ -46,6 +46,8 @@ typedef struct SRSRAN_API {
   srsran_sch_nr_args_t sch;
   bool                 measure_evm;
   bool                 measure_time;
+  bool                 disable_zero_re_around_dc; ///< PDSCH NR sets the LLR around the DC to zero to avoid noise
+  uint32_t             nof_zero_re_around_dc; ///< Number of RE to set to zero around DC. It uses default value if 0.
 } srsran_pdsch_nr_args_t;
 
 /**
@@ -66,15 +68,16 @@ typedef struct SRSRAN_API {
   uint32_t             meas_time_us;
   srsran_re_pattern_t  dmrs_re_pattern;
   uint32_t             nof_rvd_re;
+  uint32_t nof_zero_re_around_dc; ///< Sets a number of RE surrounding the center of the resource grid to zero. Set to 0
+                                  ///< for disabling.
 } srsran_pdsch_nr_t;
 
 /**
- *
+ * @brief Groups NR-PDSCH data for reception
  */
 typedef struct {
-  uint8_t* payload;
-  bool     crc;
-  float    evm;
+  srsran_sch_tb_res_nr_t tb[SRSRAN_MAX_TB];         ///< SCH payload
+  float                  evm[SRSRAN_MAX_CODEWORDS]; ///< EVM measurement if configured through arguments
 } srsran_pdsch_res_nr_t;
 
 SRSRAN_API int srsran_pdsch_nr_init_enb(srsran_pdsch_nr_t* q, const srsran_pdsch_nr_args_t* args);
@@ -96,7 +99,7 @@ SRSRAN_API int srsran_pdsch_nr_decode(srsran_pdsch_nr_t*           q,
                                       const srsran_sch_grant_nr_t* grant,
                                       srsran_chest_dl_res_t*       channel,
                                       cf_t*                        sf_symbols[SRSRAN_MAX_PORTS],
-                                      srsran_pdsch_res_nr_t        data[SRSRAN_MAX_TB]);
+                                      srsran_pdsch_res_nr_t*       res);
 
 SRSRAN_API uint32_t srsran_pdsch_nr_rx_info(const srsran_pdsch_nr_t*     q,
                                             const srsran_sch_cfg_nr_t*   cfg,
diff --git a/lib/include/srsran/phy/phch/prach.h b/lib/include/srsran/phy/phch/prach.h
index 866fdca7e..d5ee69d4b 100644
--- a/lib/include/srsran/phy/phch/prach.h
+++ b/lib/include/srsran/phy/phch/prach.h
@@ -125,6 +125,21 @@ typedef struct SRSRAN_API {
   uint32_t sf[5];
 } srsran_prach_sf_config_t;
 
+///@brief Maximum number of subframe number candidates for PRACH NR configuration
+#define PRACH_NR_CFG_MAX_NOF_SF 5
+
+/**
+ * @brief PRACH configuration for NR as described in TS 38.211 Tables 6.3.3.2-2, 6.3.3.2-3 and 6.3.3.2-4
+ */
+typedef struct {
+  uint32_t preamble_format;
+  uint32_t x;
+  uint32_t y;
+  uint32_t subframe_number[PRACH_NR_CFG_MAX_NOF_SF];
+  uint32_t nof_subframe_number;
+  uint32_t starting_symbol; // subframe number
+} prach_nr_config_t;
+
 typedef enum SRSRAN_API {
   SRSRAN_PRACH_SFN_EVEN = 0,
   SRSRAN_PRACH_SFN_ANY,
@@ -169,6 +184,12 @@ SRSRAN_API bool srsran_prach_tti_opportunity_config_tdd(uint32_t  config_idx,
                                                         uint32_t  current_tti,
                                                         uint32_t* prach_idx);
 
+SRSRAN_API const prach_nr_config_t* srsran_prach_nr_get_cfg_fr1_unpaired(uint32_t config_idx);
+
+SRSRAN_API bool srsran_prach_nr_tti_opportunity_fr1_unpaired(uint32_t config_idx, uint32_t current_tti);
+
+SRSRAN_API uint32_t srsran_prach_nr_start_symbol_fr1_unpaired(uint32_t config_idx);
+
 SRSRAN_API uint32_t srsran_prach_f_ra_tdd(uint32_t config_idx,
                                           uint32_t tdd_ul_dl_config,
                                           uint32_t current_tti,
diff --git a/lib/include/srsran/phy/phch/pusch_nr.h b/lib/include/srsran/phy/phch/pusch_nr.h
index 7809f5ee5..d384dac23 100644
--- a/lib/include/srsran/phy/phch/pusch_nr.h
+++ b/lib/include/srsran/phy/phch/pusch_nr.h
@@ -79,18 +79,17 @@ typedef struct SRSRAN_API {
  * @brief Groups NR-PUSCH data for transmission
  */
 typedef struct {
-  uint8_t*              payload; ///< SCH payload
-  srsran_uci_value_nr_t uci;     ///< UCI payload
+  uint8_t*              payload[SRSRAN_MAX_TB]; ///< SCH payload
+  srsran_uci_value_nr_t uci;                    ///< UCI payload
 } srsran_pusch_data_nr_t;
 
 /**
  * @brief Groups NR-PUSCH data for reception
  */
 typedef struct {
-  uint8_t*              payload; ///< SCH payload
-  srsran_uci_value_nr_t uci;     ///< UCI payload
-  bool                  crc;     ///< CRC match
-  float                 evm;     ///< EVM measurement if configured through arguments
+  srsran_sch_tb_res_nr_t tb[SRSRAN_MAX_TB];         ///< SCH payload
+  srsran_uci_value_nr_t  uci;                       ///< UCI payload
+  float                  evm[SRSRAN_MAX_CODEWORDS]; ///< EVM measurement if configured through arguments
 } srsran_pusch_res_nr_t;
 
 SRSRAN_API int srsran_pusch_nr_init_gnb(srsran_pusch_nr_t* q, const srsran_pusch_nr_args_t* args);
diff --git a/lib/include/srsran/phy/phch/sch_cfg_nr.h b/lib/include/srsran/phy/phch/sch_cfg_nr.h
index 129c51dd5..f1c7fcfe3 100644
--- a/lib/include/srsran/phy/phch/sch_cfg_nr.h
+++ b/lib/include/srsran/phy/phch/sch_cfg_nr.h
@@ -34,10 +34,13 @@ typedef struct SRSRAN_API {
 
 typedef struct SRSRAN_API {
   srsran_mod_t mod;
-  uint32_t     N_L; ///< the number of transmission layers that the transport block is mapped onto
-  int          tbs; ///< Payload size, TS 38.212 refers to it as A
-  double       R;   ///< Target LDPC rate
-  int          rv;
+  uint32_t     N_L;      ///< the number of transmission layers that the transport block is mapped onto
+  uint32_t     mcs;      ///< Modulation Code Scheme (MCS) for debug and trace purpose
+  int          tbs;      ///< Payload size, TS 38.212 refers to it as A
+  double       R;        ///< Target LDPC rate
+  int          rv;       ///< Redundancy version
+  int          ndi;      ///< New Data Indicator
+  int          pid;      ///< HARQ Process ID
   uint32_t     nof_re;   ///< Number of available resource elements to send, known as N_RE
   uint32_t     nof_bits; ///< Number of available bits to send, known as G
   uint32_t     cw_idx;
diff --git a/lib/include/srsran/phy/phch/sch_nr.h b/lib/include/srsran/phy/phch/sch_nr.h
index bf6c6b8e2..7d22f21ec 100644
--- a/lib/include/srsran/phy/phch/sch_nr.h
+++ b/lib/include/srsran/phy/phch/sch_nr.h
@@ -41,6 +41,15 @@
 #define SRSRAN_SCH_NR_MAX_NOF_CB_LDPC                                                                                  \
   ((SRSRAN_SLOT_MAX_NOF_BITS_NR + (SRSRAN_LDPC_BG2_MAX_LEN_CB - 1)) / SRSRAN_LDPC_BG2_MAX_LEN_CB)
 
+/**
+ * @brief Groups NR-PUSCH data for reception
+ */
+typedef struct {
+  uint8_t* payload;  ///< SCH payload
+  bool     crc;      ///< CRC match
+  float    avg_iter; ///< Average iterations
+} srsran_sch_tb_res_nr_t;
+
 typedef struct SRSRAN_API {
   srsran_carrier_nr_t carrier;
 
@@ -69,9 +78,10 @@ typedef struct SRSRAN_API {
  * @brief SCH encoder and decoder initialization arguments
  */
 typedef struct SRSRAN_API {
-  bool  disable_simd;
-  bool  decoder_use_flooded;
-  float decoder_scaling_factor;
+  bool     disable_simd;
+  bool     decoder_use_flooded;
+  float    decoder_scaling_factor;
+  uint32_t max_nof_iter; ///< Maximum number of LDPC iterations
 } srsran_sch_nr_args_t;
 
 /**
@@ -161,8 +171,7 @@ SRSRAN_API int srsran_dlsch_nr_decode(srsran_sch_nr_t*        q,
                                       const srsran_sch_cfg_t* sch_cfg,
                                       const srsran_sch_tb_t*  tb,
                                       int8_t*                 e_bits,
-                                      uint8_t*                data,
-                                      bool*                   crc_ok);
+                                      srsran_sch_tb_res_nr_t* res);
 
 SRSRAN_API int srsran_ulsch_nr_encode(srsran_sch_nr_t*        q,
                                       const srsran_sch_cfg_t* cfg,
@@ -174,9 +183,9 @@ SRSRAN_API int srsran_ulsch_nr_decode(srsran_sch_nr_t*        q,
                                       const srsran_sch_cfg_t* sch_cfg,
                                       const srsran_sch_tb_t*  tb,
                                       int8_t*                 e_bits,
-                                      uint8_t*                data,
-                                      bool*                   crc_ok);
+                                      srsran_sch_tb_res_nr_t* res);
 
-SRSRAN_API int srsran_sch_nr_tb_info(const srsran_sch_tb_t* tb, char* str, uint32_t str_len);
+SRSRAN_API int
+srsran_sch_nr_tb_info(const srsran_sch_tb_t* tb, const srsran_sch_tb_res_nr_t* res, char* str, uint32_t str_len);
 
 #endif // SRSRAN_SCH_NR_H
\ No newline at end of file
diff --git a/lib/include/srsran/srslog/bundled/fmt/core.h b/lib/include/srsran/srslog/bundled/fmt/core.h
index 252dabbe8..d676f27e5 100644
--- a/lib/include/srsran/srslog/bundled/fmt/core.h
+++ b/lib/include/srsran/srslog/bundled/fmt/core.h
@@ -1276,6 +1276,10 @@ template <typename T> const T& unwrap(const std::reference_wrapper<T>& v) {
 }
 
 class dynamic_arg_list {
+public:
+  static constexpr std::size_t max_pool_string_size = 256;
+
+private:
   // Workaround for clang's -Wweak-vtables. Unlike for regular classes, for
   // templates it doesn't complain about inability to deduce single translation
   // unit for placing vtable. So storage_node_base is made a fake template.
@@ -1284,6 +1288,10 @@ class dynamic_arg_list {
     std::unique_ptr<node<>> next;
   };
 
+  // Pool storage allocation functions.
+  static void *allocate_from_pool(std::size_t sz);
+  static void free_from_pool(void *ptr);
+
   template <typename T> struct typed_node : node<> {
     T value;
 
@@ -1295,9 +1303,35 @@ class dynamic_arg_list {
         : value(arg.data(), arg.size()) {}
   };
 
+  struct pooled_node : node<> {
+    std::array<char, max_pool_string_size> value;
+
+    static void* operator new(std::size_t sz) {
+      return allocate_from_pool(sz);
+    }
+    static void operator delete(void* ptr) {
+      free_from_pool(ptr);
+    }
+
+    pooled_node(const char *str, std::size_t sz) {
+      FMT_ASSERT(sz < value.size(), "String is too big");
+      std::copy(str, str + sz, value.begin());
+    }
+  };
+
   std::unique_ptr<node<>> head_;
 
  public:
+  static constexpr std::size_t max_pool_node_size = sizeof(pooled_node);
+
+  const char *push_small_string(const char *str, std::size_t sz) {
+    auto new_node  = std::unique_ptr<pooled_node>(new pooled_node(str, sz));
+    auto& value    = new_node->value;
+    new_node->next = std::move(head_);
+    head_          = std::move(new_node);
+    return value.data();
+  }
+
   template <typename T, typename Arg> const T& push(const Arg& arg) {
     auto new_node = std::unique_ptr<typed_node<T>>(new typed_node<T>(arg));
     auto& value = new_node->value;
@@ -1541,11 +1575,24 @@ class dynamic_format_arg_store
       std::string result = fmt::vformat("{} and {} and {}", store);
     \endrst
   */
-  template <typename T> void push_back(const T& arg) {
-    if (detail::const_check(need_copy<T>::value))
-      emplace_arg(dynamic_args_.push<stored_type<T>>(arg));
-    else
+  template <typename T,
+      typename std::enable_if<detail::is_string<typename std::decay<T>::type>::value, int>::type = 0>
+   void push_back(const T& arg) {
+    fmt::string_view view(arg);
+    if (view.size() + 1 < dynamic_args_.max_pool_string_size) {
+      emplace_arg(dynamic_args_.push_small_string(view.data(), view.size() + 1));
+    } else {
+      emplace_arg(dynamic_args_.push<stored_type<T> >(arg));
+    }
+  }
+  template <typename T,
+      typename std::enable_if<!detail::is_string<typename std::decay<T>::type>::value, int>::type = 0>
+  void push_back(const T& arg) {
+    if (detail::const_check(need_copy<T>::value)) {
+      emplace_arg(dynamic_args_.push<stored_type<T> >(arg));
+    } else {
       emplace_arg(detail::unwrap(arg));
+    }
   }
 
   /**
diff --git a/lib/include/srsran/srslog/srslog.h b/lib/include/srsran/srslog/srslog.h
index 70b6d1464..22f245dd9 100644
--- a/lib/include/srsran/srslog/srslog.h
+++ b/lib/include/srsran/srslog/srslog.h
@@ -107,6 +107,9 @@ template <typename T, typename... Args>
 inline T& fetch_logger(const std::string& id, Args&&... args)
 {
   static_assert(detail::is_logger<T>::value, "T should be a logger type");
+  if (auto *logger = find_logger<T>(id)) {
+    return *logger;
+  }
   auto logger = detail::make_any<T>(id, std::forward<Args>(args)...);
   detail::any* p = detail::fetch_logger(id, std::move(logger));
 
diff --git a/lib/include/srsran/test/ue_test_interfaces.h b/lib/include/srsran/test/ue_test_interfaces.h
index 4bceda9fd..d9c39f409 100644
--- a/lib/include/srsran/test/ue_test_interfaces.h
+++ b/lib/include/srsran/test/ue_test_interfaces.h
@@ -35,15 +35,13 @@ class stack_test_dummy : public stack_interface_rrc
 public:
   stack_test_dummy() {}
 
-  srsran::tti_point get_current_tti() override
-  {
-    return srsran::tti_point{task_sched.get_timer_handler()->get_cur_time() % 10240};
-  }
+  srsran::tti_point get_current_tti() override { return srsran::tti_point{tti % 10240}; }
 
   // Testing utility functions
   void run_tti()
   {
     // update clock and run internal tasks
+    tti++;
     task_sched.tic();
 
     task_sched.run_pending_tasks();
@@ -52,6 +50,7 @@ public:
   // run pending tasks without updating timers
   void run_pending_tasks() { task_sched.run_pending_tasks(); }
 
+  uint32_t               tti = 0;
   srsran::task_scheduler task_sched{512, 100};
 };
 
diff --git a/lib/include/srsran/upper/pdcp_entity_lte.h b/lib/include/srsran/upper/pdcp_entity_lte.h
index 270564b1e..897b245a1 100644
--- a/lib/include/srsran/upper/pdcp_entity_lte.h
+++ b/lib/include/srsran/upper/pdcp_entity_lte.h
@@ -92,7 +92,7 @@ private:
 
   uint32_t                                   count = 0;
   uint32_t                                   bytes = 0;
-  uint32_t                                   fms   = 0;
+  uint32_t                                   fms   = 0; // SN of the first missing PDCP SDU
   uint32_t                                   lms   = 0;
   srsran::circular_array<sdu_data, capacity> sdus;
 };
diff --git a/lib/include/srsran/upper/rlc_am_lte.h b/lib/include/srsran/upper/rlc_am_lte.h
index af76cec56..88dd11137 100644
--- a/lib/include/srsran/upper/rlc_am_lte.h
+++ b/lib/include/srsran/upper/rlc_am_lte.h
@@ -354,6 +354,9 @@ private:
 
     // Mutexes
     std::mutex mutex;
+
+    // default to RLC SDU queue length
+    const uint32_t MAX_SDUS_PER_RLC_PDU = RLC_TX_QUEUE_LEN;
   };
 
   // Receiver sub-class
diff --git a/lib/src/asn1/CMakeLists.txt b/lib/src/asn1/CMakeLists.txt
index aa7c6a8a8..3415178e0 100644
--- a/lib/src/asn1/CMakeLists.txt
+++ b/lib/src/asn1/CMakeLists.txt
@@ -58,7 +58,7 @@ INSTALL(TARGETS rrc_asn1 DESTINATION ${LIBRARY_DIR})
 
 # S1AP ASN1 lib
 add_library(s1ap_asn1 STATIC
-        s1ap.cc)
+            s1ap.cc s1ap_utils.cc)
 target_compile_options(s1ap_asn1 PRIVATE "-Os")
 target_link_libraries(s1ap_asn1 asn1_utils srsran_common)
 INSTALL(TARGETS s1ap_asn1 DESTINATION ${LIBRARY_DIR})
diff --git a/lib/src/asn1/rrc_nr_utils.cc b/lib/src/asn1/rrc_nr_utils.cc
index bd941d66b..84af9957c 100644
--- a/lib/src/asn1/rrc_nr_utils.cc
+++ b/lib/src/asn1/rrc_nr_utils.cc
@@ -75,6 +75,15 @@ logical_channel_config_t make_mac_logical_channel_cfg_t(uint8_t lcid, const lc_c
   return logical_channel_config;
 }
 
+bool make_mac_phr_cfg_t(const phr_cfg_s& asn1_type, phr_cfg_nr_t* phr_cfg_nr)
+{
+  phr_cfg_nr->extended             = asn1_type.ext;
+  phr_cfg_nr->periodic_timer       = asn1_type.phr_periodic_timer.to_number();
+  phr_cfg_nr->prohibit_timer       = asn1_type.phr_prohibit_timer.to_number();
+  phr_cfg_nr->tx_pwr_factor_change = asn1_type.phr_tx_pwr_factor_change.to_number();
+  return true;
+}
+
 rach_nr_cfg_t make_mac_rach_cfg(const rach_cfg_common_s& asn1_type)
 {
   rach_nr_cfg_t rach_nr_cfg                = {};
@@ -268,6 +277,42 @@ bool make_phy_tdd_cfg(const tdd_ul_dl_cfg_common_s& tdd_ul_dl_cfg_common,
   srsran_tdd_config_nr.pattern1.nof_ul_symbols = tdd_ul_dl_cfg_common.pattern1.nrof_ul_symbols;
   // Copy and return struct
   *in_srsran_tdd_config_nr = srsran_tdd_config_nr;
+
+  if (not tdd_ul_dl_cfg_common.pattern2_present) {
+    return true;
+  }
+
+  switch (tdd_ul_dl_cfg_common.pattern2.dl_ul_tx_periodicity) {
+    case tdd_ul_dl_pattern_s::dl_ul_tx_periodicity_opts::ms1:
+      srsran_tdd_config_nr.pattern2.period_ms = 1;
+      break;
+    case tdd_ul_dl_pattern_s::dl_ul_tx_periodicity_opts::ms2:
+      srsran_tdd_config_nr.pattern2.period_ms = 2;
+      break;
+    case tdd_ul_dl_pattern_s::dl_ul_tx_periodicity_opts::ms5:
+      srsran_tdd_config_nr.pattern2.period_ms = 5;
+      break;
+    case tdd_ul_dl_pattern_s::dl_ul_tx_periodicity_opts::ms10:
+      srsran_tdd_config_nr.pattern2.period_ms = 10;
+      break;
+
+    case tdd_ul_dl_pattern_s::dl_ul_tx_periodicity_opts::ms1p25:
+    case tdd_ul_dl_pattern_s::dl_ul_tx_periodicity_opts::ms0p5:
+    case tdd_ul_dl_pattern_s::dl_ul_tx_periodicity_opts::ms0p625:
+    case tdd_ul_dl_pattern_s::dl_ul_tx_periodicity_opts::ms2p5:
+    default:
+      asn1::log_warning("Invalid option for pattern2 dl_ul_tx_periodicity_opts %s",
+                        tdd_ul_dl_cfg_common.pattern2.dl_ul_tx_periodicity.to_string());
+      return false;
+  }
+
+  srsran_tdd_config_nr.pattern2.nof_dl_slots   = tdd_ul_dl_cfg_common.pattern2.nrof_dl_slots;
+  srsran_tdd_config_nr.pattern2.nof_dl_symbols = tdd_ul_dl_cfg_common.pattern2.nrof_dl_symbols;
+  srsran_tdd_config_nr.pattern2.nof_ul_slots   = tdd_ul_dl_cfg_common.pattern2.nrof_ul_slots;
+  srsran_tdd_config_nr.pattern2.nof_ul_symbols = tdd_ul_dl_cfg_common.pattern2.nrof_ul_symbols;
+  // Copy and return struct
+  *in_srsran_tdd_config_nr = srsran_tdd_config_nr;
+
   return true;
 }
 
diff --git a/lib/src/asn1/rrc_utils.cc b/lib/src/asn1/rrc_utils.cc
index 3f2a3ef03..a908d6ffe 100644
--- a/lib/src/asn1/rrc_utils.cc
+++ b/lib/src/asn1/rrc_utils.cc
@@ -548,6 +548,33 @@ void set_phy_cfg_t_dedicated_cfg(phy_cfg_t* cfg, const asn1::rrc::phys_cfg_ded_s
     // TODO
   }
 
+  if (asn1_type.cqi_report_cfg_r10.is_present()) {
+    // Parse R10 periodic CQI configuration
+    cfg->dl_cfg.cqi_report.periodic_configured =
+        asn1_type.cqi_report_cfg_r10->cqi_report_periodic_r10.type() == asn1::rrc::setup_e::setup;
+    if (cfg->dl_cfg.cqi_report.periodic_configured) {
+      const auto& cqi_report_periodic = asn1_type.cqi_report_cfg_r10->cqi_report_periodic_r10.setup();
+      cfg->ul_cfg.pucch.n_pucch_2     = cqi_report_periodic.cqi_pucch_res_idx_r10;
+      cfg->ul_cfg.pucch.simul_cqi_ack = cqi_report_periodic.simul_ack_nack_and_cqi;
+      cfg->dl_cfg.cqi_report.pmi_idx  = cqi_report_periodic.cqi_pmi_cfg_idx;
+      cfg->dl_cfg.cqi_report.format_is_subband =
+          cqi_report_periodic.cqi_format_ind_periodic_r10.type().value ==
+          asn1::rrc::cqi_report_periodic_r10_c::setup_s_::cqi_format_ind_periodic_r10_c_::types::subband_cqi_r10;
+      if (cfg->dl_cfg.cqi_report.format_is_subband) {
+        cfg->dl_cfg.cqi_report.subband_size = cqi_report_periodic.cqi_format_ind_periodic_r10.subband_cqi_r10().k;
+      }
+      if (cqi_report_periodic.ri_cfg_idx_present) {
+        cfg->dl_cfg.cqi_report.ri_idx         = cqi_report_periodic.ri_cfg_idx;
+        cfg->dl_cfg.cqi_report.ri_idx_present = true;
+      } else {
+        cfg->dl_cfg.cqi_report.ri_idx_present = false;
+      }
+    } else {
+      cfg->ul_cfg.pucch.n_pucch_2     = 0;
+      cfg->ul_cfg.pucch.simul_cqi_ack = false;
+    }
+  }
+
   if (asn1_type.cqi_report_cfg_present) {
     if (asn1_type.cqi_report_cfg.cqi_report_periodic_present) {
       cfg->dl_cfg.cqi_report.periodic_configured =
diff --git a/lib/src/asn1/s1ap.cc b/lib/src/asn1/s1ap.cc
index 37c301df1..c7bf0f459 100644
--- a/lib/src/asn1/s1ap.cc
+++ b/lib/src/asn1/s1ap.cc
@@ -14502,6 +14502,7 @@ std::string erab_modify_resp_ies_o::value_c::types_opts::to_string() const
 }
 
 template struct asn1::s1ap::protocol_ie_field_s<erab_modify_resp_ies_o>;
+template struct asn1::s1ap::protocol_ie_single_container_s<asn1::s1ap::erab_failedto_setup_item_ho_req_ack_ies_o>;
 
 erab_modify_resp_ies_container::erab_modify_resp_ies_container() :
   mme_ue_s1ap_id(0, crit_e::ignore),
diff --git a/lib/src/asn1/s1ap_utils.cc b/lib/src/asn1/s1ap_utils.cc
new file mode 100644
index 000000000..faed183f4
--- /dev/null
+++ b/lib/src/asn1/s1ap_utils.cc
@@ -0,0 +1,47 @@
+/**
+ *
+ * \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/asn1/s1ap_utils.h"
+#include "srsran/asn1/s1ap.h"
+
+namespace asn1 {
+namespace s1ap {
+
+template <>
+uint32_t get_obj_id<erab_item_s>(const erab_item_s& obj)
+{
+  return obj.erab_id;
+}
+
+template <>
+uint32_t get_obj_id<protocol_ie_single_container_s<erab_to_be_setup_item_ctxt_su_req_ies_o> >(
+    const protocol_ie_single_container_s<erab_to_be_setup_item_ctxt_su_req_ies_o>& obj)
+{
+  return obj.value.erab_to_be_setup_item_ctxt_su_req().erab_id;
+}
+
+template <>
+uint32_t get_obj_id<protocol_ie_single_container_s<erab_to_be_setup_item_bearer_su_req_ies_o> >(
+    const protocol_ie_single_container_s<erab_to_be_setup_item_bearer_su_req_ies_o>& obj)
+{
+  return obj.value.erab_to_be_setup_item_bearer_su_req().erab_id;
+}
+
+template <>
+uint32_t get_obj_id<protocol_ie_single_container_s<erab_to_be_modified_item_bearer_mod_req_ies_o> >(
+    const protocol_ie_single_container_s<erab_to_be_modified_item_bearer_mod_req_ies_o>& obj)
+{
+  return obj.value.erab_to_be_modified_item_bearer_mod_req().erab_id;
+}
+
+} // namespace s1ap
+} // namespace asn1
diff --git a/lib/src/mac/mac_rar_pdu_nr.cc b/lib/src/mac/mac_rar_pdu_nr.cc
index de7a0cd9d..cf4fa117a 100644
--- a/lib/src/mac/mac_rar_pdu_nr.cc
+++ b/lib/src/mac/mac_rar_pdu_nr.cc
@@ -31,7 +31,7 @@ extern "C" {
 namespace srsran {
 
 mac_rar_subpdu_nr::mac_rar_subpdu_nr(mac_rar_pdu_nr* parent_) :
-  parent(parent_), logger(srslog::fetch_basic_logger("MAC"))
+  parent(parent_), logger(srslog::fetch_basic_logger("MAC-NR"))
 {}
 
 // Return true if subPDU could be parsed correctly, false otherwise
@@ -156,7 +156,7 @@ std::string mac_rar_subpdu_nr::to_string()
   return ss.str();
 }
 
-mac_rar_pdu_nr::mac_rar_pdu_nr() : logger(srslog::fetch_basic_logger("MAC")) {}
+mac_rar_pdu_nr::mac_rar_pdu_nr() : logger(srslog::fetch_basic_logger("MAC-NR")) {}
 
 bool mac_rar_pdu_nr::pack()
 {
diff --git a/lib/src/mac/mac_sch_pdu_nr.cc b/lib/src/mac/mac_sch_pdu_nr.cc
index ff3efc34c..8c5e4a811 100644
--- a/lib/src/mac/mac_sch_pdu_nr.cc
+++ b/lib/src/mac/mac_sch_pdu_nr.cc
@@ -76,7 +76,7 @@ int32_t mac_sch_subpdu_nr::read_subheader(const uint8_t* ptr)
     }
     sdu = (uint8_t*)ptr;
   } else {
-    srslog::fetch_basic_logger("MAC").warning("Invalid LCID (%d) in MAC PDU", lcid);
+    srslog::fetch_basic_logger("MAC-NR").warning("Invalid LCID (%d) in MAC PDU", lcid);
     return SRSRAN_ERROR;
   }
   return header_length;
@@ -302,14 +302,14 @@ void mac_sch_pdu_nr::pack()
   }
 }
 
-void mac_sch_pdu_nr::unpack(const uint8_t* payload, const uint32_t& len)
+int mac_sch_pdu_nr::unpack(const uint8_t* payload, const uint32_t& len)
 {
   uint32_t offset = 0;
   while (offset < len) {
     mac_sch_subpdu_nr sch_pdu(this);
     if (sch_pdu.read_subheader(payload + offset) == SRSRAN_ERROR) {
-      logger.error("Error parsing NR MAC PDU (len=%d, offset=%d)\n", len, offset);
-      return;
+      logger.error("Malformed MAC PDU (len=%d, offset=%d)\n", len, offset);
+      return SRSRAN_ERROR;
     }
     offset += sch_pdu.get_total_length();
     if (sch_pdu.get_lcid() == mac_sch_subpdu_nr::PADDING) {
@@ -321,8 +321,11 @@ void mac_sch_pdu_nr::unpack(const uint8_t* payload, const uint32_t& len)
     subpdus.push_back(sch_pdu);
   }
   if (offset != len) {
-    logger.error("Error parsing NR MAC PDU (len=%d, offset=%d)\n", len, offset);
+    logger.error("Malformed MAC PDU (len=%d, offset=%d)\n", len, offset);
+    return SRSRAN_ERROR;
   }
+
+  return SRSRAN_SUCCESS;
 }
 
 uint32_t mac_sch_pdu_nr::get_num_subpdus()
diff --git a/lib/src/mac/pdu_queue.cc b/lib/src/mac/pdu_queue.cc
index 0fb9339bc..30007045c 100644
--- a/lib/src/mac/pdu_queue.cc
+++ b/lib/src/mac/pdu_queue.cc
@@ -21,6 +21,7 @@
 
 #include "srsran/mac/pdu_queue.h"
 #include "srsran/common/log_helper.h"
+#include "srsran/phy/utils/debug.h"
 
 namespace srsran {
 
diff --git a/lib/src/phy/ch_estimation/dmrs_pdcch.c b/lib/src/phy/ch_estimation/dmrs_pdcch.c
index a4bdb6263..c77bcf052 100644
--- a/lib/src/phy/ch_estimation/dmrs_pdcch.c
+++ b/lib/src/phy/ch_estimation/dmrs_pdcch.c
@@ -32,12 +32,26 @@
 /// per frequency resource.
 #define NOF_PILOTS_X_FREQ_RES 18
 
+///@brief Maximum number of pilots in a PDCCH candidate location
+#define DMRS_PDCCH_MAX_NOF_PILOTS_CANDIDATE                                                                            \
+  ((SRSRAN_NRE / 3) * (1U << (SRSRAN_SEARCH_SPACE_NOF_AGGREGATION_LEVELS_NR - 1U)) * 6U)
+
 #define DMRS_PDCCH_INFO_TX(...) INFO("PDCCH DMRS Tx: " __VA_ARGS__)
 #define DMRS_PDCCH_INFO_RX(...) INFO("PDCCH DMRS Rx: " __VA_ARGS__)
 #define DMRS_PDCCH_DEBUG_RX(...) DEBUG("PDCCH DMRS Rx: " __VA_ARGS__)
 
 /// @brief Enables interpolation at CCE frequency bandwidth to avoid interference with adjacent PDCCH DMRS
 #define DMRS_PDCCH_INTERPOLATE_GROUP 1
+
+///@brief Enables synchronization error pre-compensation before group interpolator. It should decrease EVM in expense of
+/// computing complexity.
+#define DMRS_PDCCH_SYNC_PRECOMPENSATE_INTERP 0
+
+///@brief Enables synchronization error pre-compensation before candidate measurement. It improves detection probability
+/// in expense of computing complexity.
+#define DMRS_PDCCH_SYNC_PRECOMPENSATE_MEAS 1
+
+///@brief Enables/Disables smoothing filter
 #define DMRS_PDCCH_SMOOTH_FILTER 0
 
 static uint32_t dmrs_pdcch_get_cinit(uint32_t slot_idx, uint32_t symbol_idx, uint32_t n_id)
@@ -371,21 +385,32 @@ int srsran_dmrs_pdcch_estimate(srsran_dmrs_pdcch_estimator_t* q,
   uint32_t group_size  = NOF_PILOTS_X_FREQ_RES / q->coreset.duration;
   for (uint32_t l = 0; l < q->coreset.duration; l++) {
     for (uint32_t j = 0; j < group_count; j++) {
-#if DMRS_PDCCH_SMOOTH_FILTER
       cf_t tmp[NOF_PILOTS_X_FREQ_RES];
 
+      // Copy group into temporal vector
+      srsran_vec_cf_copy(tmp, &q->lse[l][j * group_size], group_size);
+
+#if DMRS_PDCCH_SYNC_PRECOMPENSATE_INTERP
+      float sync_err = srsran_vec_estimate_frequency(tmp, group_size);
+      if (isnormal(sync_err)) {
+        srsran_vec_apply_cfo(tmp, sync_err, tmp, group_size);
+      }
+#endif // DMRS_PDCCH_SYNC_PRECOMPENSATION
+
+#if DMRS_PDCCH_SMOOTH_FILTER
       // Smoothing filter group
-      srsran_conv_same_cf(&q->lse[l][j * group_size], q->filter, tmp, group_size, q->filter_len);
-
-      srsran_interp_linear_offset(
-          &q->interpolator, tmp, &q->ce[SRSRAN_NRE * q->coreset_bw * l + j * group_size * 4], 1, 3);
-#else  // DMRS_PDCCH_SMOOTH_FILTER
-      srsran_interp_linear_offset(&q->interpolator,
-                                  &q->lse[l][j * group_size],
-                                  &q->ce[SRSRAN_NRE * q->coreset_bw * l + j * group_size * 4],
-                                  1,
-                                  3);
+      srsran_conv_same_cf(tmp, q->filter, tmp, group_size, q->filter_len);
 #endif // DMRS_PDCCH_SMOOTH_FILTER
+
+      // Interpolate group
+      cf_t* dst = &q->ce[SRSRAN_NRE * q->coreset_bw * l + j * group_size * 4];
+      srsran_interp_linear_offset(&q->interpolator, tmp, dst, 1, 3);
+
+#if DMRS_PDCCH_SYNC_PRECOMPENSATE_INTERP
+      if (isnormal(sync_err)) {
+        srsran_vec_apply_cfo(dst, -sync_err / 4, dst, group_size * 4);
+      }
+#endif // DMRS_PDCCH_SYNC_PRECOMPENSATION
     }
   }
 #else  // DMRS_PDCCH_INTERPOLATE_GROUP
@@ -421,51 +446,71 @@ int srsran_dmrs_pdcch_get_measure(const srsran_dmrs_pdcch_estimator_t* q,
   uint32_t pilot_idx  = (dci_location->ncce * 18) / q->coreset.duration;
   uint32_t nof_pilots = (L * 18) / q->coreset.duration;
 
-  float rsrp                              = 0.0f;
-  float epre                              = 0.0f;
-  float cfo                               = 0.0f;
-  float sync_err                          = 0.0f;
-  cf_t  corr[SRSRAN_CORESET_DURATION_MAX] = {};
+  // Initialise measurements
+  float rsrp                              = 0.0f; //< Averages linear RSRP
+  float epre                              = 0.0f; //< Averages linear EPRE
+  float cfo_avg_Hz                        = 0.0f; //< Averages CFO in Radians
+  float sync_err_avg                      = 0.0f; //< Averages synchronization
+  cf_t  corr[SRSRAN_CORESET_DURATION_MAX] = {};   //< Saves correlation for the different symbols
+
+  // Iterate the CORESET duration
   for (uint32_t l = 0; l < q->coreset.duration; l++) {
     if (SRSRAN_DEBUG_ENABLED && srsran_verbose >= SRSRAN_VERBOSE_DEBUG && !handler_registered) {
       DMRS_PDCCH_DEBUG_RX("Measuring PDCCH l=%d; lse=", l);
       srsran_vec_fprint_c(stdout, &q->lse[l][pilot_idx], nof_pilots);
     }
 
+    // Measure synchronization error and accumulate for average
+    float tmp_sync_err = srsran_vec_estimate_frequency(&q->lse[l][pilot_idx], nof_pilots);
+    sync_err_avg += tmp_sync_err;
+
+#if DMRS_PDCCH_SYNC_PRECOMPENSATE_MEAS
+    cf_t tmp[DMRS_PDCCH_MAX_NOF_PILOTS_CANDIDATE];
+
+    // Pre-compensate synchronization error
+    srsran_vec_apply_cfo(&q->lse[l][pilot_idx], tmp_sync_err, tmp, nof_pilots);
+#else  // DMRS_PDCCH_SYNC_PRECOMPENSATE_MEAS
+    const cf_t* tmp = &q->lse[l][pilot_idx];
+#endif // DMRS_PDCCH_SYNC_PRECOMPENSATE_MEAS
+
     // Correlate DMRS
-    corr[l] = srsran_vec_acc_cc(&q->lse[l][pilot_idx], nof_pilots) / (float)nof_pilots;
+    corr[l] = srsran_vec_acc_cc(tmp, nof_pilots) / (float)nof_pilots;
 
     // Measure symbol RSRP
     rsrp += __real__ corr[l] * __real__ corr[l] + __imag__ corr[l] * __imag__ corr[l];
 
     // Measure symbol EPRE
-    epre += srsran_vec_avg_power_cf(&q->lse[l][pilot_idx], nof_pilots);
+    epre += srsran_vec_avg_power_cf(tmp, nof_pilots);
 
     // Measure CFO only from the second and third symbols
     if (l != 0) {
-      cfo += cargf(corr[l] * conjf(corr[l - 1]));
+      // Calculates the time between the previous and the current symbol
+      float Ts = srsran_symbol_distance_s(l - 1, l, q->carrier.numerology);
+      if (isnormal(Ts)) {
+        // Compute phase difference between symbols and convert to Hz
+        cfo_avg_Hz += cargf(corr[l] * conjf(corr[l - 1])) / (2.0f * (float)M_PI * Ts);
+      }
     }
-
-    // Measure synchronization error
-    sync_err += srsran_vec_estimate_frequency(&q->lse[l][pilot_idx], nof_pilots);
   }
 
+  // Store results
+  measure->rsrp = rsrp / (float)q->coreset.duration;
+  measure->epre = epre / (float)q->coreset.duration;
   if (q->coreset.duration > 1) {
-    cfo /= (float)(q->coreset.duration - 1);
+    // Protected zero division
+    measure->cfo_hz /= (float)(q->coreset.duration - 1);
+  } else {
+    // There are not enough symbols for computing CFO, set to NAN
+    measure->cfo_hz = NAN;
   }
-
-  // Symbol time, including cyclic prefix. Required for CFO estimation
-  float Ts = (71.3541666667f / (float)(1 << q->carrier.numerology));
-
-  measure->rsrp   = rsrp / (float)q->coreset.duration;
-  measure->epre   = epre / (float)q->coreset.duration;
-  measure->cfo_hz = cfo / (2.0f * (float)M_PI * Ts);
   measure->sync_error_us =
-      sync_err / (4.0e-6f * (float)q->coreset.duration * SRSRAN_SUBC_SPACING_NR(q->carrier.numerology));
+      sync_err_avg / (4.0e-6f * (float)q->coreset.duration * SRSRAN_SUBC_SPACING_NR(q->carrier.numerology));
 
+  // Convert power measurements into logarithmic scale
   measure->rsrp_dBfs = srsran_convert_power_to_dB(measure->rsrp);
   measure->epre_dBfs = srsran_convert_power_to_dB(measure->epre);
 
+  // Store DMRS correlation
   if (isnormal(measure->rsrp) && isnormal(measure->epre)) {
     measure->norm_corr = measure->rsrp / measure->epre;
   } else {
diff --git a/lib/src/phy/ch_estimation/dmrs_sch.c b/lib/src/phy/ch_estimation/dmrs_sch.c
index 5920cb40b..f77377390 100644
--- a/lib/src/phy/ch_estimation/dmrs_sch.c
+++ b/lib/src/phy/ch_estimation/dmrs_sch.c
@@ -28,6 +28,26 @@
 #define SRSRAN_DMRS_SCH_TYPEA_SINGLE_DURATION_MIN 3
 #define SRSRAN_DMRS_SCH_TYPEA_DOUBLE_DURATION_MIN 4
 
+/**
+ * @brief Set to 1 for synchronization error pre-compensation before interpolator
+ */
+#define DMRS_SCH_SYNC_PRECOMPENSATE 1
+
+/**
+ * @brief Set to 1 for CFO error pre-compensation before interpolator
+ */
+#define DMRS_SCH_CFO_PRECOMPENSATE 1
+
+/**
+ * @brief Set Smoothing filter length, set to 0 for disabling. The recommended value is 5.
+ */
+#define DMRS_SCH_SMOOTH_FILTER_LEN 5
+
+/**
+ * @brief Set smoothing filter (gaussian) standard deviation
+ */
+#define DMRS_SCH_SMOOTH_FILTER_STDDEV 2
+
 int srsran_dmrs_sch_cfg_to_str(const srsran_dmrs_sch_cfg_t* cfg, char* msg, uint32_t max_len)
 {
   int type           = (int)cfg->type + 1;
@@ -512,10 +532,22 @@ int srsran_dmrs_sch_init(srsran_dmrs_sch_t* q, bool is_rx)
     return SRSRAN_ERROR_INVALID_INPUTS;
   }
 
+  SRSRAN_MEM_ZERO(q, srsran_dmrs_sch_t, 1);
+
   if (is_rx) {
     q->is_rx = true;
   }
 
+#if DMRS_SCH_SMOOTH_FILTER_LEN
+  if (q->filter == NULL) {
+    q->filter = srsran_vec_f_malloc(DMRS_SCH_SMOOTH_FILTER_LEN);
+    if (q->filter == NULL) {
+      return SRSRAN_ERROR;
+    }
+    srsran_chest_set_smooth_filter_gauss(q->filter, DMRS_SCH_SMOOTH_FILTER_LEN - 1, 2);
+  }
+#endif // DMRS_SCH_SMOOTH_FILTER_LEN
+
   return SRSRAN_SUCCESS;
 }
 
@@ -533,6 +565,9 @@ void srsran_dmrs_sch_free(srsran_dmrs_sch_t* q)
   if (q->temp) {
     free(q->temp);
   }
+  if (q->filter) {
+    free(q->filter);
+  }
 
   SRSRAN_MEM_ZERO(q, srsran_dmrs_sch_t, 1);
 }
@@ -731,9 +766,16 @@ int srsran_dmrs_sch_estimate(srsran_dmrs_sch_t*           q,
     return SRSRAN_ERROR;
   }
 
+  // Get DMRS reserved RE pattern
+  srsran_re_pattern_t dmrs_pattern = {};
+  if (srsran_dmrs_sch_rvd_re_pattern(dmrs_cfg, grant, &dmrs_pattern) < SRSRAN_SUCCESS) {
+    ERROR("Error computing DMRS Reserved Re pattern");
+    return SRSRAN_ERROR;
+  }
+
   uint32_t nof_pilots_x_symbol = 0;
 
-  // Iterate symbols
+  // Iterate symbols and extract LSE estimates
   for (uint32_t i = 0; i < nof_symbols; i++) {
     uint32_t l = symbols[i]; // Symbol index inside the slot
 
@@ -749,6 +791,27 @@ int srsran_dmrs_sch_estimate(srsran_dmrs_sch_t*           q,
     }
   }
 
+  // Estimate average synchronization error
+  float dmrs_stride = (dmrs_cfg->type == srsran_dmrs_sch_type_1) ? 2 : 3;
+  float sync_err    = 0.0f;
+  for (uint32_t i = 0; i < nof_symbols; i++) {
+    sync_err += srsran_vec_estimate_frequency(&q->pilot_estimates[nof_pilots_x_symbol * i], nof_pilots_x_symbol);
+  }
+  sync_err /= (float)nof_symbols;
+  chest_res->sync_error = sync_err / (dmrs_stride * SRSRAN_SUBC_SPACING_NR(q->carrier.numerology));
+
+#if DMRS_SCH_SYNC_PRECOMPENSATE
+  // Pre-compensate synchronization error
+  if (isnormal(sync_err)) {
+    for (uint32_t i = 0; i < nof_symbols; i++) {
+      srsran_vec_apply_cfo(&q->pilot_estimates[nof_pilots_x_symbol * i],
+                           sync_err,
+                           &q->pilot_estimates[nof_pilots_x_symbol * i],
+                           nof_pilots_x_symbol);
+    }
+  }
+#endif // DMRS_SCH_SYNC_ERROR_PRECOMPENSATE
+
   // Perform Power measurements
   float rsrp                              = 0.0f;
   float epre                              = 0.0f;
@@ -783,6 +846,37 @@ int srsran_dmrs_sch_estimate(srsran_dmrs_sch_t*           q,
   }
   chest_res->cfo = cfo_avg;
 
+#if DMRS_SCH_CFO_PRECOMPENSATE
+  // Pre-compensate CFO
+  cf_t cfo_correction[SRSRAN_NSYMB_PER_SLOT_NR] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
+  if (isnormal(cfo_avg)) {
+    // Calculate phase of the first OFDM symbol (l = 0)
+    float arg0 =
+        cargf(corr[0]) - 2.0f * M_PI * srsran_symbol_distance_s(0, symbols[0], q->carrier.numerology) * cfo_avg;
+
+    // Calculate CFO corrections
+    for (uint32_t l = 0; l < SRSRAN_NSYMB_PER_SLOT_NR; l++) {
+      float arg         = arg0 + 2.0f * M_PI * cfo_avg * srsran_symbol_distance_s(0, l, q->carrier.numerology);
+      cfo_correction[l] = cexpf(I * arg);
+    }
+
+    // Remove CFO phases
+    for (uint32_t i = 0; i < nof_symbols; i++) {
+      uint32_t l = symbols[i];
+      srsran_vec_sc_prod_ccc(&q->pilot_estimates[nof_pilots_x_symbol * i],
+                             conjf(cfo_correction[l]),
+                             &q->pilot_estimates[nof_pilots_x_symbol * i],
+                             nof_pilots_x_symbol);
+    }
+  }
+#endif // DMRS_SCH_CFO_PRECOMPENSATE
+
+  INFO("PDSCH-DMRS: RSRP=%+.2fdB EPRE=%+.2fdB CFO=%+.0fHz Sync=%.3fus",
+       chest_res->rsrp_dbm,
+       srsran_convert_power_to_dB(epre),
+       cfo_avg,
+       chest_res->sync_error * 1e6);
+
   // Average over time, only if more than one DMRS symbol
   for (uint32_t i = 1; i < nof_symbols; i++) {
     srsran_vec_sum_ccc(
@@ -792,6 +886,12 @@ int srsran_dmrs_sch_estimate(srsran_dmrs_sch_t*           q,
     srsran_vec_sc_prod_cfc(q->pilot_estimates, 1.0f / (float)nof_symbols, q->pilot_estimates, nof_pilots_x_symbol);
   }
 
+#if DMRS_SCH_SMOOTH_FILTER_LEN
+  // Apply smoothing filter
+  srsran_conv_same_cf(
+      q->pilot_estimates, q->filter, q->pilot_estimates, nof_pilots_x_symbol, DMRS_SCH_SMOOTH_FILTER_LEN);
+#endif // DMRS_SCH_SMOOTH_FILTER_LEN
+
   // Frequency domain interpolate
   uint32_t nof_re_x_symbol =
       (dmrs_cfg->type == srsran_dmrs_sch_type_1) ? nof_pilots_x_symbol * 2 : nof_pilots_x_symbol * 3;
@@ -816,23 +916,31 @@ int srsran_dmrs_sch_estimate(srsran_dmrs_sch_t*           q,
     srsran_interp_linear_offset(&q->interpolator_type2, q->pilot_estimates, ce, delta, 3 - delta);
   }
 
+#if DMRS_SCH_SYNC_PRECOMPENSATE
+  // Remove synchronization error pre-compensation
+  if (isnormal(sync_err)) {
+    srsran_vec_apply_cfo(ce, -sync_err / dmrs_stride, ce, nof_re_x_symbol);
+  }
+#endif // DMRS_SCH_SYNC_ERROR_PRECOMPENSATE
+
   // Time domain hold, extract resource elements estimates for PDSCH
-  uint32_t symbol_idx = 0;
-  uint32_t count      = 0;
+  uint32_t count = 0;
   for (uint32_t l = grant->S; l < grant->S + grant->L; l++) {
-    while (symbols[symbol_idx] < l && symbol_idx < nof_symbols - 1) {
-      symbol_idx++;
-    }
-
     // Initialise reserved mask
     bool rvd_mask_wb[SRSRAN_NRE * SRSRAN_MAX_PRB_NR] = {};
 
-    // Compute reserved RE
+    // Compute reserved RE mask by procedures
     if (srsran_re_pattern_list_to_symbol_mask(&cfg->rvd_re, l, rvd_mask_wb) < SRSRAN_SUCCESS) {
       ERROR("Error generating reserved RE mask");
       return SRSRAN_ERROR;
     }
 
+    // Compute reserved RE mask for DMRS
+    if (srsran_re_pattern_to_symbol_mask(&dmrs_pattern, l, rvd_mask_wb) < SRSRAN_SUCCESS) {
+      ERROR("Error generating reserved RE mask");
+      return SRSRAN_ERROR;
+    }
+
     // Narrow reserved subcarriers to the ones used in the transmission
     bool rvd_mask[SRSRAN_NRE * SRSRAN_MAX_PRB_NR] = {};
     for (uint32_t i = 0, k = 0; i < q->carrier.nof_prb; i++) {
@@ -843,40 +951,13 @@ int srsran_dmrs_sch_estimate(srsran_dmrs_sch_t*           q,
       }
     }
 
-    // Check if it s DMRS symbol
-    if (symbols[symbol_idx] == l) {
-      switch (dmrs_cfg->type) {
-        case srsran_dmrs_sch_type_1:
-          // Skip if there is no data to read
-          if (grant->nof_dmrs_cdm_groups_without_data != 1) {
-            continue;
-          }
-          for (uint32_t i = 1; i < nof_re_x_symbol; i += 2) {
-            if (!rvd_mask[i]) {
-              chest_res->ce[0][0][count++] = ce[i];
-            }
-          }
-          break;
-        case srsran_dmrs_sch_type_2:
-          // Skip if there is no data to read
-          if (grant->nof_dmrs_cdm_groups_without_data != 1 && grant->nof_dmrs_cdm_groups_without_data != 2) {
-            continue;
-          }
-          for (uint32_t i = grant->nof_dmrs_cdm_groups_without_data * 2; i < nof_re_x_symbol; i += 6) {
-            uint32_t nof_re = (3 - grant->nof_dmrs_cdm_groups_without_data) * 2;
-            for (uint32_t j = 0; j < nof_re; j++) {
-              if (!rvd_mask[i + j]) {
-                chest_res->ce[0][0][count++] = ce[i + j];
-              }
-            }
-          }
-          break;
-      }
-    } else {
-      for (uint32_t i = 0; i < nof_re_x_symbol; i++) {
-        if (!rvd_mask[i]) {
-          chest_res->ce[0][0][count++] = ce[i];
-        }
+    for (uint32_t i = 0; i < nof_re_x_symbol; i++) {
+      if (!rvd_mask[i]) {
+#if DMRS_SCH_CFO_PRECOMPENSATE
+        chest_res->ce[0][0][count++] = ce[i] * cfo_correction[l];
+#else  // DMRS_SCH_CFO_PRECOMPENSATE
+        chest_res->ce[0][0][count++] = ce[i];
+#endif // DMRS_SCH_CFO_PRECOMPENSATE
       }
     }
   }
diff --git a/lib/src/phy/ch_estimation/test/dmrs_pdcch_test.c b/lib/src/phy/ch_estimation/test/dmrs_pdcch_test.c
index e50b1f109..975a93582 100644
--- a/lib/src/phy/ch_estimation/test/dmrs_pdcch_test.c
+++ b/lib/src/phy/ch_estimation/test/dmrs_pdcch_test.c
@@ -107,7 +107,7 @@ static int run_test(srsran_dmrs_pdcch_estimator_t* estimator,
       }
       TESTASSERT(fabsf(measure.epre - 1.0f) < 1e-3f);
       TESTASSERT(fabsf(measure.rsrp - 1.0f) < 1e-3f);
-      TESTASSERT(fabsf(measure.cfo_hz) < 1e-3f);
+      TESTASSERT(coreset->duration == 1 || fabsf(measure.cfo_hz) < 1e-3f);
       TESTASSERT(fabsf(measure.sync_error_us) < 1e-3f);
 
       TESTASSERT(srsran_dmrs_pdcch_get_ce(estimator, &dci_location, ce) == SRSRAN_SUCCESS);
diff --git a/lib/src/phy/common/phy_common_nr.c b/lib/src/phy/common/phy_common_nr.c
index c874a4a7e..52f7c2231 100644
--- a/lib/src/phy/common/phy_common_nr.c
+++ b/lib/src/phy/common/phy_common_nr.c
@@ -188,10 +188,16 @@ float srsran_symbol_distance_s(uint32_t l0, uint32_t l1, uint32_t numerology)
 
 bool srsran_tdd_nr_is_dl(const srsran_tdd_config_nr_t* cfg, uint32_t numerology, uint32_t slot_idx)
 {
+  // Protect NULL pointer access
   if (cfg == NULL) {
     return false;
   }
 
+  // Prevent zero division
+  if (cfg->pattern1.period_ms == 0 && cfg->pattern2.period_ms == 0) {
+    return false;
+  }
+
   // Calculate slot index within the TDD overall period
   uint32_t slot_x_ms       = 1U << numerology; // Number of slots per millisecond
   uint32_t period_sum      = (cfg->pattern1.period_ms + cfg->pattern2.period_ms) * slot_x_ms; // Total perdiod sum
@@ -204,16 +210,23 @@ bool srsran_tdd_nr_is_dl(const srsran_tdd_config_nr_t* cfg, uint32_t numerology,
     slot_idx_period -= cfg->pattern1.period_ms * slot_x_ms; // Remove pattern 1 offset
   }
 
+  // Check DL boundaries
   return (slot_idx_period < pattern->nof_dl_slots ||
           (slot_idx_period == pattern->nof_dl_slots && pattern->nof_dl_symbols != 0));
 }
 
 bool srsran_tdd_nr_is_ul(const srsran_tdd_config_nr_t* cfg, uint32_t numerology, uint32_t slot_idx)
 {
+  // Protect NULL pointer access
   if (cfg == NULL) {
     return false;
   }
 
+  // Prevent zero division
+  if (cfg->pattern1.period_ms == 0 && cfg->pattern2.period_ms == 0) {
+    return false;
+  }
+
   // Calculate slot index within the TDD overall period
   uint32_t slot_x_ms       = 1U << numerology; // Number of slots per millisecond
   uint32_t period_sum      = (cfg->pattern1.period_ms + cfg->pattern2.period_ms) * slot_x_ms; // Total perdiod sum
@@ -229,5 +242,6 @@ bool srsran_tdd_nr_is_ul(const srsran_tdd_config_nr_t* cfg, uint32_t numerology,
   // Calculate slot in which UL starts
   uint32_t start_ul = (pattern->period_ms * slot_x_ms - pattern->nof_ul_slots) - 1;
 
+  // Check UL boundaries
   return (slot_idx_period > start_ul || (slot_idx_period == start_ul && pattern->nof_ul_symbols != 0));
 }
diff --git a/lib/src/phy/fec/crc.c b/lib/src/phy/fec/crc.c
index db5db968a..ef8f41d57 100644
--- a/lib/src/phy/fec/crc.c
+++ b/lib/src/phy/fec/crc.c
@@ -23,6 +23,10 @@
 #include "srsran/phy/utils/bit.h"
 #include "srsran/phy/utils/debug.h"
 
+#ifdef LV_HAVE_SSE
+#include <immintrin.h>
+#endif // LV_HAVE_SSE
+
 static void gen_crc_table(srsran_crc_t* h)
 {
   uint32_t pad        = (h->order < 8) ? (8 - h->order) : 0;
@@ -113,7 +117,18 @@ uint32_t srsran_crc_checksum(srsran_crc_t* h, uint8_t* data, int len)
         byte |= ((uint8_t) * (pter + k)) << (7 - k);
       }
     } else {
+#ifdef LV_HAVE_SSE
+      // Get 8 Bit
+      __m64 mask = _mm_cmpgt_pi8(*((__m64*)pter), _mm_set1_pi8(0));
+
+      // Reverse
+      mask = _mm_shuffle_pi8(mask, _mm_set_pi8(0, 1, 2, 3, 4, 5, 6, 7));
+
+      // Get mask and write
+      byte = (uint8_t)_mm_movemask_pi8(mask);
+#else  /* LV_HAVE_SSE */
       byte = (uint8_t)(srsran_bit_pack(&pter, 8) & 0xFF);
+#endif /* LV_HAVE_SSE */
     }
     srsran_crc_checksum_put_byte(h, byte);
   }
@@ -168,3 +183,11 @@ uint32_t srsran_crc_attach(srsran_crc_t* h, uint8_t* data, int len)
   srsran_bit_unpack(checksum, &ptr, h->order);
   return checksum;
 }
+
+bool srsran_crc_match(srsran_crc_t* h, uint8_t* data, int len)
+{
+  uint8_t* ptr       = &data[len];
+  uint32_t checksum1 = srsran_crc_checksum(h, data, len);
+  uint32_t checksum2 = srsran_bit_pack(&ptr, h->order);
+  return (checksum1 == checksum2);
+}
diff --git a/lib/src/phy/fec/ldpc/ldpc_dec_c_avx512.c b/lib/src/phy/fec/ldpc/ldpc_dec_c_avx512.c
index 80f407caa..19e9f518b 100644
--- a/lib/src/phy/fec/ldpc/ldpc_dec_c_avx512.c
+++ b/lib/src/phy/fec/ldpc/ldpc_dec_c_avx512.c
@@ -241,9 +241,7 @@ int extract_ldpc_message_c_avx512(void* p, uint8_t* message, uint16_t liftK)
 
   int ini = 0;
   for (int i = 0; i < liftK; i = i + vp->ls) {
-    for (int k = 0; k < vp->ls; k++) {
-      message[i + k] = (vp->soft_bits.c[ini + k] < 0);
-    }
+    fec_avx512_hard_decision_c(&vp->soft_bits.c[ini], &message[i], vp->ls);
     ini = ini + SRSRAN_AVX512_B_SIZE;
   }
 
diff --git a/lib/src/phy/fec/ldpc/ldpc_dec_c_avx512long.c b/lib/src/phy/fec/ldpc/ldpc_dec_c_avx512long.c
index 402e41f90..a67e28de6 100644
--- a/lib/src/phy/fec/ldpc/ldpc_dec_c_avx512long.c
+++ b/lib/src/phy/fec/ldpc/ldpc_dec_c_avx512long.c
@@ -301,9 +301,7 @@ int extract_ldpc_message_c_avx512long(void* p, uint8_t* message, uint16_t liftK)
 
   int ini = 0;
   for (int i = 0; i < liftK; i = i + vp->ls) {
-    for (int k = 0; k < vp->ls; k++) {
-      message[i + k] = (vp->soft_bits->c[ini + k] < 0);
-    }
+    fec_avx512_hard_decision_c(&vp->soft_bits->c[ini], &message[i], vp->ls);
     ini = ini + vp->node_size;
   }
 
diff --git a/lib/src/phy/fec/ldpc/ldpc_decoder.c b/lib/src/phy/fec/ldpc/ldpc_decoder.c
index 55454a472..e529a5051 100644
--- a/lib/src/phy/fec/ldpc/ldpc_decoder.c
+++ b/lib/src/phy/fec/ldpc/ldpc_decoder.c
@@ -39,7 +39,119 @@
 #include "srsran/phy/utils/debug.h"
 #include "srsran/phy/utils/vector.h"
 
-#define MAX_ITERATIONS 10 /*!< \brief Iterations of the BP algorithm. */
+#define LDPC_DECODER_DEFAULT_MAX_NOF_ITER 10 /*!< \brief Default maximum number of iterations of the BP algorithm. */
+
+#define LDPC_DECODER_TEMPLATE(LLR_TYPE, SUFFIX)                                                                        \
+  static int decode_##SUFFIX(                                                                                          \
+      void* o, const LLR_TYPE* llrs, uint8_t* message, uint32_t cdwd_rm_length, srsran_crc_t* crc)                     \
+  {                                                                                                                    \
+    srsran_ldpc_decoder_t* q = o;                                                                                      \
+                                                                                                                       \
+    /* it must be smaller than the codeword size */                                                                    \
+    if (cdwd_rm_length > q->liftN - 2 * q->ls) {                                                                       \
+      cdwd_rm_length = q->liftN - 2 * q->ls;                                                                           \
+    }                                                                                                                  \
+    /* We need at least q->bgK + 4 variable nodes to cover the high-rate region. However,*/                            \
+    /* 2 variable nodes are systematically punctured by the encoder. */                                                \
+    if (cdwd_rm_length < (q->bgK + 2) * q->ls) {                                                                       \
+      /* ERROR("The rate-matched codeword should have a length at least equal to the high-rate region.");*/            \
+      cdwd_rm_length = (q->bgK + 2) * q->ls;                                                                           \
+      /* return -1;*/                                                                                                  \
+    }                                                                                                                  \
+    if (cdwd_rm_length % q->ls) {                                                                                      \
+      cdwd_rm_length = (cdwd_rm_length / q->ls + 1) * q->ls;                                                           \
+      /* ERROR("The rate-matched codeword length should be a multiple of the lifting size."); */                       \
+      /* return -1;*/                                                                                                  \
+    }                                                                                                                  \
+    init_ldpc_dec_##SUFFIX(q->ptr, llrs, q->ls);                                                                       \
+                                                                                                                       \
+    uint16_t* this_pcm                   = NULL;                                                                       \
+    int8_t(*these_var_indices)[MAX_CNCT] = NULL;                                                                       \
+                                                                                                                       \
+    /* When computing the number of layers, we need to recall that the standard always removes */                      \
+    /* the first two variable nodes from the final codeword.*/                                                         \
+    uint8_t n_layers = cdwd_rm_length / q->ls - q->bgK + 2;                                                            \
+                                                                                                                       \
+    for (int i_iteration = 0; i_iteration < q->max_nof_iter; i_iteration++) {                                          \
+      for (int i_layer = 0; i_layer < n_layers; i_layer++) {                                                           \
+        update_ldpc_var_to_check_##SUFFIX(q->ptr, i_layer);                                                            \
+                                                                                                                       \
+        this_pcm          = q->pcm + i_layer * q->bgN;                                                                 \
+        these_var_indices = q->var_indices + i_layer;                                                                  \
+                                                                                                                       \
+        update_ldpc_check_to_var_##SUFFIX(q->ptr, i_layer, this_pcm, these_var_indices);                               \
+                                                                                                                       \
+        update_ldpc_soft_bits_##SUFFIX(q->ptr, i_layer, these_var_indices);                                            \
+      }                                                                                                                \
+                                                                                                                       \
+      if (crc != NULL) {                                                                                               \
+        extract_ldpc_message_##SUFFIX(q->ptr, message, q->liftK);                                                      \
+                                                                                                                       \
+        if (srsran_crc_match(crc, message, q->liftK - crc->order)) {                                                   \
+          return i_iteration + 1;                                                                                      \
+        }                                                                                                              \
+      }                                                                                                                \
+    }                                                                                                                  \
+                                                                                                                       \
+    /* If reached here, and CRC is being checked, it has failed */                                                     \
+    if (crc != NULL) {                                                                                                 \
+      return 0;                                                                                                        \
+    }                                                                                                                  \
+                                                                                                                       \
+    /* Without CRC, extract message and return the maximum number of iterations */                                     \
+    extract_ldpc_message_##SUFFIX(q->ptr, message, q->liftK);                                                          \
+    return q->max_nof_iter;                                                                                            \
+  }
+#define LDPC_DECODER_TEMPLATE_FLOOD(LLR_TYPE, SUFFIX)                                                                  \
+  static int decode_##SUFFIX(                                                                                          \
+      void* o, const LLR_TYPE* llrs, uint8_t* message, uint32_t cdwd_rm_length, srsran_crc_t* crc)                     \
+  {                                                                                                                    \
+    srsran_ldpc_decoder_t* q = o;                                                                                      \
+                                                                                                                       \
+    /* it must be smaller than the codeword size */                                                                    \
+    if (cdwd_rm_length > q->liftN - 2 * q->ls) {                                                                       \
+      cdwd_rm_length = q->liftN - 2 * q->ls;                                                                           \
+    }                                                                                                                  \
+    /* We need at least q->bgK + 4 variable nodes to cover the high-rate region. However,*/                            \
+    /* 2 variable nodes are systematically punctured by the encoder. */                                                \
+    if (cdwd_rm_length < (q->bgK + 2) * q->ls) {                                                                       \
+      /* ERROR("The rate-matched codeword should have a length at least equal to the high-rate region.");*/            \
+      cdwd_rm_length = (q->bgK + 2) * q->ls;                                                                           \
+      /* return -1;*/                                                                                                  \
+    }                                                                                                                  \
+    if (cdwd_rm_length % q->ls) {                                                                                      \
+      cdwd_rm_length = (cdwd_rm_length / q->ls + 1) * q->ls;                                                           \
+      /* ERROR("The rate-matched codeword length should be a multiple of the lifting size."); */                       \
+      /* return -1;*/                                                                                                  \
+    }                                                                                                                  \
+    init_ldpc_dec_##SUFFIX(q->ptr, llrs, q->ls);                                                                       \
+                                                                                                                       \
+    uint16_t* this_pcm                   = NULL;                                                                       \
+    int8_t(*these_var_indices)[MAX_CNCT] = NULL;                                                                       \
+                                                                                                                       \
+    /* When computing the number of layers, we need to recall that the standard always removes */                      \
+    /* the first two variable nodes from the final codeword.*/                                                         \
+    uint8_t n_layers = cdwd_rm_length / q->ls - q->bgK + 2;                                                            \
+                                                                                                                       \
+    for (int i_iteration = 0; i_iteration < 2 * q->max_nof_iter; i_iteration++) {                                      \
+      for (int i_layer = 0; i_layer < n_layers; i_layer++) {                                                           \
+        update_ldpc_var_to_check_##SUFFIX(q->ptr, i_layer);                                                            \
+      }                                                                                                                \
+                                                                                                                       \
+      for (int i_layer = 0; i_layer < n_layers; i_layer++) {                                                           \
+        this_pcm          = q->pcm + i_layer * q->bgN;                                                                 \
+        these_var_indices = q->var_indices + i_layer;                                                                  \
+                                                                                                                       \
+        update_ldpc_check_to_var_##SUFFIX(q->ptr, i_layer, this_pcm, these_var_indices);                               \
+      }                                                                                                                \
+                                                                                                                       \
+      update_ldpc_soft_bits_##SUFFIX(q->ptr, q->var_indices);                                                          \
+    }                                                                                                                  \
+                                                                                                                       \
+    extract_ldpc_message_##SUFFIX(q->ptr, message, q->liftK);                                                          \
+                                                                                                                       \
+    return q->max_nof_iter;                                                                                            \
+  }
 
 /*! Carries out the actual destruction of the memory allocated to the decoder, float-LLR case. */
 static void free_dec_f(void* o)
@@ -55,52 +167,7 @@ static void free_dec_f(void* o)
 }
 
 /*! Carries out the decoding with real-valued LLRs. */
-static int decode_f(void* o, const float* llrs, uint8_t* message, uint32_t cdwd_rm_length)
-{
-  srsran_ldpc_decoder_t* q = o;
-
-  if (cdwd_rm_length > q->liftN - 2 * q->ls) {
-    cdwd_rm_length = q->liftN - 2 * q->ls;
-  }
-  // We need at least q->bgK + 4 variable nodes to cover the high-rate region. However,
-  // 2 variable nodes are systematically punctured by the encoder.
-  if (cdwd_rm_length < (q->bgK + 2) * q->ls) {
-    // ERROR("The rate-matched codeword should have a length at least equal to the high-rate region.");
-    cdwd_rm_length = (q->bgK + 2) * q->ls;
-    // return -1;
-  }
-  if (cdwd_rm_length % q->ls) {
-    cdwd_rm_length = (cdwd_rm_length / q->ls + 1) * q->ls;
-    // ERROR("The rate-matched codeword length should be a multiple of the lifting size.");
-    // return -1;
-  }
-
-  init_ldpc_dec_f(q->ptr, llrs, q->ls);
-
-  uint16_t* this_pcm                   = NULL;
-  int8_t(*these_var_indices)[MAX_CNCT] = NULL;
-
-  // When computing the number of layers, we need to recall that the standard always removes
-  // the first two variable nodes from the final codeword.
-  uint8_t n_layers = cdwd_rm_length / q->ls - q->bgK + 2;
-
-  for (int i_iteration = 0; i_iteration < MAX_ITERATIONS; i_iteration++) {
-    for (int i_layer = 0; i_layer < n_layers; i_layer++) {
-      update_ldpc_var_to_check_f(q->ptr, i_layer);
-
-      this_pcm          = q->pcm + i_layer * q->bgN;
-      these_var_indices = q->var_indices + i_layer;
-
-      update_ldpc_check_to_var_f(q->ptr, i_layer, this_pcm, these_var_indices);
-
-      update_ldpc_soft_bits_f(q->ptr, i_layer, these_var_indices);
-    }
-  }
-
-  extract_ldpc_message_f(q->ptr, message, q->liftK);
-
-  return 0;
-}
+LDPC_DECODER_TEMPLATE(float, f)
 
 /*! Initializes the decoder to work with real valued LLRs. */
 static int init_f(srsran_ldpc_decoder_t* q)
@@ -132,53 +199,7 @@ static void free_dec_s(void* o)
 }
 
 /*! Carries out the decoding with 16-bit integer-valued LLRs. */
-static int decode_s(void* o, const int16_t* llrs, uint8_t* message, uint32_t cdwd_rm_length)
-{
-  srsran_ldpc_decoder_t* q = o;
-
-  // it must be smaller than the codeword size
-  if (cdwd_rm_length > q->liftN - 2 * q->ls) {
-    cdwd_rm_length = q->liftN - 2 * q->ls;
-  }
-  // We need at least q->bgK + 4 variable nodes to cover the high-rate region. However,
-  // 2 variable nodes are systematically punctured by the encoder.
-  if (cdwd_rm_length < (q->bgK + 2) * q->ls) {
-    // ERROR("The rate-matched codeword should have a length at least equal to the high-rate region.");
-    cdwd_rm_length = (q->bgK + 2) * q->ls;
-    // return -1;
-  }
-  if (cdwd_rm_length % q->ls) {
-    cdwd_rm_length = (cdwd_rm_length / q->ls + 1) * q->ls;
-    // ERROR("The rate-matched codeword length should be a multiple of the lifting size.");
-    // return -1;
-  }
-
-  init_ldpc_dec_s(q->ptr, llrs, q->ls);
-
-  uint16_t* this_pcm                   = NULL;
-  int8_t(*these_var_indices)[MAX_CNCT] = NULL;
-
-  // When computing the number of layers, we need to recall that the standard always removes
-  // the first two variable nodes from the final codeword.
-  uint8_t n_layers = cdwd_rm_length / q->ls - q->bgK + 2;
-
-  for (int i_iteration = 0; i_iteration < MAX_ITERATIONS; i_iteration++) {
-    for (int i_layer = 0; i_layer < n_layers; i_layer++) {
-      update_ldpc_var_to_check_s(q->ptr, i_layer);
-
-      this_pcm          = q->pcm + i_layer * q->bgN;
-      these_var_indices = q->var_indices + i_layer;
-
-      update_ldpc_check_to_var_s(q->ptr, i_layer, this_pcm, these_var_indices);
-
-      update_ldpc_soft_bits_s(q->ptr, i_layer, these_var_indices);
-    }
-  }
-
-  extract_ldpc_message_s(q->ptr, message, q->liftK);
-
-  return 0;
-}
+LDPC_DECODER_TEMPLATE(int16_t, s)
 
 /*! Initializes the decoder to work with 16-bit integer-valued LLRs. */
 static int init_s(srsran_ldpc_decoder_t* q)
@@ -210,53 +231,7 @@ static void free_dec_c(void* o)
 }
 
 /*! Carries out the decoding with 8-bit integer-valued LLRs. */
-static int decode_c(void* o, const int8_t* llrs, uint8_t* message, uint32_t cdwd_rm_length)
-{
-  srsran_ldpc_decoder_t* q = o;
-
-  // it must be smaller than the codeword size
-  if (cdwd_rm_length > q->liftN - 2 * q->ls) {
-    cdwd_rm_length = q->liftN - 2 * q->ls;
-  }
-  // We need at least q->bgK + 4 variable nodes to cover the high-rate region. However,
-  // 2 variable nodes are systematically punctured by the encoder.
-  if (cdwd_rm_length < (q->bgK + 2) * q->ls) {
-    // ERROR("The rate-matched codeword should have a length at least equal to the high-rate region.");
-    cdwd_rm_length = (q->bgK + 2) * q->ls;
-    // return -1;
-  }
-  if (cdwd_rm_length % q->ls) {
-    cdwd_rm_length = (cdwd_rm_length / q->ls + 1) * q->ls;
-    // ERROR("The rate-matched codeword length should be a multiple of the lifting size.");
-    // return -1;
-  }
-
-  init_ldpc_dec_c(q->ptr, llrs, q->ls);
-
-  uint16_t* this_pcm                   = NULL;
-  int8_t(*these_var_indices)[MAX_CNCT] = NULL;
-
-  // When computing the number of layers, we need to recall that the standard always removes
-  // the first two variable nodes from the final codeword.
-  uint8_t n_layers = cdwd_rm_length / q->ls - q->bgK + 2;
-
-  for (int i_iteration = 0; i_iteration < MAX_ITERATIONS; i_iteration++) {
-    for (int i_layer = 0; i_layer < n_layers; i_layer++) {
-      update_ldpc_var_to_check_c(q->ptr, i_layer);
-
-      this_pcm          = q->pcm + i_layer * q->bgN;
-      these_var_indices = q->var_indices + i_layer;
-
-      update_ldpc_check_to_var_c(q->ptr, i_layer, this_pcm, these_var_indices);
-
-      update_ldpc_soft_bits_c(q->ptr, i_layer, these_var_indices);
-    }
-  }
-
-  extract_ldpc_message_c(q->ptr, message, q->liftK);
-
-  return 0;
-}
+LDPC_DECODER_TEMPLATE(int8_t, c)
 
 /*! Initializes the decoder to work with 8-bit integer-valued LLRs. */
 static int init_c(srsran_ldpc_decoder_t* q)
@@ -288,53 +263,7 @@ static void free_dec_c_flood(void* o)
 }
 
 /*! Carries out the decoding with 8-bit integer-valued LLRs, flooded scheduling. */
-static int decode_c_flood(void* o, const int8_t* llrs, uint8_t* message, uint32_t cdwd_rm_length)
-{
-  srsran_ldpc_decoder_t* q = o;
-
-  // it must be smaller than the codeword size
-  if (cdwd_rm_length > q->liftN - 2 * q->ls) {
-    cdwd_rm_length = q->liftN - 2 * q->ls;
-  }
-  // We need at least q->bgK + 4 variable nodes to cover the high-rate region. However,
-  // 2 variable nodes are systematically punctured by the encoder.
-  if (cdwd_rm_length < (q->bgK + 2) * q->ls) {
-    // ERROR("The rate-matched codeword should have a length at least equal to the high-rate region.");
-    cdwd_rm_length = (q->bgK + 2) * q->ls;
-    // return -1;
-  }
-  if (cdwd_rm_length % q->ls) {
-    cdwd_rm_length = (cdwd_rm_length / q->ls + 1) * q->ls;
-    // ERROR("The rate-matched codeword length should be a multiple of the lifting size.");
-    // return -1;
-  }
-  init_ldpc_dec_c_flood(q->ptr, llrs, q->ls);
-
-  uint16_t* this_pcm                   = NULL;
-  int8_t(*these_var_indices)[MAX_CNCT] = NULL;
-
-  // When computing the number of layers, we need to recall that the standard always removes
-  // the first two variable nodes from the final codeword.
-  uint8_t n_layers = cdwd_rm_length / q->ls - q->bgK + 2;
-
-  for (int i_iteration = 0; i_iteration < 2 * MAX_ITERATIONS; i_iteration++) {
-    for (int i_layer = 0; i_layer < n_layers; i_layer++) {
-      update_ldpc_var_to_check_c_flood(q->ptr, i_layer);
-    }
-
-    for (int i_layer = 0; i_layer < n_layers; i_layer++) {
-      this_pcm          = q->pcm + i_layer * q->bgN;
-      these_var_indices = q->var_indices + i_layer;
-
-      update_ldpc_check_to_var_c_flood(q->ptr, i_layer, this_pcm, these_var_indices);
-    }
-    update_ldpc_soft_bits_c_flood(q->ptr, q->var_indices);
-  }
-
-  extract_ldpc_message_c_flood(q->ptr, message, q->liftK);
-
-  return 0;
-}
+LDPC_DECODER_TEMPLATE_FLOOD(int8_t, c_flood);
 
 /*! Initializes the decoder to work with 8-bit integer-valued LLRs. */
 static int init_c_flood(srsran_ldpc_decoder_t* q)
@@ -367,52 +296,7 @@ static void free_dec_c_avx2(void* o)
 }
 
 /*! Carries out the decoding with 8-bit integer-valued LLRs (AVX2 implementation). */
-static int decode_c_avx2(void* o, const int8_t* llrs, uint8_t* message, uint32_t cdwd_rm_length)
-{
-  srsran_ldpc_decoder_t* q = o;
-
-  // it must be smaller than the codeword size
-  if (cdwd_rm_length > q->liftN - 2 * q->ls) {
-    cdwd_rm_length = q->liftN - 2 * q->ls;
-  }
-  // We need at least q->bgK + 4 variable nodes to cover the high-rate region. However,
-  // 2 variable nodes are systematically punctured by the encoder.
-  if (cdwd_rm_length < (q->bgK + 2) * q->ls) {
-    // ERROR("The rate-matched codeword should have a length at least equal to the high-rate region.");
-    cdwd_rm_length = (q->bgK + 2) * q->ls;
-    // return -1;
-  }
-  if (cdwd_rm_length % q->ls) {
-    cdwd_rm_length = (cdwd_rm_length / q->ls + 1) * q->ls;
-    // ERROR("The rate-matched codeword length should be a multiple of the lifting size.");
-    // return -1;
-  }
-  init_ldpc_dec_c_avx2(q->ptr, llrs, q->ls);
-
-  uint16_t* this_pcm                   = NULL;
-  int8_t(*these_var_indices)[MAX_CNCT] = NULL;
-
-  // When computing the number of layers, we need to recall that the standard always removes
-  // the first two variable nodes from the final codeword.
-  uint8_t n_layers = cdwd_rm_length / q->ls - q->bgK + 2;
-
-  for (int i_iteration = 0; i_iteration < MAX_ITERATIONS; i_iteration++) {
-    for (int i_layer = 0; i_layer < n_layers; i_layer++) {
-      update_ldpc_var_to_check_c_avx2(q->ptr, i_layer);
-
-      this_pcm          = q->pcm + i_layer * q->bgN;
-      these_var_indices = q->var_indices + i_layer;
-
-      update_ldpc_check_to_var_c_avx2(q->ptr, i_layer, this_pcm, these_var_indices);
-
-      update_ldpc_soft_bits_c_avx2(q->ptr, i_layer, these_var_indices);
-    }
-  }
-
-  extract_ldpc_message_c_avx2(q->ptr, message, q->liftK);
-
-  return 0;
-}
+LDPC_DECODER_TEMPLATE(int8_t, c_avx2);
 
 /*! Initializes the decoder to work with 8-bit integer-valued LLRs (AVX2 implementation). */
 static int init_c_avx2(srsran_ldpc_decoder_t* q)
@@ -445,52 +329,7 @@ static void free_dec_c_avx2long(void* o)
 }
 
 /*! Carries out the decoding with 8-bit integer-valued LLRs (AVX2 implementation, large lifting size). */
-static int decode_c_avx2long(void* o, const int8_t* llrs, uint8_t* message, uint32_t cdwd_rm_length)
-{
-  srsran_ldpc_decoder_t* q = o;
-
-  // it must be smaller than the codeword size
-  if (cdwd_rm_length > q->liftN - 2 * q->ls) {
-    cdwd_rm_length = q->liftN - 2 * q->ls;
-  }
-  // We need at least q->bgK + 4 variable nodes to cover the high-rate region. However,
-  // 2 variable nodes are systematically punctured by the encoder.
-  if (cdwd_rm_length < (q->bgK + 2) * q->ls) {
-    // ERROR("The rate-matched codeword should have a length at least equal to the high-rate region.");
-    cdwd_rm_length = (q->bgK + 2) * q->ls;
-    // return -1;
-  }
-  if (cdwd_rm_length % q->ls) {
-    cdwd_rm_length = (cdwd_rm_length / q->ls + 1) * q->ls;
-    // ERROR("The rate-matched codeword length should be a multiple of the lifting size.");
-    // return -1;
-  }
-  init_ldpc_dec_c_avx2long(q->ptr, llrs, q->ls);
-
-  uint16_t* this_pcm                   = NULL;
-  int8_t(*these_var_indices)[MAX_CNCT] = NULL;
-
-  // When computing the number of layers, we need to recall that the standard always removes
-  // the first two variable nodes from the final codeword.
-  uint8_t n_layers = cdwd_rm_length / q->ls - q->bgK + 2;
-
-  for (int i_iteration = 0; i_iteration < MAX_ITERATIONS; i_iteration++) {
-    for (int i_layer = 0; i_layer < n_layers; i_layer++) {
-      update_ldpc_var_to_check_c_avx2long(q->ptr, i_layer);
-
-      this_pcm          = q->pcm + i_layer * q->bgN;
-      these_var_indices = q->var_indices + i_layer;
-
-      update_ldpc_check_to_var_c_avx2long(q->ptr, i_layer, this_pcm, these_var_indices);
-
-      update_ldpc_soft_bits_c_avx2long(q->ptr, i_layer, these_var_indices);
-    }
-  }
-
-  extract_ldpc_message_c_avx2long(q->ptr, message, q->liftK);
-
-  return 0;
-}
+LDPC_DECODER_TEMPLATE(int8_t, c_avx2long);
 
 /*! Initializes the decoder to work with 8-bit integer-valued LLRs (AVX2 implementation, large lifting size). */
 static int init_c_avx2long(srsran_ldpc_decoder_t* q)
@@ -523,53 +362,7 @@ static void free_dec_c_avx2_flood(void* o)
 }
 
 /*! Carries out the decoding with 8-bit integer-valued LLRs (AVX2 implementation, flooded scheduling). */
-static int decode_c_avx2_flood(void* o, const int8_t* llrs, uint8_t* message, uint32_t cdwd_rm_length)
-{
-  srsran_ldpc_decoder_t* q = o;
-
-  // it must be smaller than the codeword size
-  if (cdwd_rm_length > q->liftN - 2 * q->ls) {
-    cdwd_rm_length = q->liftN - 2 * q->ls;
-  }
-  // We need at least q->bgK + 4 variable nodes to cover the high-rate region. However,
-  // 2 variable nodes are systematically punctured by the encoder.
-  if (cdwd_rm_length < (q->bgK + 2) * q->ls) {
-    // ERROR("The rate-matched codeword should have a length at least equal to the high-rate region.");
-    cdwd_rm_length = (q->bgK + 2) * q->ls;
-    // return -1;
-  }
-  if (cdwd_rm_length % q->ls) {
-    cdwd_rm_length = (cdwd_rm_length / q->ls + 1) * q->ls;
-    // ERROR("The rate-matched codeword length should be a multiple of the lifting size.");
-    // return -1;
-  }
-  init_ldpc_dec_c_avx2_flood(q->ptr, llrs, q->ls);
-
-  uint16_t* this_pcm                   = NULL;
-  int8_t(*these_var_indices)[MAX_CNCT] = NULL;
-
-  // When computing the number of layers, we need to recall that the standard always removes
-  // the first two variable nodes from the final codeword.
-  uint8_t n_layers = cdwd_rm_length / q->ls - q->bgK + 2;
-
-  for (int i_iteration = 0; i_iteration < 2 * MAX_ITERATIONS; i_iteration++) {
-    for (int i_layer = 0; i_layer < n_layers; i_layer++) {
-      update_ldpc_var_to_check_c_avx2_flood(q->ptr, i_layer);
-    }
-
-    for (int i_layer = 0; i_layer < n_layers; i_layer++) {
-      this_pcm          = q->pcm + i_layer * q->bgN;
-      these_var_indices = q->var_indices + i_layer;
-
-      update_ldpc_check_to_var_c_avx2_flood(q->ptr, i_layer, this_pcm, these_var_indices);
-    }
-    update_ldpc_soft_bits_c_avx2_flood(q->ptr, q->var_indices);
-  }
-
-  extract_ldpc_message_c_avx2_flood(q->ptr, message, q->liftK);
-
-  return 0;
-}
+LDPC_DECODER_TEMPLATE_FLOOD(int8_t, c_avx2_flood);
 
 /*! Initializes the decoder to work with 8-bit integer-valued LLRs (AVX2 implementation, flooded scheduling). */
 static int init_c_avx2_flood(srsran_ldpc_decoder_t* q)
@@ -603,54 +396,7 @@ static void free_dec_c_avx2long_flood(void* o)
 
 /*! Carries out the decoding with 8-bit integer-valued LLRs (flooded scheduling, AVX2 implementation, large lifting
  * size). */
-static int decode_c_avx2long_flood(void* o, const int8_t* llrs, uint8_t* message, uint32_t cdwd_rm_length)
-{
-  srsran_ldpc_decoder_t* q = o;
-
-  // it must be smaller than the codeword size
-  if (cdwd_rm_length > q->liftN - 2 * q->ls) {
-    cdwd_rm_length = q->liftN - 2 * q->ls;
-  }
-  // We need at least q->bgK + 4 variable nodes to cover the high-rate region. However,
-  // 2 variable nodes are systematically punctured by the encoder.
-  if (cdwd_rm_length < (q->bgK + 2) * q->ls) {
-    // ERROR("The rate-matched codeword should have a length at least equal to the high-rate region.");
-    cdwd_rm_length = (q->bgK + 2) * q->ls;
-    // return -1;
-  }
-  if (cdwd_rm_length % q->ls) {
-    cdwd_rm_length = (cdwd_rm_length / q->ls + 1) * q->ls;
-    // ERROR("The rate-matched codeword length should be a multiple of the lifting size.");
-    // return -1;
-  }
-  init_ldpc_dec_c_avx2long_flood(q->ptr, llrs, q->ls);
-
-  uint16_t* this_pcm                   = NULL;
-  int8_t(*these_var_indices)[MAX_CNCT] = NULL;
-
-  // When computing the number of layers, we need to recall that the standard always removes
-  // the first two variable nodes from the final codeword.
-  uint8_t n_layers = cdwd_rm_length / q->ls - q->bgK + 2;
-
-  for (int i_iteration = 0; i_iteration < 2 * MAX_ITERATIONS; i_iteration++) {
-    for (int i_layer = 0; i_layer < n_layers; i_layer++) {
-      update_ldpc_var_to_check_c_avx2long_flood(q->ptr, i_layer);
-    }
-
-    for (int i_layer = 0; i_layer < n_layers; i_layer++) {
-      this_pcm          = q->pcm + i_layer * q->bgN;
-      these_var_indices = q->var_indices + i_layer;
-
-      update_ldpc_check_to_var_c_avx2long_flood(q->ptr, i_layer, this_pcm, these_var_indices);
-    }
-
-    update_ldpc_soft_bits_c_avx2long_flood(q->ptr, q->var_indices);
-  }
-
-  extract_ldpc_message_c_avx2long_flood(q->ptr, message, q->liftK);
-
-  return 0;
-}
+LDPC_DECODER_TEMPLATE_FLOOD(int8_t, c_avx2long_flood)
 
 /*! Initializes the decoder to work with 8-bit integer-valued LLRs
  * (flooded scheduling, AVX2 implementation, large lifting size). */
@@ -689,52 +435,7 @@ static void free_dec_c_avx512(void* o)
 }
 
 /*! Carries out the decoding with 8-bit integer-valued LLRs (AVX512 implementation). */
-static int decode_c_avx512(void* o, const int8_t* llrs, uint8_t* message, uint32_t cdwd_rm_length)
-{
-  srsran_ldpc_decoder_t* q = o;
-
-  // it must be smaller than the codeword size
-  if (cdwd_rm_length > q->liftN - 2 * q->ls) {
-    cdwd_rm_length = q->liftN - 2 * q->ls;
-  }
-  // We need at least q->bgK + 4 variable nodes to cover the high-rate region. However,
-  // 2 variable nodes are systematically punctured by the encoder.
-  if (cdwd_rm_length < (q->bgK + 2) * q->ls) {
-    // ERROR("The rate-matched codeword should have a length at least equal to the high-rate region.");
-    cdwd_rm_length = (q->bgK + 2) * q->ls;
-    // return -1;
-  }
-  if (cdwd_rm_length % q->ls) {
-    cdwd_rm_length = (cdwd_rm_length / q->ls + 1) * q->ls;
-    // ERROR("The rate-matched codeword length should be a multiple of the lifting size.");
-    // return -1;
-  }
-  init_ldpc_dec_c_avx512(q->ptr, llrs, q->ls);
-
-  uint16_t* this_pcm                   = NULL;
-  int8_t(*these_var_indices)[MAX_CNCT] = NULL;
-
-  // When computing the number of layers, we need to recall that the standard always removes
-  // the first two variable nodes from the final codeword.
-  uint8_t n_layers = cdwd_rm_length / q->ls - q->bgK + 2;
-
-  for (int i_iteration = 0; i_iteration < MAX_ITERATIONS; i_iteration++) {
-    for (int i_layer = 0; i_layer < n_layers; i_layer++) {
-      update_ldpc_var_to_check_c_avx512(q->ptr, i_layer);
-
-      this_pcm          = q->pcm + i_layer * q->bgN;
-      these_var_indices = q->var_indices + i_layer;
-
-      update_ldpc_check_to_var_c_avx512(q->ptr, i_layer, this_pcm, these_var_indices);
-
-      update_ldpc_soft_bits_c_avx512(q->ptr, i_layer, these_var_indices);
-    }
-  }
-
-  extract_ldpc_message_c_avx512(q->ptr, message, q->liftK);
-
-  return 0;
-}
+LDPC_DECODER_TEMPLATE(int8_t, c_avx512)
 
 /*! Initializes the decoder to work with 8-bit integer-valued LLRs (AVX512 implementation). */
 static int init_c_avx512(srsran_ldpc_decoder_t* q)
@@ -767,52 +468,7 @@ static void free_dec_c_avx512long(void* o)
 }
 
 /*! Carries out the decoding with 8-bit integer-valued LLRs (AVX512 implementation, large lifting size). */
-static int decode_c_avx512long(void* o, const int8_t* llrs, uint8_t* message, uint32_t cdwd_rm_length)
-{
-  srsran_ldpc_decoder_t* q = o;
-
-  // it must be smaller than the codeword size
-  if (cdwd_rm_length > q->liftN - 2 * q->ls) {
-    cdwd_rm_length = q->liftN - 2 * q->ls;
-  }
-  // We need at least q->bgK + 4 variable nodes to cover the high-rate region. However,
-  // 2 variable nodes are systematically punctured by the encoder.
-  if (cdwd_rm_length < (q->bgK + 2) * q->ls) {
-    // ERROR("The rate-matched codeword should have a length at least equal to the high-rate region.");
-    cdwd_rm_length = (q->bgK + 2) * q->ls;
-    // return -1;
-  }
-  if (cdwd_rm_length % q->ls) {
-    cdwd_rm_length = (cdwd_rm_length / q->ls + 1) * q->ls;
-    // ERROR("The rate-matched codeword length should be a multiple of the lifting size.");
-    // return -1;
-  }
-
-  init_ldpc_dec_c_avx512long(q->ptr, llrs, q->ls);
-
-  uint16_t* this_pcm                   = NULL;
-  int8_t(*these_var_indices)[MAX_CNCT] = NULL;
-
-  // When computing the number of layers, we need to recall that the standard always removes
-  // the first two variable nodes from the final codeword.
-  uint8_t n_layers = cdwd_rm_length / q->ls - q->bgK + 2;
-
-  for (int i_iteration = 0; i_iteration < MAX_ITERATIONS; i_iteration++) {
-    for (int i_layer = 0; i_layer < n_layers; i_layer++) {
-      update_ldpc_var_to_check_c_avx512long(q->ptr, i_layer);
-
-      this_pcm          = q->pcm + i_layer * q->bgN;
-      these_var_indices = q->var_indices + i_layer;
-
-      update_ldpc_check_to_var_c_avx512long(q->ptr, i_layer, this_pcm, these_var_indices);
-
-      update_ldpc_soft_bits_c_avx512long(q->ptr, i_layer, these_var_indices);
-    }
-  }
-  extract_ldpc_message_c_avx512long(q->ptr, message, q->liftK);
-
-  return 0;
-}
+LDPC_DECODER_TEMPLATE(int8_t, c_avx512long)
 
 /*! Initializes the decoder to work with 8-bit integer-valued LLRs (AVX512 implementation, large lifting size). */
 static int init_c_avx512long(srsran_ldpc_decoder_t* q)
@@ -846,54 +502,7 @@ static void free_dec_c_avx512long_flood(void* o)
 
 /*! Carries out the decoding with 8-bit integer-valued LLRs (flooded scheduling, AVX512 implementation, large lifting
  * size). */
-static int decode_c_avx512long_flood(void* o, const int8_t* llrs, uint8_t* message, uint32_t cdwd_rm_length)
-{
-  srsran_ldpc_decoder_t* q = o;
-
-  // it must be smaller than the codeword size
-  if (cdwd_rm_length > q->liftN - 2 * q->ls) {
-    cdwd_rm_length = q->liftN - 2 * q->ls;
-  }
-  // We need at least q->bgK + 4 variable nodes to cover the high-rate region. However,
-  // 2 variable nodes are systematically punctured by the encoder.
-  if (cdwd_rm_length < (q->bgK + 2) * q->ls) {
-    // ERROR("The rate-matched codeword should have a length at least equal to the high-rate region.");
-    cdwd_rm_length = (q->bgK + 2) * q->ls;
-    // return -1;
-  }
-  if (cdwd_rm_length % q->ls) {
-    cdwd_rm_length = (cdwd_rm_length / q->ls + 1) * q->ls;
-    // ERROR("The rate-matched codeword length should be a multiple of the lifting size.");
-    // return -1;
-  }
-  init_ldpc_dec_c_avx512long_flood(q->ptr, llrs, q->ls);
-
-  uint16_t* this_pcm                   = NULL;
-  int8_t(*these_var_indices)[MAX_CNCT] = NULL;
-
-  // When computing the number of layers, we need to recall that the standard always removes
-  // the first two variable nodes from the final codeword.
-  uint8_t n_layers = cdwd_rm_length / q->ls - q->bgK + 2;
-
-  for (int i_iteration = 0; i_iteration < 2 * MAX_ITERATIONS; i_iteration++) {
-    for (int i_layer = 0; i_layer < n_layers; i_layer++) {
-      update_ldpc_var_to_check_c_avx512long_flood(q->ptr, i_layer);
-    }
-
-    for (int i_layer = 0; i_layer < n_layers; i_layer++) {
-      this_pcm          = q->pcm + i_layer * q->bgN;
-      these_var_indices = q->var_indices + i_layer;
-
-      update_ldpc_check_to_var_c_avx512long_flood(q->ptr, i_layer, this_pcm, these_var_indices);
-    }
-
-    update_ldpc_soft_bits_c_avx512long_flood(q->ptr, q->var_indices);
-  }
-
-  extract_ldpc_message_c_avx512long_flood(q->ptr, message, q->liftK);
-
-  return 0;
-}
+LDPC_DECODER_TEMPLATE_FLOOD(int8_t, c_avx512long_flood)
 
 /*! Initializes the decoder to work with 8-bit integer-valued LLRs
  * (flooded scheduling, AVX512 implementation, large lifting size). */
@@ -914,14 +523,19 @@ static int init_c_avx512long_flood(srsran_ldpc_decoder_t* q)
 
 #endif // LV_HAVE_AVX512
 
-int srsran_ldpc_decoder_init(srsran_ldpc_decoder_t*     q,
-                             srsran_ldpc_decoder_type_t type,
-                             srsran_basegraph_t         bg,
-                             uint16_t                   ls,
-                             float                      scaling_fctr)
+int srsran_ldpc_decoder_init(srsran_ldpc_decoder_t* q, const srsran_ldpc_decoder_args_t* args)
 {
-  int ls_index = get_ls_index(ls);
+  if (q == NULL || args == NULL) {
+    return -1;
+  }
 
+  // Extract configuration arguments
+  uint16_t                   ls           = args->ls;
+  srsran_basegraph_t         bg           = args->bg;
+  float                      scaling_fctr = args->scaling_fctr;
+  srsran_ldpc_decoder_type_t type         = args->type;
+
+  int ls_index = get_ls_index(ls);
   if (ls_index == VOID_LIFTSIZE) {
     ERROR("Invalid lifting size %d", ls);
     return -1;
@@ -948,6 +562,8 @@ int srsran_ldpc_decoder_init(srsran_ldpc_decoder_t*     q,
   q->liftM = ls * q->bgM;
   q->liftN = ls * q->bgN;
 
+  q->max_nof_iter = (args->max_nof_iter == 0) ? LDPC_DECODER_DEFAULT_MAX_NOF_ITER : args->max_nof_iter;
+
   q->pcm = srsran_vec_u16_malloc(q->bgM * q->bgN);
   if (!q->pcm) {
     perror("malloc");
@@ -1026,7 +642,7 @@ void srsran_ldpc_decoder_free(srsran_ldpc_decoder_t* q)
 
 int srsran_ldpc_decoder_decode_f(srsran_ldpc_decoder_t* q, const float* llrs, uint8_t* message, uint32_t cdwd_rm_length)
 {
-  return q->decode_f(q, llrs, message, cdwd_rm_length);
+  return q->decode_f(q, llrs, message, cdwd_rm_length, NULL);
 }
 
 int srsran_ldpc_decoder_decode_s(srsran_ldpc_decoder_t* q,
@@ -1034,7 +650,7 @@ int srsran_ldpc_decoder_decode_s(srsran_ldpc_decoder_t* q,
                                  uint8_t*               message,
                                  uint32_t               cdwd_rm_length)
 {
-  return q->decode_s(q, llrs, message, cdwd_rm_length);
+  return q->decode_s(q, llrs, message, cdwd_rm_length, NULL);
 }
 
 int srsran_ldpc_decoder_decode_c(srsran_ldpc_decoder_t* q,
@@ -1042,5 +658,14 @@ int srsran_ldpc_decoder_decode_c(srsran_ldpc_decoder_t* q,
                                  uint8_t*               message,
                                  uint32_t               cdwd_rm_length)
 {
-  return q->decode_c(q, llrs, message, cdwd_rm_length);
+  return q->decode_c(q, llrs, message, cdwd_rm_length, NULL);
+}
+
+int srsran_ldpc_decoder_decode_crc_c(srsran_ldpc_decoder_t* q,
+                                     const int8_t*          llrs,
+                                     uint8_t*               message,
+                                     uint32_t               cdwd_rm_length,
+                                     srsran_crc_t*          crc)
+{
+  return q->decode_c(q, llrs, message, cdwd_rm_length, crc);
 }
diff --git a/lib/src/phy/fec/ldpc/test/ldpc_chain_test.c b/lib/src/phy/fec/ldpc/test/ldpc_chain_test.c
index 2e1a620bd..c1821a4d4 100644
--- a/lib/src/phy/fec/ldpc/test/ldpc_chain_test.c
+++ b/lib/src/phy/fec/ldpc/test/ldpc_chain_test.c
@@ -169,7 +169,7 @@ int main(int argc, char** argv)
     perror("encoder init");
     exit(-1);
   }
-#else  // no AVX2
+#else // no AVX2
   if (srsran_ldpc_encoder_init(&encoder, SRSRAN_LDPC_ENCODER_C, base_graph, lift_size) != 0) {
     perror("encoder init");
     exit(-1);
@@ -177,42 +177,53 @@ int main(int argc, char** argv)
 #endif // LV_HAVE_AVX2
 #endif // LV_HAVE_AVX512
 
+  // Create LDPC configuration arguments
+  srsran_ldpc_decoder_args_t decoder_args = {};
+  decoder_args.bg                         = base_graph;
+  decoder_args.ls                         = lift_size;
+  decoder_args.scaling_fctr               = MS_SF;
+
   // create an LDPC decoder (float)
   srsran_ldpc_decoder_t decoder_f;
-  if (srsran_ldpc_decoder_init(&decoder_f, SRSRAN_LDPC_DECODER_F, base_graph, lift_size, MS_SF) != 0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_F;
+  if (srsran_ldpc_decoder_init(&decoder_f, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
   // create an LDPC decoder (16 bit)
   srsran_ldpc_decoder_t decoder_s;
-  if (srsran_ldpc_decoder_init(&decoder_s, SRSRAN_LDPC_DECODER_S, base_graph, lift_size, MS_SF) != 0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_S;
+  if (srsran_ldpc_decoder_init(&decoder_s, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
   // create an LDPC decoder (8 bit)
   srsran_ldpc_decoder_t decoder_c;
-  if (srsran_ldpc_decoder_init(&decoder_c, SRSRAN_LDPC_DECODER_C, base_graph, lift_size, MS_SF) != 0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_C;
+  if (srsran_ldpc_decoder_init(&decoder_c, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
   // create an LDPC decoder (8 bit, flooded)
   srsran_ldpc_decoder_t decoder_c_flood;
-  if (srsran_ldpc_decoder_init(&decoder_c_flood, SRSRAN_LDPC_DECODER_C_FLOOD, base_graph, lift_size, MS_SF) != 0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_C_FLOOD;
+  if (srsran_ldpc_decoder_init(&decoder_c_flood, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
 #ifdef LV_HAVE_AVX2
   // create an LDPC decoder (8 bit, AVX2 version)
   srsran_ldpc_decoder_t decoder_avx;
-  if (srsran_ldpc_decoder_init(&decoder_avx, SRSRAN_LDPC_DECODER_C_AVX2, base_graph, lift_size, MS_SF) != 0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_C_AVX2;
+  if (srsran_ldpc_decoder_init(&decoder_avx, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
 
   // create an LDPC decoder (8 bit, flooded scheduling, AVX2 version)
   srsran_ldpc_decoder_t decoder_avx_flood;
-  if (srsran_ldpc_decoder_init(&decoder_avx_flood, SRSRAN_LDPC_DECODER_C_AVX2_FLOOD, base_graph, lift_size, MS_SF) !=
-      0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_C_AVX2_FLOOD;
+  if (srsran_ldpc_decoder_init(&decoder_avx_flood, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
@@ -221,15 +232,16 @@ int main(int argc, char** argv)
 #ifdef LV_HAVE_AVX512
   // create an LDPC decoder (8 bit, AVX512 version)
   srsran_ldpc_decoder_t decoder_avx512;
-  if (srsran_ldpc_decoder_init(&decoder_avx512, SRSRAN_LDPC_DECODER_C_AVX512, base_graph, lift_size, MS_SF) != 0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_C_AVX512;
+  if (srsran_ldpc_decoder_init(&decoder_avx512, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
 
   // create an LDPC decoder (8 bit, flooded scheduling, AVX512 version)
   srsran_ldpc_decoder_t decoder_avx512_flood;
-  if (srsran_ldpc_decoder_init(
-          &decoder_avx512_flood, SRSRAN_LDPC_DECODER_C_AVX512_FLOOD, base_graph, lift_size, MS_SF) != 0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_C_AVX512_FLOOD;
+  if (srsran_ldpc_decoder_init(&decoder_avx512_flood, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
diff --git a/lib/src/phy/fec/ldpc/test/ldpc_dec_avx2_test.c b/lib/src/phy/fec/ldpc/test/ldpc_dec_avx2_test.c
index b3c2e1a56..5e550fa3d 100644
--- a/lib/src/phy/fec/ldpc/test/ldpc_dec_avx2_test.c
+++ b/lib/src/phy/fec/ldpc/test/ldpc_dec_avx2_test.c
@@ -162,9 +162,16 @@ int main(int argc, char** argv)
   srsran_ldpc_decoder_type_t dectype =
       (scheduling == 0) ? SRSRAN_LDPC_DECODER_C_AVX2 : SRSRAN_LDPC_DECODER_C_AVX2_FLOOD;
 
+  // Create LDPC configuration arguments
+  srsran_ldpc_decoder_args_t decoder_args = {};
+  decoder_args.type                       = dectype;
+  decoder_args.bg                         = base_graph;
+  decoder_args.ls                         = lift_size;
+  decoder_args.scaling_fctr               = 1.0f;
+
   // create an LDPC decoder
   srsran_ldpc_decoder_t decoder;
-  if (srsran_ldpc_decoder_init(&decoder, dectype, base_graph, lift_size, 1) != 0) {
+  if (srsran_ldpc_decoder_init(&decoder, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
diff --git a/lib/src/phy/fec/ldpc/test/ldpc_dec_avx512_test.c b/lib/src/phy/fec/ldpc/test/ldpc_dec_avx512_test.c
index c13086f03..4ade39c8e 100644
--- a/lib/src/phy/fec/ldpc/test/ldpc_dec_avx512_test.c
+++ b/lib/src/phy/fec/ldpc/test/ldpc_dec_avx512_test.c
@@ -161,9 +161,16 @@ int main(int argc, char** argv)
   srsran_ldpc_decoder_type_t dectype =
       (scheduling == 0) ? SRSRAN_LDPC_DECODER_C_AVX512 : SRSRAN_LDPC_DECODER_C_AVX512_FLOOD;
 
+  // Create LDPC configuration arguments
+  srsran_ldpc_decoder_args_t decoder_args = {};
+  decoder_args.type                       = dectype;
+  decoder_args.bg                         = base_graph;
+  decoder_args.ls                         = lift_size;
+  decoder_args.scaling_fctr               = 1.0f;
+
   // create an LDPC decoder
   srsran_ldpc_decoder_t decoder;
-  if (srsran_ldpc_decoder_init(&decoder, dectype, base_graph, lift_size, 1) != 0) {
+  if (srsran_ldpc_decoder_init(&decoder, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
diff --git a/lib/src/phy/fec/ldpc/test/ldpc_dec_c_test.c b/lib/src/phy/fec/ldpc/test/ldpc_dec_c_test.c
index df011aeb2..5f0f87b61 100644
--- a/lib/src/phy/fec/ldpc/test/ldpc_dec_c_test.c
+++ b/lib/src/phy/fec/ldpc/test/ldpc_dec_c_test.c
@@ -155,9 +155,16 @@ int main(int argc, char** argv)
 
   srsran_ldpc_decoder_type_t dectype = (scheduling == 0) ? SRSRAN_LDPC_DECODER_C : SRSRAN_LDPC_DECODER_C_FLOOD;
 
+  // Create LDPC configuration arguments
+  srsran_ldpc_decoder_args_t decoder_args = {};
+  decoder_args.type                       = dectype;
+  decoder_args.bg                         = base_graph;
+  decoder_args.ls                         = lift_size;
+  decoder_args.scaling_fctr               = 1.0f;
+
   // create an LDPC decoder
   srsran_ldpc_decoder_t decoder;
-  if (srsran_ldpc_decoder_init(&decoder, dectype, base_graph, lift_size, 1) != 0) {
+  if (srsran_ldpc_decoder_init(&decoder, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
diff --git a/lib/src/phy/fec/ldpc/test/ldpc_dec_s_test.c b/lib/src/phy/fec/ldpc/test/ldpc_dec_s_test.c
index a9bab0100..ee1190e2a 100644
--- a/lib/src/phy/fec/ldpc/test/ldpc_dec_s_test.c
+++ b/lib/src/phy/fec/ldpc/test/ldpc_dec_s_test.c
@@ -148,9 +148,16 @@ int main(int argc, char** argv)
 
   parse_args(argc, argv);
 
+  // Create LDPC configuration arguments
+  srsran_ldpc_decoder_args_t decoder_args = {};
+  decoder_args.type                       = SRSRAN_LDPC_DECODER_S;
+  decoder_args.bg                         = base_graph;
+  decoder_args.ls                         = lift_size;
+  decoder_args.scaling_fctr               = 1.0f;
+
   // create an LDPC decoder
   srsran_ldpc_decoder_t decoder;
-  if (srsran_ldpc_decoder_init(&decoder, SRSRAN_LDPC_DECODER_S, base_graph, lift_size, 1) != 0) {
+  if (srsran_ldpc_decoder_init(&decoder, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
diff --git a/lib/src/phy/fec/ldpc/test/ldpc_dec_test.c b/lib/src/phy/fec/ldpc/test/ldpc_dec_test.c
index ffd3e9419..f2fa8c3c2 100644
--- a/lib/src/phy/fec/ldpc/test/ldpc_dec_test.c
+++ b/lib/src/phy/fec/ldpc/test/ldpc_dec_test.c
@@ -148,9 +148,16 @@ int main(int argc, char** argv)
 
   parse_args(argc, argv);
 
+  // Create LDPC configuration arguments
+  srsran_ldpc_decoder_args_t decoder_args = {};
+  decoder_args.type                       = SRSRAN_LDPC_DECODER_F;
+  decoder_args.bg                         = base_graph;
+  decoder_args.ls                         = lift_size;
+  decoder_args.scaling_fctr               = 1.0f;
+
   // create an LDPC decoder
   srsran_ldpc_decoder_t decoder;
-  if (srsran_ldpc_decoder_init(&decoder, SRSRAN_LDPC_DECODER_F, base_graph, lift_size, 1) != 0) {
+  if (srsran_ldpc_decoder_init(&decoder, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
diff --git a/lib/src/phy/fec/ldpc/test/ldpc_rm_chain_test.c b/lib/src/phy/fec/ldpc/test/ldpc_rm_chain_test.c
index e625acb6b..760181eee 100644
--- a/lib/src/phy/fec/ldpc/test/ldpc_rm_chain_test.c
+++ b/lib/src/phy/fec/ldpc/test/ldpc_rm_chain_test.c
@@ -197,7 +197,7 @@ int main(int argc, char** argv)
     perror("encoder init");
     exit(-1);
   }
-#else  // no AVX2
+#else // no AVX2
   if (srsran_ldpc_encoder_init(&encoder, SRSRAN_LDPC_ENCODER_C, base_graph, lift_size) != 0) {
     perror("encoder init");
     exit(-1);
@@ -243,42 +243,53 @@ int main(int argc, char** argv)
     exit(-1);
   }
 
+  // Create LDPC configuration arguments
+  srsran_ldpc_decoder_args_t decoder_args = {};
+  decoder_args.bg                         = base_graph;
+  decoder_args.ls                         = lift_size;
+  decoder_args.scaling_fctr               = MS_SF;
+
   // create an LDPC decoder (float)
   srsran_ldpc_decoder_t decoder_f;
-  if (srsran_ldpc_decoder_init(&decoder_f, SRSRAN_LDPC_DECODER_F, base_graph, lift_size, MS_SF) != 0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_F;
+  if (srsran_ldpc_decoder_init(&decoder_f, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
   // create an LDPC decoder (16 bit)
   srsran_ldpc_decoder_t decoder_s;
-  if (srsran_ldpc_decoder_init(&decoder_s, SRSRAN_LDPC_DECODER_S, base_graph, lift_size, MS_SF) != 0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_S;
+  if (srsran_ldpc_decoder_init(&decoder_s, &decoder_args) != 0) {
     perror("decoder init (int16_t)");
     exit(-1);
   }
   // create an LDPC decoder (8 bit)
   srsran_ldpc_decoder_t decoder_c;
-  if (srsran_ldpc_decoder_init(&decoder_c, SRSRAN_LDPC_DECODER_C, base_graph, lift_size, MS_SF) != 0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_C;
+  if (srsran_ldpc_decoder_init(&decoder_c, &decoder_args) != 0) {
     perror("decoder init (int8_t)");
     exit(-1);
   }
   // create an LDPC decoder (8 bit, flooded)
   srsran_ldpc_decoder_t decoder_c_flood;
-  if (srsran_ldpc_decoder_init(&decoder_c_flood, SRSRAN_LDPC_DECODER_C_FLOOD, base_graph, lift_size, MS_SF) != 0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_C_FLOOD;
+  if (srsran_ldpc_decoder_init(&decoder_c_flood, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
 #ifdef LV_HAVE_AVX2
   // create an LDPC decoder (8 bit, AVX2 version)
   srsran_ldpc_decoder_t decoder_avx;
-  if (srsran_ldpc_decoder_init(&decoder_avx, SRSRAN_LDPC_DECODER_C_AVX2, base_graph, lift_size, MS_SF) != 0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_C_AVX2;
+  if (srsran_ldpc_decoder_init(&decoder_avx, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
 
   // create an LDPC decoder (8 bit, flooded scheduling, AVX2 version)
   srsran_ldpc_decoder_t decoder_avx_flood;
-  if (srsran_ldpc_decoder_init(&decoder_avx_flood, SRSRAN_LDPC_DECODER_C_AVX2_FLOOD, base_graph, lift_size, MS_SF) !=
-      0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_C_AVX2_FLOOD;
+  if (srsran_ldpc_decoder_init(&decoder_avx_flood, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
@@ -287,15 +298,16 @@ int main(int argc, char** argv)
 #ifdef LV_HAVE_AVX512
   // create an LDPC decoder (8 bit, AVX2 version)
   srsran_ldpc_decoder_t decoder_avx512;
-  if (srsran_ldpc_decoder_init(&decoder_avx512, SRSRAN_LDPC_DECODER_C_AVX512, base_graph, lift_size, MS_SF) != 0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_C_AVX512;
+  if (srsran_ldpc_decoder_init(&decoder_avx512, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
 
   // create an LDPC decoder (8 bit, flooded scheduling, AVX512 version)
   srsran_ldpc_decoder_t decoder_avx512_flood;
-  if (srsran_ldpc_decoder_init(
-          &decoder_avx512_flood, SRSRAN_LDPC_DECODER_C_AVX512_FLOOD, base_graph, lift_size, MS_SF) != 0) {
+  decoder_args.type = SRSRAN_LDPC_DECODER_C_AVX512_FLOOD;
+  if (srsran_ldpc_decoder_init(&decoder_avx512_flood, &decoder_args) != 0) {
     perror("decoder init");
     exit(-1);
   }
diff --git a/lib/src/phy/fec/utils_avx512.h b/lib/src/phy/fec/utils_avx512.h
index 666402f8e..3c5e7e460 100644
--- a/lib/src/phy/fec/utils_avx512.h
+++ b/lib/src/phy/fec/utils_avx512.h
@@ -35,4 +35,21 @@
 #define SRSRAN_AVX512_B_SIZE 64    /*!< \brief Number of packed bytes in an AVX512 instruction. */
 #define SRSRAN_AVX512_B_SIZE_LOG 6 /*!< \brief \f$\log_2\f$ of \ref SRSRAN_AVX512_B_SIZE. */
 
+#ifdef LV_HAVE_AVX512
+
+#include <immintrin.h>
+
+static inline void fec_avx512_hard_decision_c(const int8_t* llr, uint8_t* message, int nof_llr)
+{
+  int k = 0;
+  for (; k < nof_llr - (SRSRAN_AVX512_B_SIZE - 1); k += SRSRAN_AVX512_B_SIZE) {
+    __mmask64 mask = _mm512_cmpge_epi8_mask(_mm512_load_si512((__m512i*)&llr[k]), _mm512_set1_epi8(0));
+    _mm512_storeu_si512((__m512i*)&message[k], _mm512_mask_blend_epi8(mask, _mm512_set1_epi8(1), _mm512_set1_epi8(0)));
+  }
+  for (; k < nof_llr; k++) {
+    message[k] = (llr[k] < 0);
+  }
+}
+#endif // LV_HAVE_AVX512
+
 #endif // SRSRAN_UTILS_AVX512_H
diff --git a/lib/src/phy/modem/demod_soft.c b/lib/src/phy/modem/demod_soft.c
index 26517cb9c..d9e82169b 100644
--- a/lib/src/phy/modem/demod_soft.c
+++ b/lib/src/phy/modem/demod_soft.c
@@ -892,6 +892,9 @@ int srsran_demod_soft_demodulate(srsran_mod_t modulation, const cf_t* symbols, f
 
 int srsran_demod_soft_demodulate_s(srsran_mod_t modulation, const cf_t* symbols, short* llr, int nsymbols)
 {
+  if (symbols == NULL || llr == NULL) {
+    return SRSRAN_ERROR_INVALID_INPUTS;
+  }
   switch (modulation) {
     case SRSRAN_MOD_BPSK:
       demod_bpsk_lte_s(symbols, llr, nsymbols);
@@ -917,6 +920,9 @@ int srsran_demod_soft_demodulate_s(srsran_mod_t modulation, const cf_t* symbols,
 
 int srsran_demod_soft_demodulate_b(srsran_mod_t modulation, const cf_t* symbols, int8_t* llr, int nsymbols)
 {
+  if (symbols == NULL || llr == NULL) {
+    return SRSRAN_ERROR_INVALID_INPUTS;
+  }
   switch (modulation) {
     case SRSRAN_MOD_BPSK:
       demod_bpsk_lte_b(symbols, llr, nsymbols);
@@ -935,7 +941,34 @@ int srsran_demod_soft_demodulate_b(srsran_mod_t modulation, const cf_t* symbols,
       break;
     default:
       ERROR("Invalid modulation %d", modulation);
-      return -1;
+      return SRSRAN_ERROR;
   }
-  return 0;
+  return SRSRAN_SUCCESS;
 }
+
+int srsran_demod_soft_demodulate2_b(srsran_mod_t modulation, const cf_t* symbols, int8_t* llr, int nsymbols)
+{
+  if (symbols == NULL || llr == NULL) {
+    return SRSRAN_ERROR_INVALID_INPUTS;
+  }
+  if (srsran_demod_soft_demodulate_b(modulation, symbols, llr, nsymbols) < SRSRAN_SUCCESS) {
+    return SRSRAN_ERROR;
+  }
+
+  // If the number of bits is 2 or less, this is unnecessary
+  if (modulation < SRSRAN_MOD_16QAM) {
+    return SRSRAN_SUCCESS;
+  }
+
+  // Iterate all symbols seeking for zero LLR
+  uint32_t nof_bits_x_symbol = srsran_mod_bits_x_symbol(modulation);
+  for (uint32_t i = 0; i < nsymbols; i++) {
+    if (symbols[i] == 0.0f) {
+      for (uint32_t j = 0; j < nof_bits_x_symbol; j++) {
+        llr[i * nof_bits_x_symbol + j] = 0;
+      }
+    }
+  }
+
+  return SRSRAN_SUCCESS;
+}
\ No newline at end of file
diff --git a/lib/src/phy/modem/test/soft_demod_test.c b/lib/src/phy/modem/test/soft_demod_test.c
index 2b4b6349e..730532fc1 100644
--- a/lib/src/phy/modem/test/soft_demod_test.c
+++ b/lib/src/phy/modem/test/soft_demod_test.c
@@ -19,11 +19,8 @@
  *
  */
 
-#include <math.h>
-#include <stdbool.h>
 #include <stdio.h>
 #include <stdlib.h>
-#include <string.h>
 #include <strings.h>
 #include <time.h>
 #include <unistd.h>
@@ -34,7 +31,7 @@ static uint32_t     nof_frames = 10;
 static uint32_t     num_bits   = 1000;
 static srsran_mod_t modulation = SRSRAN_MOD_NITEMS;
 
-void usage(char* prog)
+static void usage(char* prog)
 {
   printf("Usage: %s [nfv] -m modulation (1: BPSK, 2: QPSK, 4: QAM16, 6: QAM64)\n", prog);
   printf("\t-n num_bits [Default %d]\n", num_bits);
@@ -42,7 +39,7 @@ void usage(char* prog)
   printf("\t-v srsran_verbose [Default None]\n");
 }
 
-void parse_args(int argc, char** argv)
+static void parse_args(int argc, char** argv)
 {
   int opt;
   while ((opt = getopt(argc, argv, "nmvf")) != -1) {
@@ -91,33 +88,16 @@ void parse_args(int argc, char** argv)
   }
 }
 
-float mse_threshold()
-{
-  switch (modulation) {
-    case SRSRAN_MOD_BPSK:
-      return 1.0e-6;
-    case SRSRAN_MOD_QPSK:
-      return 1.0e-6;
-    case SRSRAN_MOD_16QAM:
-      return 0.11;
-    case SRSRAN_MOD_64QAM:
-      return 0.19;
-    case SRSRAN_MOD_256QAM:
-      return 0.3;
-    default:
-      return -1.0f;
-  }
-}
-
 int main(int argc, char** argv)
 {
-  int                  i;
   srsran_modem_table_t mod;
-  uint8_t *            input, *output;
-  cf_t*                symbols;
-  float*               llr;
-  short*               llr_s;
-  int8_t*              llr_b;
+  uint8_t*             input      = NULL;
+  cf_t*                symbols    = NULL;
+  float*               llr        = NULL;
+  short*               llr_s      = NULL;
+  int8_t*              llr_b      = NULL;
+  int8_t*              llr_b2     = NULL;
+  srsran_random_t      random_gen = srsran_random_init(0);
 
   parse_args(argc, argv);
 
@@ -136,11 +116,6 @@ int main(int argc, char** argv)
     perror("malloc");
     exit(-1);
   }
-  output = srsran_vec_u8_malloc(num_bits);
-  if (!output) {
-    perror("malloc");
-    exit(-1);
-  }
   symbols = srsran_vec_cf_malloc(num_bits / mod.nbits_x_symbol);
   if (!symbols) {
     perror("malloc");
@@ -165,17 +140,21 @@ int main(int argc, char** argv)
     exit(-1);
   }
 
-  /* generate random data */
-  srand(0);
+  llr_b2 = srsran_vec_i8_malloc(num_bits);
+  if (!llr_b2) {
+    perror("malloc");
+    exit(-1);
+  }
 
   int            ret = -1;
   struct timeval t[3];
-  float          mean_texec   = 0.0;
-  float          mean_texec_s = 0.0;
-  float          mean_texec_b = 0.0;
+  float          mean_texec    = 0.0f;
+  float          mean_texec_s  = 0.0f;
+  float          mean_texec_b  = 0.0f;
+  float          mean_texec_b2 = 0.0f;
   for (int n = 0; n < nof_frames; n++) {
-    for (i = 0; i < num_bits; i++) {
-      input[i] = rand() % 2;
+    for (int i = 0; i < num_bits; i++) {
+      input[i] = srsran_random_uniform_int_dist(random_gen, 0, 1);
     }
 
     /* modulate */
@@ -209,6 +188,15 @@ int main(int argc, char** argv)
       mean_texec_b = SRSRAN_VEC_CMA((float)t[0].tv_usec, mean_texec_b, n - 1);
     }
 
+    gettimeofday(&t[1], NULL);
+    srsran_demod_soft_demodulate2_b(modulation, symbols, llr_b2, num_bits / mod.nbits_x_symbol);
+    gettimeofday(&t[2], NULL);
+    get_time_interval(t);
+
+    if (n > 0) {
+      mean_texec_b2 = SRSRAN_VEC_CMA((float)t[0].tv_usec, mean_texec_b2, n - 1);
+    }
+
     if (SRSRAN_VERBOSE_ISDEBUG()) {
       printf("bits=");
       srsran_vec_fprint_b(stdout, input, num_bits);
@@ -224,12 +212,27 @@ int main(int argc, char** argv)
 
       printf("llr_b=");
       srsran_vec_fprint_bs(stdout, llr_b, num_bits);
+
+      printf("llr_b2=");
+      srsran_vec_fprint_bs(stdout, llr_b2, num_bits);
     }
 
     // Check demodulation errors
-    for (int i = 0; i < num_bits; i++) {
-      if (input[i] != (llr[i] > 0 ? 1 : 0)) {
-        printf("Error in bit %d\n", i);
+    for (int j = 0; j < num_bits; j++) {
+      if (input[j] != (llr[j] > 0 ? 1 : 0)) {
+        ERROR("Error in bit %d\n", j);
+        goto clean_exit;
+      }
+      if (input[j] != (llr_s[j] > 0 ? 1 : 0)) {
+        ERROR("Error in bit %d\n", j);
+        goto clean_exit;
+      }
+      if (input[j] != (llr_b[j] > 0 ? 1 : 0)) {
+        ERROR("Error in bit %d\n", j);
+        goto clean_exit;
+      }
+      if (input[j] != (llr_b2[j] > 0 ? 1 : 0)) {
+        ERROR("Error in bit %d\n", j);
         goto clean_exit;
       }
     }
@@ -237,21 +240,23 @@ int main(int argc, char** argv)
   ret = 0;
 
 clean_exit:
+  srsran_random_free(random_gen);
   free(llr_b);
   free(llr_s);
   free(llr);
   free(symbols);
-  free(output);
   free(input);
 
   srsran_modem_table_free(&mod);
 
-  printf("Mean Throughput: %.2f/%.2f/%.2f. Mbps ExTime: %.2f/%.2f/%.2f us\n",
+  printf("Mean Throughput: %.2f/%.2f/%.2f/%.2f. Mbps ExTime: %.2f/%.2f/%.2f/%.2f us\n",
          num_bits / mean_texec,
          num_bits / mean_texec_s,
          num_bits / mean_texec_b,
+         num_bits / mean_texec_b2,
          mean_texec,
          mean_texec_s,
-         mean_texec_b);
+         mean_texec_b,
+         mean_texec_b2);
   exit(ret);
 }
diff --git a/lib/src/phy/phch/pdcch_nr.c b/lib/src/phy/phch/pdcch_nr.c
index 2eeb60485..c83bea69b 100644
--- a/lib/src/phy/phch/pdcch_nr.c
+++ b/lib/src/phy/phch/pdcch_nr.c
@@ -33,6 +33,7 @@
 
 #define PDCCH_INFO_TX(...) INFO("PDCCH Tx: " __VA_ARGS__)
 #define PDCCH_INFO_RX(...) INFO("PDCCH Rx: " __VA_ARGS__)
+#define PDCCH_DEBUG_RX(...) DEBUG("PDCCH Rx: " __VA_ARGS__)
 
 /**
  * @brief Recursive Y_p_n function
@@ -482,8 +483,8 @@ int srsran_pdcch_nr_decode(srsran_pdcch_nr_t*      q,
   }
 
   // Print channel estimates if enabled
-  if (SRSRAN_DEBUG_ENABLED && srsran_verbose >= SRSRAN_VERBOSE_INFO && !handler_registered) {
-    PDCCH_INFO_RX("ce=");
+  if (SRSRAN_DEBUG_ENABLED && srsran_verbose >= SRSRAN_VERBOSE_DEBUG && !handler_registered) {
+    PDCCH_DEBUG_RX("ce=");
     srsran_vec_fprint_c(stdout, ce->ce, q->M);
   }
 
@@ -491,8 +492,8 @@ int srsran_pdcch_nr_decode(srsran_pdcch_nr_t*      q,
   srsran_predecoding_single(q->symbols, ce->ce, q->symbols, NULL, q->M, 1.0f, ce->noise_var);
 
   // Print symbols if enabled
-  if (SRSRAN_DEBUG_ENABLED && srsran_verbose >= SRSRAN_VERBOSE_INFO && !handler_registered) {
-    PDCCH_INFO_RX("symbols=");
+  if (SRSRAN_DEBUG_ENABLED && srsran_verbose >= SRSRAN_VERBOSE_DEBUG && !handler_registered) {
+    PDCCH_DEBUG_RX("symbols=");
     srsran_vec_fprint_c(stdout, q->symbols, q->M);
   }
 
@@ -522,8 +523,8 @@ int srsran_pdcch_nr_decode(srsran_pdcch_nr_t*      q,
   }
 
   // Print d
-  if (SRSRAN_DEBUG_ENABLED && srsran_verbose >= SRSRAN_VERBOSE_INFO && !handler_registered) {
-    PDCCH_INFO_RX("d=");
+  if (SRSRAN_DEBUG_ENABLED && srsran_verbose >= SRSRAN_VERBOSE_DEBUG && !handler_registered) {
+    PDCCH_DEBUG_RX("d=");
     srsran_vec_fprint_bs(stdout, d, q->K);
   }
 
diff --git a/lib/src/phy/phch/pdsch_nr.c b/lib/src/phy/phch/pdsch_nr.c
index 14afbe2e1..a98620ce6 100644
--- a/lib/src/phy/phch/pdsch_nr.c
+++ b/lib/src/phy/phch/pdsch_nr.c
@@ -25,10 +25,14 @@
 #include "srsran/phy/mimo/layermap.h"
 #include "srsran/phy/mimo/precoding.h"
 #include "srsran/phy/modem/demod_soft.h"
-#include "srsran/phy/phch/ra_nr.h"
+
+///@brief Default number of zero RE around DC
+#define PDSCH_NR_DEFAULT_NOF_ZERO_RE_AROUND_DC 3
 
 int pdsch_nr_init_common(srsran_pdsch_nr_t* q, const srsran_pdsch_nr_args_t* args)
 {
+  SRSRAN_MEM_ZERO(q, srsran_pdsch_nr_t, 1);
+
   for (srsran_mod_t mod = SRSRAN_MOD_BPSK; mod < SRSRAN_MOD_NITEMS; mod++) {
     if (srsran_modem_table_lte(&q->modem_tables[mod], mod) < SRSRAN_SUCCESS) {
       ERROR("Error initialising modem table for %s", srsran_mod_string(mod));
@@ -39,6 +43,14 @@ int pdsch_nr_init_common(srsran_pdsch_nr_t* q, const srsran_pdsch_nr_args_t* arg
     }
   }
 
+  if (!args->disable_zero_re_around_dc) {
+    if (args->nof_zero_re_around_dc == 0) {
+      q->nof_zero_re_around_dc = PDSCH_NR_DEFAULT_NOF_ZERO_RE_AROUND_DC;
+    } else {
+      q->nof_zero_re_around_dc = args->nof_zero_re_around_dc;
+    }
+  }
+
   return SRSRAN_SUCCESS;
 }
 
@@ -245,7 +257,23 @@ static int srsran_pdsch_nr_cp(const srsran_pdsch_nr_t*     q,
       if (put) {
         count += pdsch_nr_put_rb(&sf_symbols[re_idx], &symbols[count], &rvd_mask[rb * SRSRAN_NRE]);
       } else {
-        count += pdsch_nr_get_rb(&symbols[count], &sf_symbols[re_idx], &rvd_mask[rb * SRSRAN_NRE]);
+        uint32_t k_begin    = rb * SRSRAN_NRE;
+        uint32_t k_end      = (rb + 1) * SRSRAN_NRE;
+        uint32_t k_dc_begin = q->carrier.nof_prb * SRSRAN_NRE / 2 - q->nof_zero_re_around_dc / 2;
+        uint32_t k_dc_end   = q->carrier.nof_prb * SRSRAN_NRE / 2 + SRSRAN_CEIL(q->nof_zero_re_around_dc, 2);
+        if (k_begin <= k_dc_end && k_end >= k_dc_begin && q->nof_zero_re_around_dc > 0) {
+          for (uint32_t k = k_begin; k < k_end; k++) {
+            if (!rvd_mask[k]) {
+              if (k >= k_dc_begin && k < k_dc_end) {
+                symbols[count++] = 0.0f;
+              } else {
+                symbols[count++] = sf_symbols[q->carrier.nof_prb * l * SRSRAN_NRE + k];
+              }
+            }
+          }
+        } else {
+          count += pdsch_nr_get_rb(&symbols[count], &sf_symbols[re_idx], &rvd_mask[rb * SRSRAN_NRE]);
+        }
       }
     }
   }
@@ -437,13 +465,14 @@ static inline int pdsch_nr_decode_codeword(srsran_pdsch_nr_t*         q,
 
   // Demodulation
   int8_t* llr = (int8_t*)q->b[tb->cw_idx];
-  if (srsran_demod_soft_demodulate_b(tb->mod, q->d[tb->cw_idx], llr, tb->nof_re)) {
+  if (srsran_demod_soft_demodulate2_b(tb->mod, q->d[tb->cw_idx], llr, tb->nof_re)) {
     return SRSRAN_ERROR;
   }
 
   // EVM
   if (q->evm_buffer != NULL) {
-    res->evm = srsran_evm_run_b(q->evm_buffer, &q->modem_tables[tb->mod], q->d[tb->cw_idx], llr, tb->nof_bits);
+    res->evm[tb->cw_idx] =
+        srsran_evm_run_b(q->evm_buffer, &q->modem_tables[tb->mod], q->d[tb->cw_idx], llr, tb->nof_bits);
   }
 
   // Change LLR sign and set to zero the LLR that are not used
@@ -458,7 +487,7 @@ static inline int pdsch_nr_decode_codeword(srsran_pdsch_nr_t*         q,
   }
 
   // Decode SCH
-  if (srsran_dlsch_nr_decode(&q->sch, &cfg->sch_cfg, tb, llr, res->payload, &res->crc) < SRSRAN_SUCCESS) {
+  if (srsran_dlsch_nr_decode(&q->sch, &cfg->sch_cfg, tb, llr, &res->tb[tb->cw_idx]) < SRSRAN_SUCCESS) {
     ERROR("Error in DL-SCH encoding");
     return SRSRAN_ERROR;
   }
@@ -547,13 +576,15 @@ int srsran_pdsch_nr_decode(srsran_pdsch_nr_t*           q,
   return SRSRAN_SUCCESS;
 }
 
-static uint32_t srsran_pdsch_nr_grant_info(const srsran_sch_cfg_nr_t*   cfg,
-                                           const srsran_sch_grant_nr_t* grant,
-                                           char*                        str,
-                                           uint32_t                     str_len)
+static uint32_t pdsch_nr_grant_info(const srsran_pdsch_nr_t*     q,
+                                    const srsran_sch_cfg_nr_t*   cfg,
+                                    const srsran_sch_grant_nr_t* grant,
+                                    const srsran_pdsch_res_nr_t* res,
+                                    char*                        str,
+                                    uint32_t                     str_len)
 {
   uint32_t len = 0;
-  len          = srsran_print_check(str, str_len, len, "rnti=0x%x", grant->rnti);
+  len          = srsran_print_check(str, str_len, len, "rnti=0x%x ", grant->rnti);
 
   uint32_t first_prb = SRSRAN_MAX_PRB_NR;
   for (uint32_t i = 0; i < SRSRAN_MAX_PRB_NR && first_prb == SRSRAN_MAX_PRB_NR; i++) {
@@ -566,7 +597,9 @@ static uint32_t srsran_pdsch_nr_grant_info(const srsran_sch_cfg_nr_t*   cfg,
   len = srsran_print_check(str,
                            str_len,
                            len,
-                           ",k0=%d,prb=%d:%d,symb=%d:%d,mapping=%s",
+                           "beta_dmrs=%.3f CDM-grp=%d k0=%d prb=%d:%d symb=%d:%d mapping=%s ",
+                           isnormal(grant->beta_dmrs) ? grant->beta_dmrs : 1.0f,
+                           grant->nof_dmrs_cdm_groups_without_data,
                            grant->k,
                            first_prb,
                            grant->nof_prb,
@@ -578,14 +611,22 @@ static uint32_t srsran_pdsch_nr_grant_info(const srsran_sch_cfg_nr_t*   cfg,
   // ...
 
   // Append spatial resources
-  len = srsran_print_check(str, str_len, len, ",Nl=%d", grant->nof_layers);
+  len = srsran_print_check(str, str_len, len, "Nl=%d ", grant->nof_layers);
 
   // Append scrambling ID
-  len = srsran_print_check(str, str_len, len, ",n_scid=%d,", grant->n_scid);
+  len = srsran_print_check(str, str_len, len, "n_scid=%d ", grant->n_scid);
 
   // Append TB info
   for (uint32_t i = 0; i < SRSRAN_MAX_TB; i++) {
-    len += srsran_sch_nr_tb_info(&grant->tb[i], &str[len], str_len - len);
+    len += srsran_sch_nr_tb_info(&grant->tb[i], &res->tb[i], &str[len], str_len - len);
+
+    if (res != NULL) {
+      if (grant->tb[i].enabled && !isnan(res->evm[i])) {
+        len = srsran_print_check(str, str_len, len, "evm=%.2f ", res->evm[i]);
+        if (i < SRSRAN_MAX_CODEWORDS - 1) {
+        }
+      }
+    }
   }
 
   return len;
@@ -594,52 +635,21 @@ static uint32_t srsran_pdsch_nr_grant_info(const srsran_sch_cfg_nr_t*   cfg,
 uint32_t srsran_pdsch_nr_rx_info(const srsran_pdsch_nr_t*     q,
                                  const srsran_sch_cfg_nr_t*   cfg,
                                  const srsran_sch_grant_nr_t* grant,
-                                 const srsran_pdsch_res_nr_t  res[SRSRAN_MAX_CODEWORDS],
+                                 const srsran_pdsch_res_nr_t* res,
                                  char*                        str,
                                  uint32_t                     str_len)
 {
   uint32_t len = 0;
 
-  len += srsran_pdsch_nr_grant_info(cfg, grant, &str[len], str_len - len);
+  len += pdsch_nr_grant_info(q, cfg, grant, res, &str[len], str_len - len);
 
   if (cfg->rvd_re.count != 0) {
-    len = srsran_print_check(str, str_len, len, ", Reserved={");
+    len = srsran_print_check(str, str_len, len, "Reserved: ");
     len += srsran_re_pattern_list_info(&cfg->rvd_re, &str[len], str_len - len);
-    len = srsran_print_check(str, str_len, len, "}");
-  }
-
-  if (q->evm_buffer != NULL) {
-    len = srsran_print_check(str, str_len, len, ",evm={", 0);
-    for (uint32_t i = 0; i < SRSRAN_MAX_CODEWORDS; i++) {
-      if (grant->tb[i].enabled && !isnan(res[i].evm)) {
-        len = srsran_print_check(str, str_len, len, "%.2f", res[i].evm);
-        if (i < SRSRAN_MAX_CODEWORDS - 1) {
-          if (grant->tb[i + 1].enabled) {
-            len = srsran_print_check(str, str_len, len, ",", 0);
-          }
-        }
-      }
-    }
-    len = srsran_print_check(str, str_len, len, "}", 0);
-  }
-
-  if (res != NULL) {
-    len = srsran_print_check(str, str_len, len, ",crc={", 0);
-    for (uint32_t i = 0; i < SRSRAN_MAX_CODEWORDS; i++) {
-      if (grant->tb[i].enabled) {
-        len = srsran_print_check(str, str_len, len, "%s", res[i].crc ? "OK" : "KO");
-        if (i < SRSRAN_MAX_CODEWORDS - 1) {
-          if (grant->tb[i + 1].enabled) {
-            len = srsran_print_check(str, str_len, len, ",", 0);
-          }
-        }
-      }
-    }
-    len = srsran_print_check(str, str_len, len, "}", 0);
   }
 
   if (q->meas_time_en) {
-    len = srsran_print_check(str, str_len, len, ", t=%d us", q->meas_time_us);
+    len = srsran_print_check(str, str_len, len, " t=%d us", q->meas_time_us);
   }
 
   return len;
diff --git a/lib/src/phy/phch/prach.c b/lib/src/phy/phch/prach.c
index ff6df7af7..c969665bc 100644
--- a/lib/src/phy/phch/prach.c
+++ b/lib/src/phy/phch/prach.c
@@ -123,6 +123,14 @@ srsran_prach_sfn_t srsran_prach_get_sfn(uint32_t config_idx)
  */
 bool srsran_prach_tti_opportunity(srsran_prach_t* p, uint32_t current_tti, int allowed_subframe)
 {
+  if (p == NULL) {
+    return false;
+  }
+
+  if (p->is_nr) {
+    return srsran_prach_nr_tti_opportunity_fr1_unpaired(p->config_idx, current_tti);
+  }
+
   uint32_t config_idx = p->config_idx;
   if (!p->tdd_config.configured) {
     return srsran_prach_tti_opportunity_config_fdd(config_idx, current_tti, allowed_subframe);
@@ -265,6 +273,66 @@ void srsran_prach_sf_config(uint32_t config_idx, srsran_prach_sf_config_t* sf_co
   memcpy(sf_config, &prach_sf_config[config_idx % 16], sizeof(srsran_prach_sf_config_t));
 }
 
+const prach_nr_config_t* srsran_prach_nr_get_cfg_fr1_unpaired(uint32_t config_idx)
+{
+  if (config_idx < PRACH_NR_CFG_FR1_UNPAIRED_NOF_CFG) {
+    return &prach_nr_cfg_fr1_unpaired[config_idx];
+  }
+
+  ERROR("Invalid configuration index %d", config_idx);
+  return NULL;
+}
+
+bool srsran_prach_nr_tti_opportunity_fr1_unpaired(uint32_t config_idx, uint32_t current_tti)
+{
+  uint32_t sfn    = current_tti / SRSRAN_NOF_SF_X_FRAME;
+  uint32_t sf_idx = current_tti % SRSRAN_NOF_SF_X_FRAME;
+
+  // Get configuration
+  const prach_nr_config_t* cfg = srsran_prach_nr_get_cfg_fr1_unpaired(config_idx);
+  if (cfg == NULL) {
+    return false;
+  }
+
+  // Protect zero division
+  if (cfg->x == 0) {
+    ERROR("Invalid Zero value");
+    return false;
+  }
+
+  // Check for System Frame Number match
+  if (sfn % cfg->x != cfg->y) {
+    return false;
+  }
+
+  // Protect subframe number vector access
+  if (cfg->nof_subframe_number > PRACH_NR_CFG_MAX_NOF_SF) {
+    ERROR("Invalid number of subframes (%d)", cfg->nof_subframe_number);
+    return false;
+  }
+
+  // Check for subframe number match
+  for (uint32_t i = 0; i < cfg->nof_subframe_number; i++) {
+    if (cfg->subframe_number[i] == sf_idx) {
+      return true;
+    }
+  }
+
+  // If reached here, no opportunity
+  return false;
+}
+
+uint32_t srsran_prach_nr_start_symbol_fr1_unpaired(uint32_t config_idx)
+{
+  // Get configuration
+  const prach_nr_config_t* cfg = srsran_prach_nr_get_cfg_fr1_unpaired(config_idx);
+  if (cfg == NULL) {
+    return false;
+  }
+
+  return cfg->starting_symbol;
+}
+
 // For debug use only
 void print(void* d, uint32_t size, uint32_t len, char* file_str)
 {
diff --git a/lib/src/phy/phch/prach_tables.h b/lib/src/phy/phch/prach_tables.h
index 1d67a5ae2..68c12d852 100644
--- a/lib/src/phy/phch/prach_tables.h
+++ b/lib/src/phy/phch/prach_tables.h
@@ -442,4 +442,23 @@ srsran_prach_tdd_loc_table_t prach_tdd_loc_table[64][7] = {
      {4, {{0, 0, 0, 0}, {1, 0, 0, 0}, {2, 0, 0, 0}, {3, 0, 0, 0}}},
      {0, {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}},
      {0, {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}},
-     {4, {{0, 0, 0, 0}, {1, 0, 0, 0}, {2, 0, 0, 0}, {3, 0, 0, 0}}}}};
\ No newline at end of file
+     {4, {{0, 0, 0, 0}, {1, 0, 0, 0}, {2, 0, 0, 0}, {3, 0, 0, 0}}}}};
+
+#define PRACH_NR_CFG_FR1_UNPAIRED_NOF_CFG 28
+
+// Table 6.3.3.2-3: Random access configurations for FR1 and unpaired spectrum.
+static const prach_nr_config_t prach_nr_cfg_fr1_unpaired[PRACH_NR_CFG_FR1_UNPAIRED_NOF_CFG] = {
+    {0, 16, 1, {9}, 1, 0},         {0, 8, 1, {9}, 1, 0},
+    {0, 4, 1, {9}, 1, 0},          {0, 2, 0, {9}, 1, 0},
+    {0, 2, 1, {9}, 1, 0},          {0, 2, 0, {4}, 1, 0},
+    {0, 2, 1, {4}, 1, 0},          {0, 1, 0, {9}, 1, 0},
+    {0, 1, 0, {8}, 1, 0},          {0, 1, 0, {7}, 1, 0},
+    {0, 1, 0, {6}, 1, 0},          {0, 1, 0, {5}, 1, 0},
+    {0, 1, 0, {4}, 1, 0},          {0, 1, 0, {3}, 1, 0},
+    {0, 1, 0, {2}, 1, 0},          {0, 1, 0, {1, 6}, 1, 0},
+    {0, 1, 0, {1, 6}, 1, 7},       {0, 1, 0, {4, 9}, 1, 0},
+    {0, 1, 0, {3, 8}, 1, 0},       {0, 1, 0, {2, 7}, 1, 0},
+    {0, 1, 0, {8, 9}, 1, 0},       {0, 1, 0, {4, 8, 9}, 1, 0},
+    {0, 1, 0, {3, 4, 9}, 1, 0},    {0, 1, 0, {7, 8, 9}, 1, 0},
+    {0, 1, 0, {3, 4, 8, 9}, 1, 0}, {0, 1, 0, {6, 7, 8, 9}, 1, 0},
+    {0, 1, 0, {1, 4, 6, 9}, 1, 0}, {0, 1, 0, {1, 3, 5, 7, 9}, 1, 0}};
diff --git a/lib/src/phy/phch/pusch_nr.c b/lib/src/phy/phch/pusch_nr.c
index 32219a32e..ef57db062 100644
--- a/lib/src/phy/phch/pusch_nr.c
+++ b/lib/src/phy/phch/pusch_nr.c
@@ -970,7 +970,7 @@ int srsran_pusch_nr_encode(srsran_pusch_nr_t*            q,
   for (uint32_t tb = 0; tb < SRSRAN_MAX_TB; tb++) {
     nof_cw += grant->tb[tb].enabled ? 1 : 0;
 
-    if (pusch_nr_encode_codeword(q, cfg, &grant->tb[tb], data[tb].payload, &data[0].uci, grant->rnti) <
+    if (pusch_nr_encode_codeword(q, cfg, &grant->tb[tb], data->payload[tb], &data[0].uci, grant->rnti) <
         SRSRAN_SUCCESS) {
       ERROR("Error encoding TB %d", tb);
       return SRSRAN_ERROR;
@@ -1073,7 +1073,8 @@ static inline int pusch_nr_decode_codeword(srsran_pusch_nr_t*         q,
 
   // EVM
   if (q->evm_buffer != NULL) {
-    res->evm = srsran_evm_run_b(q->evm_buffer, &q->modem_tables[tb->mod], q->d[tb->cw_idx], llr, tb->nof_bits);
+    res->evm[tb->cw_idx] =
+        srsran_evm_run_b(q->evm_buffer, &q->modem_tables[tb->mod], q->d[tb->cw_idx], llr, tb->nof_bits);
   }
 
   // Descrambling
@@ -1142,7 +1143,7 @@ static inline int pusch_nr_decode_codeword(srsran_pusch_nr_t*         q,
 
   // Decode Ul-SCH
   if (tb->nof_bits != 0) {
-    if (srsran_ulsch_nr_decode(&q->sch, &cfg->sch_cfg, tb, llr, res->payload, &res->crc) < SRSRAN_SUCCESS) {
+    if (srsran_ulsch_nr_decode(&q->sch, &cfg->sch_cfg, tb, llr, &res->tb[tb->cw_idx]) < SRSRAN_SUCCESS) {
       ERROR("Error in SCH decoding");
       return SRSRAN_ERROR;
     }
@@ -1240,6 +1241,7 @@ int srsran_pusch_nr_decode(srsran_pusch_nr_t*           q,
 
 static uint32_t srsran_pusch_nr_grant_info(const srsran_sch_cfg_nr_t*   cfg,
                                            const srsran_sch_grant_nr_t* grant,
+                                           const srsran_pusch_res_nr_t* res,
                                            char*                        str,
                                            uint32_t                     str_len)
 {
@@ -1276,7 +1278,7 @@ static uint32_t srsran_pusch_nr_grant_info(const srsran_sch_cfg_nr_t*   cfg,
 
   // Append TB info
   for (uint32_t i = 0; i < SRSRAN_MAX_TB; i++) {
-    len += srsran_sch_nr_tb_info(&grant->tb[i], &str[len], str_len - len);
+    len += srsran_sch_nr_tb_info(&grant->tb[i], &res->tb[i], &str[len], str_len - len);
   }
 
   return len;
@@ -1285,7 +1287,7 @@ static uint32_t srsran_pusch_nr_grant_info(const srsran_sch_cfg_nr_t*   cfg,
 uint32_t srsran_pusch_nr_rx_info(const srsran_pusch_nr_t*     q,
                                  const srsran_sch_cfg_nr_t*   cfg,
                                  const srsran_sch_grant_nr_t* grant,
-                                 const srsran_pusch_res_nr_t  res[SRSRAN_MAX_CODEWORDS],
+                                 const srsran_pusch_res_nr_t* res,
                                  char*                        str,
                                  uint32_t                     str_len)
 {
@@ -1295,12 +1297,12 @@ uint32_t srsran_pusch_nr_rx_info(const srsran_pusch_nr_t*     q,
     return 0;
   }
 
-  len += srsran_pusch_nr_grant_info(cfg, grant, &str[len], str_len - len);
+  len += srsran_pusch_nr_grant_info(cfg, grant, res, &str[len], str_len - len);
 
   if (q->evm_buffer != NULL) {
     len = srsran_print_check(str, str_len, len, ",evm={", 0);
     for (uint32_t i = 0; i < SRSRAN_MAX_CODEWORDS; i++) {
-      if (grant->tb[i].enabled && !isnan(res[i].evm)) {
+      if (grant->tb[i].enabled && !isnan(res->evm[i])) {
         len = srsran_print_check(str, str_len, len, "%.2f", res[i].evm);
         if (i < SRSRAN_MAX_CODEWORDS - 1) {
           if (grant->tb[i + 1].enabled) {
@@ -1321,7 +1323,7 @@ uint32_t srsran_pusch_nr_rx_info(const srsran_pusch_nr_t*     q,
     len = srsran_print_check(str, str_len, len, ",crc={", 0);
     for (uint32_t i = 0; i < SRSRAN_MAX_CODEWORDS; i++) {
       if (grant->tb[i].enabled) {
-        len = srsran_print_check(str, str_len, len, "%s", res[i].crc ? "OK" : "KO");
+        len = srsran_print_check(str, str_len, len, "%s", res->tb[i].crc ? "OK" : "KO");
         if (i < SRSRAN_MAX_CODEWORDS - 1) {
           if (grant->tb[i + 1].enabled) {
             len = srsran_print_check(str, str_len, len, ",", 0);
@@ -1352,7 +1354,7 @@ uint32_t srsran_pusch_nr_tx_info(const srsran_pusch_nr_t*     q,
     return 0;
   }
 
-  len += srsran_pusch_nr_grant_info(cfg, grant, &str[len], str_len - len);
+  len += srsran_pusch_nr_grant_info(cfg, grant, NULL, &str[len], str_len - len);
 
   if (uci_value != NULL) {
     srsran_uci_data_nr_t uci_data = {};
diff --git a/lib/src/phy/phch/ra_nr.c b/lib/src/phy/phch/ra_nr.c
index f4a404661..6bac2fce9 100644
--- a/lib/src/phy/phch/ra_nr.c
+++ b/lib/src/phy/phch/ra_nr.c
@@ -685,6 +685,7 @@ int srsran_ra_dl_dci_to_grant_nr(const srsran_carrier_nr_t*    carrier,
   pdsch_grant->rnti       = dci_dl->ctx.rnti;
   pdsch_grant->rnti_type  = dci_dl->ctx.rnti_type;
   pdsch_grant->tb[0].rv   = dci_dl->rv;
+  pdsch_grant->tb[0].mcs  = dci_dl->mcs;
 
   // 5.1.4 PDSCH resource mapping
   if (ra_dl_resource_mapping(carrier, slot, pdsch_hl_cfg, pdsch_cfg) < SRSRAN_SUCCESS) {
@@ -793,6 +794,10 @@ int srsran_ra_ul_dci_to_grant_nr(const srsran_carrier_nr_t*    carrier,
   pusch_grant->dci_format = dci_ul->ctx.format;
   pusch_grant->rnti       = dci_ul->ctx.rnti;
   pusch_grant->rnti_type  = dci_ul->ctx.rnti_type;
+  pusch_grant->tb[0].rv   = dci_ul->rv;
+  pusch_grant->tb[0].mcs  = dci_ul->mcs;
+  pusch_grant->tb[0].ndi  = dci_ul->ndi;
+  pusch_grant->tb[0].pid  = dci_ul->pid;
 
   // 5.1.6.2 DM-RS reception procedure
   if (ra_ul_dmrs(pusch_hl_cfg, pusch_grant, pusch_cfg) < SRSRAN_SUCCESS) {
diff --git a/lib/src/phy/phch/sch_nr.c b/lib/src/phy/phch/sch_nr.c
index 72a55de3a..5d624dc08 100644
--- a/lib/src/phy/phch/sch_nr.c
+++ b/lib/src/phy/phch/sch_nr.c
@@ -284,24 +284,35 @@ int srsran_sch_nr_init_rx(srsran_sch_nr_t* q, const srsran_sch_nr_args_t* args)
       continue;
     }
 
-    q->decoder_bg1[ls] = calloc(1, sizeof(srsran_ldpc_decoder_t));
+    // Initialise LDPC configuration arguments
+    srsran_ldpc_decoder_args_t decoder_args = {};
+    decoder_args.type                       = decoder_type;
+    decoder_args.ls                         = ls;
+    decoder_args.scaling_fctr               = scaling_factor;
+    decoder_args.max_nof_iter               = args->max_nof_iter;
+
+    q->decoder_bg1[ls] = SRSRAN_MEM_ALLOC(srsran_ldpc_decoder_t, 1);
     if (!q->decoder_bg1[ls]) {
       ERROR("Error: calloc");
       return SRSRAN_ERROR;
     }
+    SRSRAN_MEM_ZERO(q->decoder_bg1[ls], srsran_ldpc_decoder_t, 1);
 
-    if (srsran_ldpc_decoder_init(q->decoder_bg1[ls], decoder_type, BG1, ls, scaling_factor) < SRSRAN_SUCCESS) {
+    decoder_args.bg = BG1;
+    if (srsran_ldpc_decoder_init(q->decoder_bg1[ls], &decoder_args) < SRSRAN_SUCCESS) {
       ERROR("Error: initialising BG1 LDPC decoder for ls=%d", ls);
       return SRSRAN_ERROR;
     }
 
-    q->decoder_bg2[ls] = calloc(1, sizeof(srsran_ldpc_decoder_t));
+    q->decoder_bg2[ls] = SRSRAN_MEM_ALLOC(srsran_ldpc_decoder_t, 1);
     if (!q->decoder_bg2[ls]) {
       ERROR("Error: calloc");
       return SRSRAN_ERROR;
     }
+    SRSRAN_MEM_ZERO(q->decoder_bg2[ls], srsran_ldpc_decoder_t, 1);
 
-    if (srsran_ldpc_decoder_init(q->decoder_bg2[ls], decoder_type, BG2, ls, scaling_factor) < SRSRAN_SUCCESS) {
+    decoder_args.bg = BG2;
+    if (srsran_ldpc_decoder_init(q->decoder_bg2[ls], &decoder_args) < SRSRAN_SUCCESS) {
       ERROR("Error: initialising BG2 LDPC decoder for ls=%d", ls);
       return SRSRAN_ERROR;
     }
@@ -507,19 +518,19 @@ static inline int sch_nr_encode(srsran_sch_nr_t*        q,
   return SRSRAN_SUCCESS;
 }
 
-int sch_nr_decode(srsran_sch_nr_t*        q,
-                  const srsran_sch_cfg_t* sch_cfg,
-                  const srsran_sch_tb_t*  tb,
-                  int8_t*                 e_bits,
-                  uint8_t*                data,
-                  bool*                   crc_ok)
+static int sch_nr_decode(srsran_sch_nr_t*        q,
+                         const srsran_sch_cfg_t* sch_cfg,
+                         const srsran_sch_tb_t*  tb,
+                         int8_t*                 e_bits,
+                         srsran_sch_tb_res_nr_t* res)
 {
   // Pointer protection
-  if (!q || !sch_cfg || !tb || !data || !e_bits || !crc_ok) {
+  if (!q || !sch_cfg || !tb || !e_bits || !res) {
     return SRSRAN_ERROR_INVALID_INPUTS;
   }
 
-  int8_t* input_ptr = e_bits;
+  int8_t*  input_ptr    = e_bits;
+  uint32_t nof_iter_sum = 0;
 
   srsran_sch_nr_tb_info_t cfg = {};
   if (srsran_sch_nr_fill_tb_info(&q->carrier, sch_cfg, tb, &cfg) < SRSRAN_SUCCESS) {
@@ -597,27 +608,25 @@ int sch_nr_decode(srsran_sch_nr_t*        q,
       return SRSRAN_ERROR;
     }
 
-    // Decode
-    srsran_ldpc_decoder_decode_c(decoder, rm_buffer, q->temp_cb, n_llr);
-
-    // Compute CB CRC
-    uint32_t cb_len = cfg.Kp - cfg.L_cb;
+    // Select CB or TB early stop CRC
+    srsran_crc_t* crc = (cfg.L_tb == 16) ? &q->crc_tb_16 : &q->crc_tb_24;
     if (cfg.L_cb) {
-      uint8_t* ptr                 = q->temp_cb + cb_len;
-      uint32_t checksum1           = srsran_crc_checksum(&q->crc_cb, q->temp_cb, (int)cb_len);
-      uint32_t checksum2           = srsran_bit_pack(&ptr, cfg.L_cb);
-      tb->softbuffer.rx->cb_crc[r] = (checksum1 == checksum2);
-
-      SCH_INFO_RX("CB %d/%d: CRC={%06x, %06x} ... %s",
-                  r,
-                  cfg.C,
-                  checksum1,
-                  checksum2,
-                  tb->softbuffer.rx->cb_crc[r] ? "OK" : "KO");
-    } else {
-      tb->softbuffer.rx->cb_crc[r] = true;
+      crc = &q->crc_cb;
     }
 
+    // Decode
+    int n_iter = srsran_ldpc_decoder_decode_crc_c(decoder, rm_buffer, q->temp_cb, n_llr, crc);
+    if (n_iter < SRSRAN_SUCCESS) {
+      ERROR("Error decoding CB");
+      return SRSRAN_ERROR;
+    }
+    nof_iter_sum += ((n_iter == 0) ? decoder->max_nof_iter : (uint32_t)n_iter);
+
+    // Compute CB CRC only if LDPC decoder reached the end
+    uint32_t cb_len              = cfg.Kp - cfg.L_cb;
+    tb->softbuffer.rx->cb_crc[r] = (n_iter != 0);
+    SCH_INFO_RX("CB %d/%d CRC=%s", r, cfg.C, tb->softbuffer.rx->cb_crc[r] ? "OK" : "KO");
+
     // Pack and count CRC OK only if CRC is match
     if (tb->softbuffer.rx->cb_crc[r]) {
       srsran_bit_pack_vector(q->temp_cb, tb->softbuffer.rx->data[r], cb_len);
@@ -627,51 +636,64 @@ int sch_nr_decode(srsran_sch_nr_t*        q,
     input_ptr += E;
   }
 
-  // All CB are decoded
-  if (cb_ok == cfg.C) {
-    uint32_t checksum2  = 0;
-    uint8_t* output_ptr = data;
+  // Not all CB are decoded, skip TB union and CRC check
+  if (cb_ok != cfg.C) {
+    return SRSRAN_SUCCESS;
+  }
 
-    for (uint32_t r = 0; r < cfg.C; r++) {
-      uint32_t cb_len = cfg.Kp - cfg.L_cb;
+  uint32_t checksum2  = 0;
+  uint8_t* output_ptr = res->payload;
 
-      // Subtract TB CRC from the last code block
-      if (r == cfg.C - 1) {
-        cb_len -= cfg.L_tb;
-      }
+  for (uint32_t r = 0; r < cfg.C; r++) {
+    uint32_t cb_len = cfg.Kp - cfg.L_cb;
 
-      srsran_vec_u8_copy(output_ptr, tb->softbuffer.rx->data[r], cb_len / 8);
-      output_ptr += cb_len / 8;
+    // Subtract TB CRC from the last code block
+    if (r == cfg.C - 1) {
+      cb_len -= cfg.L_tb;
+    }
 
-      if (SRSRAN_DEBUG_ENABLED && srsran_verbose >= SRSRAN_VERBOSE_DEBUG && !handler_registered) {
-        DEBUG("CB %d:", r);
-        srsran_vec_fprint_byte(stdout, tb->softbuffer.rx->data[r], cb_len / 8);
-      }
-      if (r == cfg.C - 1) {
-        uint8_t  tb_crc_unpacked[24] = {};
-        uint8_t* tb_crc_unpacked_ptr = tb_crc_unpacked;
-        srsran_bit_unpack_vector(&tb->softbuffer.rx->data[r][cb_len / 8], tb_crc_unpacked, cfg.L_tb);
-        checksum2 = srsran_bit_pack(&tb_crc_unpacked_ptr, cfg.L_tb);
-      }
+    // Append CB
+    srsran_vec_u8_copy(output_ptr, tb->softbuffer.rx->data[r], cb_len / 8);
+    output_ptr += cb_len / 8;
+
+    // CB Debug trace
+    if (SRSRAN_DEBUG_ENABLED && srsran_verbose >= SRSRAN_VERBOSE_DEBUG && !handler_registered) {
+      DEBUG("CB %d/%d:", r, cfg.C);
+      srsran_vec_fprint_byte(stdout, tb->softbuffer.rx->data[r], cb_len / 8);
     }
 
-    // Check if TB is all zeros
-    bool all_zeros = true;
-    for (uint32_t i = 0; i < tb->tbs / 8 && all_zeros; i++) {
-      all_zeros = (data[i] == 0);
+    // Compute TB CRC for last block
+    if (cfg.C > 1 && r == cfg.C - 1) {
+      uint8_t  tb_crc_unpacked[24] = {};
+      uint8_t* tb_crc_unpacked_ptr = tb_crc_unpacked;
+      srsran_bit_unpack_vector(&tb->softbuffer.rx->data[r][cb_len / 8], tb_crc_unpacked, cfg.L_tb);
+      checksum2 = srsran_bit_pack(&tb_crc_unpacked_ptr, cfg.L_tb);
     }
+  }
 
-    // Calculate TB CRC from packed data
-    uint32_t checksum1 = srsran_crc_checksum_byte(crc_tb, data, tb->tbs);
-    *crc_ok            = (checksum1 == checksum2 && !all_zeros);
+  // Check if TB is all zeros
+  bool all_zeros = true;
+  for (uint32_t i = 0; i < tb->tbs / 8 && all_zeros; i++) {
+    all_zeros = (res->payload[i] == 0);
+  }
 
-    SCH_INFO_RX("TB: TBS=%d; CRC={%06x, %06x}", tb->tbs, checksum1, checksum2);
-    if (SRSRAN_DEBUG_ENABLED && srsran_verbose >= SRSRAN_VERBOSE_DEBUG && !handler_registered) {
-      DEBUG("Decode: ");
-      srsran_vec_fprint_byte(stdout, data, tb->tbs / 8);
-    }
+  // Calculate TB CRC from packed data
+  if (cfg.C == 1) {
+    res->crc = !all_zeros;
+    SCH_INFO_RX("TB: TBS=%d; CRC=%s", tb->tbs, tb->softbuffer.rx->cb_crc[0] ? "OK" : "KO");
   } else {
-    *crc_ok = false;
+    // More than one
+    uint32_t checksum1 = srsran_crc_checksum_byte(crc_tb, res->payload, tb->tbs);
+    res->crc           = (checksum1 == checksum2 && !all_zeros);
+    SCH_INFO_RX("TB: TBS=%d; CRC={%06x, %06x}", tb->tbs, checksum1, checksum2);
+  }
+
+  // Set average number of iterations
+  res->avg_iter = (float)nof_iter_sum / (float)cfg.C;
+
+  if (SRSRAN_DEBUG_ENABLED && srsran_verbose >= SRSRAN_VERBOSE_DEBUG && !handler_registered) {
+    DEBUG("Decode: ");
+    srsran_vec_fprint_byte(stdout, res->payload, tb->tbs / 8);
   }
 
   return SRSRAN_SUCCESS;
@@ -690,10 +712,9 @@ int srsran_dlsch_nr_decode(srsran_sch_nr_t*        q,
                            const srsran_sch_cfg_t* sch_cfg,
                            const srsran_sch_tb_t*  tb,
                            int8_t*                 e_bits,
-                           uint8_t*                data,
-                           bool*                   crc_ok)
+                           srsran_sch_tb_res_nr_t* res)
 {
-  return sch_nr_decode(q, sch_cfg, tb, e_bits, data, crc_ok);
+  return sch_nr_decode(q, sch_cfg, tb, e_bits, res);
 }
 
 int srsran_ulsch_nr_encode(srsran_sch_nr_t*        q,
@@ -709,30 +730,33 @@ int srsran_ulsch_nr_decode(srsran_sch_nr_t*        q,
                            const srsran_sch_cfg_t* sch_cfg,
                            const srsran_sch_tb_t*  tb,
                            int8_t*                 e_bits,
-                           uint8_t*                data,
-                           bool*                   crc_ok)
+                           srsran_sch_tb_res_nr_t* res)
 {
-  return sch_nr_decode(q, sch_cfg, tb, e_bits, data, crc_ok);
+  return sch_nr_decode(q, sch_cfg, tb, e_bits, res);
 }
 
-int srsran_sch_nr_tb_info(const srsran_sch_tb_t* tb, char* str, uint32_t str_len)
+int srsran_sch_nr_tb_info(const srsran_sch_tb_t* tb, const srsran_sch_tb_res_nr_t* res, char* str, uint32_t str_len)
 {
   int len = 0;
 
   if (tb->enabled) {
-    len += srsran_print_check(str,
-                              str_len,
-                              len,
-                              "tb={mod=%s,Nl=%d,tbs=%d,R=%.3f,rv=%d,Nre=%d,Nbit=%d,cw=%d}",
-                              srsran_mod_string(tb->mod),
-                              tb->N_L,
-                              tb->tbs / 8,
-                              tb->R,
-                              tb->rv,
-                              tb->nof_re,
-                              tb->nof_bits,
-                              tb->cw_idx);
+    len = srsran_print_check(str,
+                             str_len,
+                             len,
+                             "CW%d: mod=%s Nl=%d tbs=%d R=%.3f rv=%d Nre=%d Nbit=%d ",
+                             tb->cw_idx,
+                             srsran_mod_string(tb->mod),
+                             tb->N_L,
+                             tb->tbs / 8,
+                             tb->R,
+                             tb->rv,
+                             tb->nof_re,
+                             tb->nof_bits);
+
+    if (res != NULL) {
+      len = srsran_print_check(str, str_len, len, "CRC=%s iter=%.1f ", res->crc ? "OK" : "KO", res->avg_iter);
+    }
   }
 
   return len;
-}
+}
\ No newline at end of file
diff --git a/lib/src/phy/phch/test/pdsch_nr_test.c b/lib/src/phy/phch/test/pdsch_nr_test.c
index 30ced3a9f..8c45d1a96 100644
--- a/lib/src/phy/phch/test/pdsch_nr_test.c
+++ b/lib/src/phy/phch/test/pdsch_nr_test.c
@@ -84,12 +84,12 @@ int parse_args(int argc, char** argv)
 
 int main(int argc, char** argv)
 {
-  int                   ret                      = SRSRAN_ERROR;
-  srsran_pdsch_nr_t     pdsch_tx                 = {};
-  srsran_pdsch_nr_t     pdsch_rx                 = {};
-  srsran_chest_dl_res_t chest                    = {};
-  srsran_pdsch_res_nr_t pdsch_res[SRSRAN_MAX_TB] = {};
-  srsran_random_t       rand_gen                 = srsran_random_init(1234);
+  int                   ret       = SRSRAN_ERROR;
+  srsran_pdsch_nr_t     pdsch_tx  = {};
+  srsran_pdsch_nr_t     pdsch_rx  = {};
+  srsran_chest_dl_res_t chest     = {};
+  srsran_pdsch_res_nr_t pdsch_res = {};
+  srsran_random_t       rand_gen  = srsran_random_init(1234);
 
   uint8_t* data_tx[SRSRAN_MAX_TB]           = {};
   uint8_t* data_rx[SRSRAN_MAX_CODEWORDS]    = {};
@@ -142,7 +142,7 @@ int main(int argc, char** argv)
       goto clean_exit;
     }
 
-    pdsch_res[i].payload = data_rx[i];
+    pdsch_res.tb[i].payload = data_rx[i];
   }
 
   srsran_softbuffer_tx_t softbuffer_tx = {};
@@ -233,14 +233,14 @@ int main(int argc, char** argv)
       }
       chest.nof_re = pdsch_cfg.grant.tb->nof_re;
 
-      if (srsran_pdsch_nr_decode(&pdsch_rx, &pdsch_cfg, &pdsch_cfg.grant, &chest, sf_symbols, pdsch_res) <
+      if (srsran_pdsch_nr_decode(&pdsch_rx, &pdsch_cfg, &pdsch_cfg.grant, &chest, sf_symbols, &pdsch_res) <
           SRSRAN_SUCCESS) {
         ERROR("Error encoding");
         goto clean_exit;
       }
 
-      if (pdsch_res->evm > 0.001f) {
-        ERROR("Error PDSCH EVM is too high %f", pdsch_res->evm);
+      if (pdsch_res.evm[0] > 0.001f) {
+        ERROR("Error PDSCH EVM is too high %f", pdsch_res.evm[0]);
         goto clean_exit;
       }
 
@@ -265,7 +265,7 @@ int main(int argc, char** argv)
         goto clean_exit;
       }
 
-      if (!pdsch_res[0].crc) {
+      if (!pdsch_res.tb[0].crc) {
         ERROR("Failed to match CRC; n_prb=%d; mcs=%d; TBS=%d;", n_prb, mcs, pdsch_cfg.grant.tb[0].tbs);
         goto clean_exit;
       }
@@ -279,7 +279,7 @@ int main(int argc, char** argv)
         goto clean_exit;
       }
 
-      INFO("n_prb=%d; mcs=%d; TBS=%d; EVM=%f; PASSED!\n", n_prb, mcs, pdsch_cfg.grant.tb[0].tbs, pdsch_res[0].evm);
+      INFO("n_prb=%d; mcs=%d; TBS=%d; EVM=%f; PASSED!\n", n_prb, mcs, pdsch_cfg.grant.tb[0].tbs, pdsch_res.evm[0]);
     }
   }
 
diff --git a/lib/src/phy/phch/test/pusch_nr_test.c b/lib/src/phy/phch/test/pusch_nr_test.c
index 3f0fe6042..0d950e3b9 100644
--- a/lib/src/phy/phch/test/pusch_nr_test.c
+++ b/lib/src/phy/phch/test/pusch_nr_test.c
@@ -99,8 +99,8 @@ int main(int argc, char** argv)
   srsran_chest_dl_res_t chest    = {};
   srsran_random_t       rand_gen = srsran_random_init(1234);
 
-  srsran_pusch_data_nr_t data_tx[SRSRAN_MAX_TB]           = {};
-  srsran_pusch_res_nr_t  data_rx[SRSRAN_MAX_CODEWORDS]    = {};
+  srsran_pusch_data_nr_t data_tx                          = {};
+  srsran_pusch_res_nr_t  data_rx                          = {};
   cf_t*                  sf_symbols[SRSRAN_MAX_LAYERS_NR] = {};
 
   // Set default PUSCH configuration
@@ -143,9 +143,9 @@ int main(int argc, char** argv)
   }
 
   for (uint32_t i = 0; i < pusch_tx.max_cw; i++) {
-    data_tx[i].payload = srsran_vec_u8_malloc(SRSRAN_SLOT_MAX_NOF_BITS_NR);
-    data_rx[i].payload = srsran_vec_u8_malloc(SRSRAN_SLOT_MAX_NOF_BITS_NR);
-    if (data_tx[i].payload == NULL || data_rx[i].payload == NULL) {
+    data_tx.payload[i]    = srsran_vec_u8_malloc(SRSRAN_SLOT_MAX_NOF_BITS_NR);
+    data_rx.tb[i].payload = srsran_vec_u8_malloc(SRSRAN_SLOT_MAX_NOF_BITS_NR);
+    if (data_tx.payload[i] == NULL || data_rx.tb[i].payload == NULL) {
       ERROR("Error malloc");
       goto clean_exit;
     }
@@ -221,12 +221,12 @@ int main(int argc, char** argv)
       // Generate SCH payload
       for (uint32_t tb = 0; tb < SRSRAN_MAX_TB; tb++) {
         // Skip TB if no allocated
-        if (data_tx[tb].payload == NULL) {
+        if (data_tx.payload[tb] == NULL) {
           continue;
         }
 
         for (uint32_t i = 0; i < pusch_cfg.grant.tb[tb].tbs; i++) {
-          data_tx[tb].payload[i] = (uint8_t)srsran_random_uniform_int_dist(rand_gen, 0, UINT8_MAX);
+          data_tx.payload[tb][i] = (uint8_t)srsran_random_uniform_int_dist(rand_gen, 0, UINT8_MAX);
         }
         pusch_cfg.grant.tb[tb].softbuffer.tx = &softbuffer_tx;
       }
@@ -235,7 +235,7 @@ int main(int argc, char** argv)
       if (nof_ack_bits > 0) {
         pusch_cfg.uci.o_ack = nof_ack_bits;
         for (uint32_t i = 0; i < nof_ack_bits; i++) {
-          data_tx->uci.ack[i] = (uint8_t)srsran_random_uniform_int_dist(rand_gen, 0, 1);
+          data_tx.uci.ack[i] = (uint8_t)srsran_random_uniform_int_dist(rand_gen, 0, 1);
         }
       }
 
@@ -246,15 +246,15 @@ int main(int argc, char** argv)
         pusch_cfg.uci.csi[0].quantity = SRSRAN_CSI_REPORT_QUANTITY_NONE;
         pusch_cfg.uci.csi[0].K_csi_rs = nof_csi_bits;
         pusch_cfg.uci.nof_csi         = 1;
-        data_tx->uci.csi[0].none      = csi_report_tx;
+        data_tx.uci.csi[0].none       = csi_report_tx;
         for (uint32_t i = 0; i < nof_csi_bits; i++) {
           csi_report_tx[i] = (uint8_t)srsran_random_uniform_int_dist(rand_gen, 0, 1);
         }
 
-        data_rx->uci.csi[0].none = csi_report_rx;
+        data_rx.uci.csi[0].none = csi_report_rx;
       }
 
-      if (srsran_pusch_nr_encode(&pusch_tx, &pusch_cfg, &pusch_cfg.grant, data_tx, sf_symbols) < SRSRAN_SUCCESS) {
+      if (srsran_pusch_nr_encode(&pusch_tx, &pusch_cfg, &pusch_cfg.grant, &data_tx, sf_symbols) < SRSRAN_SUCCESS) {
         ERROR("Error encoding");
         goto clean_exit;
       }
@@ -269,14 +269,14 @@ int main(int argc, char** argv)
       }
       chest.nof_re = pusch_cfg.grant.tb->nof_re;
 
-      if (srsran_pusch_nr_decode(&pusch_rx, &pusch_cfg, &pusch_cfg.grant, &chest, sf_symbols, data_rx) <
+      if (srsran_pusch_nr_decode(&pusch_rx, &pusch_cfg, &pusch_cfg.grant, &chest, sf_symbols, &data_rx) <
           SRSRAN_SUCCESS) {
         ERROR("Error encoding");
         goto clean_exit;
       }
 
-      if (data_rx[0].evm > 0.001f) {
-        ERROR("Error PUSCH EVM is too high %f", data_rx[0].evm);
+      if (data_rx.evm[0] > 0.001f) {
+        ERROR("Error PUSCH EVM is too high %f", data_rx.evm[0]);
         goto clean_exit;
       }
 
@@ -302,24 +302,24 @@ int main(int argc, char** argv)
       }
 
       // Validate UL-SCH CRC check
-      if (!data_rx[0].crc) {
+      if (!data_rx.tb[0].crc) {
         ERROR("Failed to match CRC; n_prb=%d; mcs=%d; TBS=%d;", n_prb, mcs, pusch_cfg.grant.tb[0].tbs);
         goto clean_exit;
       }
 
       // Validate UL-SCH payload
-      if (memcmp(data_tx[0].payload, data_rx[0].payload, pusch_cfg.grant.tb[0].tbs / 8) != 0) {
+      if (memcmp(data_tx.payload[0], data_rx.tb[0].payload, pusch_cfg.grant.tb[0].tbs / 8) != 0) {
         ERROR("Failed to match Tx/Rx data; n_prb=%d; mcs=%d; TBS=%d;", n_prb, mcs, pusch_cfg.grant.tb[0].tbs);
         printf("Tx data: ");
-        srsran_vec_fprint_byte(stdout, data_tx[0].payload, pusch_cfg.grant.tb[0].tbs / 8);
+        srsran_vec_fprint_byte(stdout, data_tx.payload[0], pusch_cfg.grant.tb[0].tbs / 8);
         printf("Rx data: ");
-        srsran_vec_fprint_byte(stdout, data_tx[0].payload, pusch_cfg.grant.tb[0].tbs / 8);
+        srsran_vec_fprint_byte(stdout, data_tx.payload[0], pusch_cfg.grant.tb[0].tbs / 8);
         goto clean_exit;
       }
 
       // Validate UCI is decoded successfully
       if (nof_ack_bits > 0 || nof_csi_bits > 0) {
-        if (!data_rx[0].uci.valid) {
+        if (!data_rx.uci.valid) {
           ERROR("UCI data was not decoded ok");
           goto clean_exit;
         }
@@ -327,29 +327,29 @@ int main(int argc, char** argv)
 
       // Validate HARQ-ACK is decoded successfully
       if (nof_ack_bits > 0) {
-        if (memcmp(data_tx[0].uci.ack, data_rx[0].uci.ack, nof_ack_bits) != 0) {
+        if (memcmp(data_tx.uci.ack, data_rx.uci.ack, nof_ack_bits) != 0) {
           ERROR("UCI HARQ-ACK bits are unmatched");
           printf("Tx data: ");
-          srsran_vec_fprint_byte(stdout, data_tx[0].uci.ack, nof_ack_bits);
+          srsran_vec_fprint_byte(stdout, data_tx.uci.ack, nof_ack_bits);
           printf("Rx data: ");
-          srsran_vec_fprint_byte(stdout, data_rx[0].uci.ack, nof_ack_bits);
+          srsran_vec_fprint_byte(stdout, data_rx.uci.ack, nof_ack_bits);
           goto clean_exit;
         }
       }
 
       // Validate CSI is decoded successfully
       if (nof_csi_bits > 0) {
-        if (memcmp(data_tx[0].uci.csi[0].none, data_rx[0].uci.csi[0].none, nof_csi_bits) != 0) {
+        if (memcmp(data_tx.uci.csi[0].none, data_rx.uci.csi[0].none, nof_csi_bits) != 0) {
           ERROR("UCI CSI bits are unmatched");
           printf("Tx data: ");
-          srsran_vec_fprint_byte(stdout, data_tx[0].uci.csi[0].none, nof_csi_bits);
+          srsran_vec_fprint_byte(stdout, data_tx.uci.csi[0].none, nof_csi_bits);
           printf("Rx data: ");
-          srsran_vec_fprint_byte(stdout, data_rx[0].uci.csi[0].none, nof_csi_bits);
+          srsran_vec_fprint_byte(stdout, data_rx.uci.csi[0].none, nof_csi_bits);
           goto clean_exit;
         }
       }
 
-      printf("n_prb=%d; mcs=%d; TBS=%d; EVM=%f; PASSED!\n", n_prb, mcs, pusch_cfg.grant.tb[0].tbs, data_rx[0].evm);
+      printf("n_prb=%d; mcs=%d; TBS=%d; EVM=%f; PASSED!\n", n_prb, mcs, pusch_cfg.grant.tb[0].tbs, data_rx.evm[0]);
     }
   }
 
@@ -361,11 +361,11 @@ clean_exit:
   srsran_pusch_nr_free(&pusch_tx);
   srsran_pusch_nr_free(&pusch_rx);
   for (uint32_t i = 0; i < SRSRAN_MAX_CODEWORDS; i++) {
-    if (data_tx[i].payload) {
-      free(data_tx[i].payload);
+    if (data_tx.payload[i]) {
+      free(data_tx.payload[i]);
     }
-    if (data_rx[i].payload) {
-      free(data_rx[i].payload);
+    if (data_rx.tb[i].payload) {
+      free(data_rx.tb[i].payload);
     }
   }
   for (uint32_t i = 0; i < SRSRAN_MAX_LAYERS_NR; i++) {
diff --git a/lib/src/phy/phch/test/sch_nr_test.c b/lib/src/phy/phch/test/sch_nr_test.c
index f76f91b3b..4ac4ed2c4 100644
--- a/lib/src/phy/phch/test/sch_nr_test.c
+++ b/lib/src/phy/phch/test/sch_nr_test.c
@@ -215,14 +215,15 @@ int main(int argc, char** argv)
         tb.softbuffer.rx = &softbuffer_rx;
         srsran_softbuffer_rx_reset(tb.softbuffer.rx);
 
-        bool crc = false;
-        if (srsran_dlsch_nr_decode(&sch_nr_rx, &pdsch_cfg.sch_cfg, &tb, llr, data_rx, &crc) < SRSRAN_SUCCESS) {
+        srsran_sch_tb_res_nr_t res = {};
+        res.payload                = data_rx;
+        if (srsran_dlsch_nr_decode(&sch_nr_rx, &pdsch_cfg.sch_cfg, &tb, llr, &res) < SRSRAN_SUCCESS) {
           ERROR("Error encoding");
           goto clean_exit;
         }
 
         if (rv == 0) {
-          if (!crc) {
+          if (!res.crc) {
             ERROR("Failed to match CRC; n_prb=%d; mcs=%d; TBS=%d;", n_prb, mcs, tb.tbs);
             goto clean_exit;
           }
diff --git a/lib/src/phy/rf/rf_uhd_generic.h b/lib/src/phy/rf/rf_uhd_generic.h
index 8fee51f32..57488918c 100644
--- a/lib/src/phy/rf/rf_uhd_generic.h
+++ b/lib/src/phy/rf/rf_uhd_generic.h
@@ -57,7 +57,6 @@ private:
 
   uhd_error test_ad936x_device(uint32_t nof_channels)
   {
-
     uhd_error err = set_rx_rate(1.92e6);
     if (err != UHD_ERROR_NONE) {
       return err;
@@ -218,7 +217,6 @@ public:
       Info("The device is based on AD9361, get RX stream for checking LIBUSB_TRANSFER_ERROR");
       uint32_t ntrials = 10;
       do {
-
         // If no error getting RX stream, return
         err = test_ad936x_device(nof_channels);
         if (err == UHD_ERROR_NONE) {
@@ -277,13 +275,13 @@ public:
   {
     UHD_SAFE_C_SAVE_ERROR(this, timespec = usrp->get_time_now();)
   }
-  uhd_error set_sync_source(const std::string& source) override
+  uhd_error set_sync_source(const std::string& sync_source, const std::string& clock_source) override
   {
-    Debug("Setting PPS source to '" << source << "'");
+    Debug("Setting PPS source to '" << sync_source << "' and clock source to '" << clock_source << "'");
 #if UHD_VERSION < 3140099
-        UHD_SAFE_C_SAVE_ERROR(this, usrp->set_clock_source(source); usrp->set_time_source(source);)
+    UHD_SAFE_C_SAVE_ERROR(this, usrp->set_clock_source(clock_source); usrp->set_time_source(sync_source);)
 #else
-        UHD_SAFE_C_SAVE_ERROR(this, usrp->set_sync_source(source, source);)
+    UHD_SAFE_C_SAVE_ERROR(this, usrp->set_sync_source(clock_source, sync_source);)
 #endif
   }
   uhd_error get_gain_range(uhd::gain_range_t& tx_gain_range, uhd::gain_range_t& rx_gain_range) override
diff --git a/lib/src/phy/rf/rf_uhd_imp.cc b/lib/src/phy/rf/rf_uhd_imp.cc
index e10af9526..5363cbf3a 100644
--- a/lib/src/phy/rf/rf_uhd_imp.cc
+++ b/lib/src/phy/rf/rf_uhd_imp.cc
@@ -187,7 +187,7 @@ static cf_t zero_mem[64 * 1024] = {};
 
 #define print_usrp_error(h)                                                                                            \
   do {                                                                                                                 \
-    ERROR("USRP reported the following error: %s", h->uhd->last_error.c_str());                                      \
+    ERROR("USRP reported the following error: %s", h->uhd->last_error.c_str());                                        \
   } while (false)
 
 static void log_overflow(rf_uhd_handler_t* h)
@@ -612,6 +612,12 @@ static int uhd_init(rf_uhd_handler_t* handler, char* args, uint32_t nof_channels
     clock_src = device_addr.pop("clock");
   }
 
+  // Select same synchronization source only if more than one channel is opened
+  std::string sync_src = "internal";
+  if (nof_channels > 1) {
+    sync_src = clock_src;
+  }
+
   // Logging level
 #ifdef UHD_LOG_INFO
   uhd::log::severity_level severity_level = uhd::log::severity_level::info;
@@ -788,7 +794,7 @@ static int uhd_init(rf_uhd_handler_t* handler, char* args, uint32_t nof_channels
   std::string sensor_name;
 
   // Set sync source
-  if (handler->uhd->set_sync_source(clock_src) != UHD_ERROR_NONE) {
+  if (handler->uhd->set_sync_source(sync_src, clock_src) != UHD_ERROR_NONE) {
     print_usrp_error(handler);
     return SRSRAN_ERROR;
   }
diff --git a/lib/src/phy/rf/rf_uhd_rfnoc.h b/lib/src/phy/rf/rf_uhd_rfnoc.h
index e8ce61e13..fb638c50b 100644
--- a/lib/src/phy/rf/rf_uhd_rfnoc.h
+++ b/lib/src/phy/rf/rf_uhd_rfnoc.h
@@ -114,18 +114,19 @@ private:
   template <class T>
   uhd_error parse_param(uhd::device_addr_t& args, const std::string& param, T& value, bool pop = true)
   {
-    UHD_SAFE_C_SAVE_ERROR(this,
-                          // Check if parameter exists
-                          if (not args.has_key(param)) {
-                            last_error = "RF-NOC requires " + param + " parameter";
-                            return UHD_ERROR_KEY;
-                          }
-
-                          // Parse parameter
-                          value = args.cast(param, value);
-
-                          // Remove parameter from list
-                          if (pop) args.pop(param);)
+    UHD_SAFE_C_SAVE_ERROR(
+        this,
+        // Check if parameter exists
+        if (not args.has_key(param)) {
+          last_error = "RF-NOC requires " + param + " parameter";
+          return UHD_ERROR_KEY;
+        }
+
+        // Parse parameter
+        value = args.cast(param, value);
+
+        // Remove parameter from list
+        if (pop) args.pop(param);)
   }
 
   uhd_error parse_args(uhd::device_addr_t& args)
@@ -236,7 +237,6 @@ private:
           }
 
           for (size_t j = 0; j < nof_channels; j++) {
-
             uhd::device_addr_t args;
             args.set("input_rate", std::to_string(master_clock_rate));
             args.set("fullscale", "1.0");
@@ -271,7 +271,6 @@ private:
           }
 
           for (size_t j = 0; j < nof_channels; j++) {
-
             uhd::device_addr_t args;
             args.set("output_rate", std::to_string(master_clock_rate));
             args.set("fullscale", "1.0");
@@ -471,15 +470,16 @@ public:
   };
   uhd_error get_mboard_sensor_names(std::vector<std::string>& sensors) override
   {
-    UHD_SAFE_C_SAVE_ERROR(this, if (device3->get_tree()->exists(TREE_MBOARD_SENSORS)) {
-      sensors = device3->get_tree()->list(TREE_MBOARD_SENSORS);
-    })
+    UHD_SAFE_C_SAVE_ERROR(
+        this, if (device3->get_tree()->exists(TREE_MBOARD_SENSORS)) {
+          sensors = device3->get_tree()->list(TREE_MBOARD_SENSORS);
+        })
   }
   uhd_error get_rx_sensor_names(std::vector<std::string>& sensors) override
   {
-    UHD_SAFE_C_SAVE_ERROR(this, if (device3->get_tree()->exists(TREE_RX_SENSORS)) {
-      sensors = device3->get_tree()->list(TREE_RX_SENSORS);
-    })
+    UHD_SAFE_C_SAVE_ERROR(
+        this,
+        if (device3->get_tree()->exists(TREE_RX_SENSORS)) { sensors = device3->get_tree()->list(TREE_RX_SENSORS); })
   }
   uhd_error get_sensor(const std::string& sensor_name, double& sensor_value) override
   {
@@ -504,24 +504,29 @@ public:
   uhd_error set_time_unknown_pps(const uhd::time_spec_t& timespec) override
   {
     Info("Setting time " << timespec.get_real_secs() << " at next PPS...");
-    UHD_SAFE_C_SAVE_ERROR(this, for (auto& r : radio_ctrl) { r->set_time_next_pps(timespec); });
+    UHD_SAFE_C_SAVE_ERROR(
+        this,
+        for (auto& r
+             : radio_ctrl) { r->set_time_next_pps(timespec); });
   }
   uhd_error get_time_now(uhd::time_spec_t& timespec) override
   {
     UHD_SAFE_C_SAVE_ERROR(this, timespec = device3->get_tree()->access<uhd::time_spec_t>(TREE_TIME_NOW).get();
                           Info("-- " << timespec.get_real_secs());)
   }
-  uhd_error set_sync_source(const std::string& source) override
+  uhd_error set_sync_source(const std::string& sync_source, const std::string& clock_source) override
   {
     if (loopback) {
       return UHD_ERROR_NONE;
     }
 
-    UHD_SAFE_C_SAVE_ERROR(this, for (size_t radio_idx = 0; radio_idx < nof_radios; radio_idx++) {
-      UHD_LOG_DEBUG(radio_id[radio_idx], "Setting sync source to " << source);
-      radio_ctrl[radio_idx]->set_clock_source(source);
-      radio_ctrl[radio_idx]->set_time_source(source);
-    })
+    UHD_SAFE_C_SAVE_ERROR(
+        this, for (size_t radio_idx = 0; radio_idx < nof_radios; radio_idx++) {
+          UHD_LOG_DEBUG(radio_id[radio_idx],
+                        "Setting PPS source to '" << sync_source << "' and clock source to '" << clock_source << "'");
+          radio_ctrl[radio_idx]->set_clock_source(clock_source);
+          radio_ctrl[radio_idx]->set_time_source(sync_source);
+        })
   }
   uhd_error get_gain_range(uhd::gain_range_t& tx_gain_range, uhd::gain_range_t& rx_gain_range) override
   {
@@ -533,21 +538,23 @@ public:
   uhd_error set_master_clock_rate(double rate) override { return UHD_ERROR_NONE; }
   uhd_error set_rx_rate(double rate) override
   {
-    UHD_SAFE_C_SAVE_ERROR(this, for (size_t i = 0; i < nof_radios; i++) {
-      for (size_t j = 0; j < nof_channels; j++) {
-        UHD_LOG_DEBUG(ddc_id[i], "Setting channel " << j << " output rate to " << rate / 1e6 << " MHz");
-        ddc_ctrl[i]->set_arg("output_rate", std::to_string(rate), j);
-      }
-    })
+    UHD_SAFE_C_SAVE_ERROR(
+        this, for (size_t i = 0; i < nof_radios; i++) {
+          for (size_t j = 0; j < nof_channels; j++) {
+            UHD_LOG_DEBUG(ddc_id[i], "Setting channel " << j << " output rate to " << rate / 1e6 << " MHz");
+            ddc_ctrl[i]->set_arg("output_rate", std::to_string(rate), j);
+          }
+        })
   }
   uhd_error set_tx_rate(double rate) override
   {
-    UHD_SAFE_C_SAVE_ERROR(this, for (size_t i = 0; i < nof_radios; i++) {
-      for (size_t j = 0; j < nof_channels; j++) {
-        UHD_LOG_DEBUG(duc_id[i], "Setting channel " << j << " input rate to " << rate / 1e6 << " MHz");
-        duc_ctrl[i]->set_arg("input_rate", std::to_string(rate), j);
-      }
-    })
+    UHD_SAFE_C_SAVE_ERROR(
+        this, for (size_t i = 0; i < nof_radios; i++) {
+          for (size_t j = 0; j < nof_channels; j++) {
+            UHD_LOG_DEBUG(duc_id[i], "Setting channel " << j << " input rate to " << rate / 1e6 << " MHz");
+            duc_ctrl[i]->set_arg("input_rate", std::to_string(rate), j);
+          }
+        })
   }
   uhd_error set_command_time(const uhd::time_spec_t& timespec) override { return UHD_ERROR_NONE; }
   uhd_error get_rx_stream(size_t& max_num_samps) override
@@ -665,7 +672,6 @@ public:
   }
   uhd_error get_rx_gain(double& gain) override
   {
-
     if (radio_ctrl.size() == 0) {
       return UHD_ERROR_NONE;
     }
diff --git a/lib/src/phy/rf/rf_uhd_safe.h b/lib/src/phy/rf/rf_uhd_safe.h
index bdfdbaf0b..6c6203961 100644
--- a/lib/src/phy/rf/rf_uhd_safe.h
+++ b/lib/src/phy/rf/rf_uhd_safe.h
@@ -152,7 +152,7 @@ public:
                           stream_cmd.stream_now = true;
                           rx_stream->issue_stream_cmd(stream_cmd);)
   }
-  virtual uhd_error set_sync_source(const std::string& source)                                         = 0;
+  virtual uhd_error set_sync_source(const std::string& sync_source, const std::string& clock_source)   = 0;
   virtual uhd_error get_gain_range(uhd::gain_range_t& tx_gain_range, uhd::gain_range_t& rx_gain_range) = 0;
   virtual uhd_error set_master_clock_rate(double rate)                                                 = 0;
   virtual uhd_error set_rx_rate(double rate)                                                           = 0;
diff --git a/lib/src/phy/ue/ue_dl_nr.c b/lib/src/phy/ue/ue_dl_nr.c
index 61c7ac27d..2e0363f94 100644
--- a/lib/src/phy/ue/ue_dl_nr.c
+++ b/lib/src/phy/ue/ue_dl_nr.c
@@ -25,6 +25,13 @@
 #define UE_DL_NR_PDCCH_CORR_DEFAULT_THR 0.5f
 #define UE_DL_NR_PDCCH_EPRE_DEFAULT_THR -80.0f
 
+/**
+ * @brief Shifts FFT window a fraction of the cyclic prefix. Set to 0.0f for disabling.
+ * @note Increases protection against inter-symbol interference in case of synchronization error in expense of computing
+ * performance
+ */
+#define UE_DL_NR_FFT_WINDOW_OFFSET 0.5f
+
 static int ue_dl_nr_alloc_prb(srsran_ue_dl_nr_t* q, uint32_t new_nof_prb)
 {
   if (q->max_prb < new_nof_prb) {
@@ -91,6 +98,7 @@ int srsran_ue_dl_nr_init(srsran_ue_dl_nr_t* q, cf_t* input[SRSRAN_MAX_PORTS], co
   fft_cfg.nof_prb           = args->nof_max_prb;
   fft_cfg.symbol_sz         = srsran_symbol_sz(args->nof_max_prb);
   fft_cfg.keep_dc           = true;
+  fft_cfg.rx_window_offset  = UE_DL_NR_FFT_WINDOW_OFFSET;
 
   for (uint32_t i = 0; i < q->nof_rx_antennas; i++) {
     fft_cfg.in_buffer  = input[i];
@@ -165,6 +173,7 @@ int srsran_ue_dl_nr_set_carrier(srsran_ue_dl_nr_t* q, const srsran_carrier_nr_t*
       cfg.symbol_sz         = srsran_min_symbol_sz_rb(carrier->nof_prb);
       cfg.cp                = SRSRAN_CP_NORM;
       cfg.keep_dc           = true;
+      cfg.rx_window_offset  = UE_DL_NR_FFT_WINDOW_OFFSET;
       srsran_ofdm_rx_init_cfg(&q->fft[i], &cfg);
     }
   }
@@ -585,7 +594,7 @@ int srsran_ue_dl_nr_pdsch_info(const srsran_ue_dl_nr_t*     q,
   len += srsran_pdsch_nr_rx_info(&q->pdsch, cfg, &cfg->grant, res, &str[len], str_len - len);
 
   // Append channel estimator info
-  len = srsran_print_check(str, str_len, len, ",SNR=%+.1f", q->chest.snr_db);
+  len = srsran_print_check(str, str_len, len, "SNR=%+.1f", q->chest.snr_db);
 
   return len;
 }
diff --git a/lib/src/phy/utils/vector_simd.c b/lib/src/phy/utils/vector_simd.c
index eb263f1fc..a853641af 100644
--- a/lib/src/phy/utils/vector_simd.c
+++ b/lib/src/phy/utils/vector_simd.c
@@ -1719,14 +1719,13 @@ void srsran_vec_apply_cfo_simd(const cf_t* x, float cfo, cf_t* z, int len)
     }
   } else {
     for (; i < len - SRSRAN_SIMD_F_SIZE + 1; i += SRSRAN_SIMD_F_SIZE) {
-      for (; i < len - SRSRAN_SIMD_CF_SIZE + 1; i += SRSRAN_SIMD_CF_SIZE) {
-        simd_cf_t a = srsran_simd_cfi_loadu(&x[i]);
+      simd_cf_t a = srsran_simd_cfi_loadu(&x[i]);
 
-        simd_cf_t r = srsran_simd_cf_prod(a, _simd_phase);
-        _simd_phase = srsran_simd_cf_prod(_simd_phase, _simd_osc);
+      simd_cf_t r = srsran_simd_cf_prod(a, _simd_phase);
 
-        srsran_simd_cfi_storeu(&z[i], r);
-      }
+      srsran_simd_cfi_storeu(&z[i], r);
+
+      _simd_phase = srsran_simd_cf_prod(_simd_phase, _simd_osc);
     }
   }
 #endif
diff --git a/lib/src/srslog/bundled/fmt/format.cc b/lib/src/srslog/bundled/fmt/format.cc
index a64a1f389..4864ac26e 100644
--- a/lib/src/srslog/bundled/fmt/format.cc
+++ b/lib/src/srslog/bundled/fmt/format.cc
@@ -6,6 +6,7 @@
 // For the license information refer to format.h.
 
 #include "fmt/format-inl.h"
+#include <mutex>
 
 FMT_BEGIN_NAMESPACE
 namespace detail {
@@ -23,6 +24,79 @@ int format_float(char* buf, std::size_t size, const char* format, int precision,
   return precision < 0 ? snprintf_ptr(buf, size, format, value)
                        : snprintf_ptr(buf, size, format, precision, value);
 }
+
+#define NODE_POOL_SIZE (10000u)
+static constexpr uint8_t memory_heap_tag = 0xAA;
+class dyn_node_pool
+{
+  /// The extra byte is used to store the memory tag at position 0 in the array.
+  using type = std::array<uint8_t, dynamic_arg_list::max_pool_node_size + 1>;
+
+public:
+  dyn_node_pool() {
+    pool.resize(NODE_POOL_SIZE);
+    free_list.reserve(NODE_POOL_SIZE);
+    for (auto& elem : pool) {
+      free_list.push_back(elem.data());
+    }
+  }
+
+  dyn_node_pool(const dyn_node_pool&) = delete;
+  dyn_node_pool(dyn_node_pool&&) = delete;
+  dyn_node_pool& operator=(dyn_node_pool&&) = delete;
+  dyn_node_pool& operator=(const dyn_node_pool&) = delete;
+
+  void* alloc(std::size_t sz) {
+    assert(sz <= dynamic_arg_list::max_pool_node_size && "Object is too large to fit in the pool");
+
+    std::lock_guard<std::mutex> lock(m);
+    if (free_list.empty()) {
+      // Tag that this allocation was performed by the heap.
+      auto *p = new type;
+      (*p)[0] = memory_heap_tag;
+      return p->data() + 1;
+    }
+
+    auto* p = free_list.back();
+    free_list.pop_back();
+
+    // Tag that this allocation was performed by the pool.
+    p[0] = 0;
+    return p + 1;
+  }
+
+  void dealloc(void* p) {
+    if (!p) {
+      return;
+    }
+
+    std::lock_guard<std::mutex> lock(m);
+    uint8_t* base_ptr = reinterpret_cast<uint8_t *>(p) - 1;
+    if (*base_ptr == memory_heap_tag) {
+      // This pointer was allocated using the heap.
+      delete reinterpret_cast<type *>(base_ptr);
+      return;
+    }
+
+    free_list.push_back(base_ptr);
+  }
+
+private:
+  std::vector<type> pool;
+  std::vector<uint8_t *> free_list;
+  mutable std::mutex m;
+};
+
+static dyn_node_pool node_pool;
+
+void *dynamic_arg_list::allocate_from_pool(std::size_t sz) {
+  return node_pool.alloc(sz);
+}
+
+void dynamic_arg_list::free_from_pool(void *ptr) {
+  return node_pool.dealloc(ptr);
+}
+
 }  // namespace detail
 
 template struct FMT_INSTANTIATION_DEF_API detail::basic_data<void>;
diff --git a/lib/src/srslog/formatters/text_formatter.cpp b/lib/src/srslog/formatters/text_formatter.cpp
index 3c9286a5f..ce62af7a2 100644
--- a/lib/src/srslog/formatters/text_formatter.cpp
+++ b/lib/src/srslog/formatters/text_formatter.cpp
@@ -56,7 +56,7 @@ static void format_metadata(const detail::log_entry_metadata& metadata, fmt::mem
 
   // Format optional fields if present.
   if (!metadata.log_name.empty()) {
-    fmt::format_to(buffer, "[{: <4}] ", metadata.log_name);
+    fmt::format_to(buffer, "[{: <7}] ", metadata.log_name);
   }
   if (metadata.log_tag != '\0') {
     fmt::format_to(buffer, "[{}] ", metadata.log_tag);
diff --git a/lib/src/srslog/srslog.cpp b/lib/src/srslog/srslog.cpp
index 6ffa888cf..369bc989e 100644
--- a/lib/src/srslog/srslog.cpp
+++ b/lib/src/srslog/srslog.cpp
@@ -37,13 +37,10 @@ static std::string remove_sharp_chars(const std::string& s)
 
 /// Generic argument function that fetches a log channel from the repository.
 template <typename... Args>
-static log_channel& fetch_log_channel_helper(const std::string& id,
-                                             Args&&... args)
+static log_channel& fetch_log_channel_helper(const std::string& id, Args&&... args)
 {
   return srslog_instance::get().get_channel_repo().emplace(
-      std::piecewise_construct,
-      std::forward_as_tuple(id),
-      std::forward_as_tuple(id, std::forward<Args>(args)...));
+      std::piecewise_construct, std::forward_as_tuple(id), std::forward_as_tuple(id, std::forward<Args>(args)...));
 }
 
 ///
@@ -60,21 +57,27 @@ log_channel& srslog::fetch_log_channel(const std::string& id)
   assert(!id.empty() && "Empty id string");
 
   std::string clean_id = remove_sharp_chars(id);
+
+  if (auto* c = find_log_channel(clean_id)) {
+    return *c;
+  }
+
   srslog_instance& instance = srslog_instance::get();
-  return fetch_log_channel_helper(
-      clean_id, instance.get_default_sink(), instance.get_backend());
+  return fetch_log_channel_helper(clean_id, instance.get_default_sink(), instance.get_backend());
 }
 
-log_channel& srslog::fetch_log_channel(const std::string& id,
-                                       sink& s,
-                                       log_channel_config config)
+log_channel& srslog::fetch_log_channel(const std::string& id, sink& s, log_channel_config config)
 {
   assert(!id.empty() && "Empty id string");
 
   std::string clean_id = remove_sharp_chars(id);
+
+  if (auto* c = find_log_channel(clean_id)) {
+    return *c;
+  }
+
   srslog_instance& instance = srslog_instance::get();
-  return fetch_log_channel_helper(
-      clean_id, s, instance.get_backend(), std::move(config));
+  return fetch_log_channel_helper(clean_id, s, instance.get_backend(), std::move(config));
 }
 
 ///
@@ -121,40 +124,46 @@ sink* srslog::find_sink(const std::string& id)
   return (ptr) ? ptr->get() : nullptr;
 }
 
-sink& srslog::fetch_stdout_sink(const std::string& id,
-                                std::unique_ptr<log_formatter> f)
+sink& srslog::fetch_stdout_sink(const std::string& id, std::unique_ptr<log_formatter> f)
 {
   assert(!id.empty() && "Empty id string");
 
+  if (auto* s = find_sink(id)) {
+    return *s;
+  }
+
   auto& s = srslog_instance::get().get_sink_repo().emplace(
       std::piecewise_construct,
       std::forward_as_tuple(id),
-      std::forward_as_tuple(
-          new stream_sink(sink_stream_type::stdout, std::move(f))));
+      std::forward_as_tuple(new stream_sink(sink_stream_type::stdout, std::move(f))));
 
   return *s;
 }
 
-sink& srslog::fetch_stderr_sink(const std::string& id,
-                                std::unique_ptr<log_formatter> f)
+sink& srslog::fetch_stderr_sink(const std::string& id, std::unique_ptr<log_formatter> f)
 {
   assert(!id.empty() && "Empty id string");
 
+  if (auto* s = find_sink(id)) {
+    return *s;
+  }
+
   auto& s = srslog_instance::get().get_sink_repo().emplace(
       std::piecewise_construct,
       std::forward_as_tuple(id),
-      std::forward_as_tuple(
-          new stream_sink(sink_stream_type::stderr, std::move(f))));
+      std::forward_as_tuple(new stream_sink(sink_stream_type::stderr, std::move(f))));
 
   return *s;
 }
 
-sink& srslog::fetch_file_sink(const std::string& path,
-                              size_t max_size,
-                              std::unique_ptr<log_formatter> f)
+sink& srslog::fetch_file_sink(const std::string& path, size_t max_size, std::unique_ptr<log_formatter> f)
 {
   assert(!path.empty() && "Empty path string");
 
+  if (auto* s = find_sink(path)) {
+    return *s;
+  }
+
   //:TODO: GCC5 or lower versions emits an error if we use the new() expression
   // directly, use redundant piecewise_construct instead.
   auto& s = srslog_instance::get().get_sink_repo().emplace(
@@ -169,9 +178,8 @@ bool srslog::install_custom_sink(const std::string& id, std::unique_ptr<sink> s)
 {
   assert(!id.empty() && "Empty path string");
 
-  sink* input_sink = s.get();
-  sink* returned_sink =
-      srslog_instance::get().get_sink_repo().emplace(id, std::move(s)).get();
+  sink* input_sink    = s.get();
+  sink* returned_sink = srslog_instance::get().get_sink_repo().emplace(id, std::move(s)).get();
 
   // Successful insertion occurs when the returned object is the same one as the
   // input object.
@@ -199,7 +207,7 @@ void srslog::flush()
   // The backend will set this shared variable when done.
   detail::shared_variable<bool> completion_flag(false);
 
-  auto sink_ptrs = instance.get_sink_repo().contents();
+  auto               sink_ptrs = instance.get_sink_repo().contents();
   std::vector<sink*> sinks;
   sinks.reserve(sink_ptrs.size());
   for (const auto& s : sink_ptrs) {
@@ -208,8 +216,8 @@ void srslog::flush()
 
   detail::log_entry cmd;
   cmd.metadata.store = nullptr;
-  cmd.flush_cmd = std::unique_ptr<detail::flush_backend_cmd>(
-      new detail::flush_backend_cmd{completion_flag, std::move(sinks)});
+  cmd.flush_cmd =
+      std::unique_ptr<detail::flush_backend_cmd>(new detail::flush_backend_cmd{completion_flag, std::move(sinks)});
 
   // Make sure the flush command gets into the backend, otherwise we will be
   // stuck waiting forever for the command to succeed.
@@ -236,12 +244,10 @@ detail::any* srslog::detail::find_logger(const std::string& id)
   return srslog_instance::get().get_logger_repo().find(id);
 }
 
-detail::any* srslog::detail::fetch_logger(const std::string& id,
-                                          detail::any&& logger)
+detail::any* srslog::detail::fetch_logger(const std::string& id, detail::any&& logger)
 {
   assert(!id.empty() && "Empty id string");
-  return &srslog_instance::get().get_logger_repo().emplace(id,
-                                                           std::move(logger));
+  return &srslog_instance::get().get_logger_repo().emplace(id, std::move(logger));
 }
 
 /// Builds a logger name out of the id and tag.
@@ -251,9 +257,7 @@ static std::string build_logger_name(const std::string& id, char tag)
 }
 
 /// Fetches a logger with all its log channels.
-static basic_logger& fetch_basic_logger_helper(const std::string& id,
-                                               sink& s,
-                                               bool should_print_context)
+static basic_logger& fetch_basic_logger_helper(const std::string& id, sink& s, bool should_print_context)
 {
   static constexpr char basic_logger_chan_tags[] = {'E', 'W', 'I', 'D'};
 
@@ -262,43 +266,48 @@ static basic_logger& fetch_basic_logger_helper(const std::string& id,
   // User created log channels cannot have ids with a # character, encode the
   // ids here with a # to ensure all channels are unique.
 
-  log_channel& error = fetch_log_channel_helper(
-      build_logger_name(id, basic_logger_chan_tags[0]),
-      s,
-      instance.get_backend(),
-      log_channel_config{id, basic_logger_chan_tags[0], should_print_context});
-  log_channel& warning = fetch_log_channel_helper(
-      build_logger_name(id, basic_logger_chan_tags[1]),
-      s,
-      instance.get_backend(),
-      log_channel_config{id, basic_logger_chan_tags[1], should_print_context});
-  log_channel& info = fetch_log_channel_helper(
-      build_logger_name(id, basic_logger_chan_tags[2]),
-      s,
-      instance.get_backend(),
-      log_channel_config{id, basic_logger_chan_tags[2], should_print_context});
-  log_channel& debug = fetch_log_channel_helper(
-      build_logger_name(id, basic_logger_chan_tags[3]),
-      s,
-      instance.get_backend(),
-      log_channel_config{id, basic_logger_chan_tags[3], should_print_context});
+  log_channel& error =
+      fetch_log_channel_helper(build_logger_name(id, basic_logger_chan_tags[0]),
+                               s,
+                               instance.get_backend(),
+                               log_channel_config{id, basic_logger_chan_tags[0], should_print_context});
+  log_channel& warning =
+      fetch_log_channel_helper(build_logger_name(id, basic_logger_chan_tags[1]),
+                               s,
+                               instance.get_backend(),
+                               log_channel_config{id, basic_logger_chan_tags[1], should_print_context});
+  log_channel& info = fetch_log_channel_helper(build_logger_name(id, basic_logger_chan_tags[2]),
+                                               s,
+                                               instance.get_backend(),
+                                               log_channel_config{id, basic_logger_chan_tags[2], should_print_context});
+  log_channel& debug =
+      fetch_log_channel_helper(build_logger_name(id, basic_logger_chan_tags[3]),
+                               s,
+                               instance.get_backend(),
+                               log_channel_config{id, basic_logger_chan_tags[3], should_print_context});
 
   return fetch_logger<basic_logger>(id, error, warning, info, debug);
 }
 
-basic_logger& srslog::fetch_basic_logger(const std::string& id,
-                                         bool should_print_context)
+basic_logger& srslog::fetch_basic_logger(const std::string& id, bool should_print_context)
 {
   assert(!id.empty() && "Empty id string");
-  return fetch_basic_logger_helper(
-      id, srslog_instance::get().get_default_sink(), should_print_context);
+
+  if (auto* logger = find_logger<basic_logger>(id)) {
+    return *logger;
+  }
+
+  return fetch_basic_logger_helper(id, srslog_instance::get().get_default_sink(), should_print_context);
 }
 
-basic_logger& srslog::fetch_basic_logger(const std::string& id,
-                                         sink& s,
-                                         bool should_print_context)
+basic_logger& srslog::fetch_basic_logger(const std::string& id, sink& s, bool should_print_context)
 {
   assert(!id.empty() && "Empty id string");
+
+  if (auto* logger = find_logger<basic_logger>(id)) {
+    return *logger;
+  }
+
   return fetch_basic_logger_helper(id, s, should_print_context);
 }
 
@@ -308,33 +317,28 @@ basic_logger& srslog::fetch_basic_logger(const std::string& id,
 
 /// Creates and registers a log channel. Returns a pointer to the newly created
 /// channel on success, otherwise nullptr.
-static log_channel* create_and_register_log_channel(const std::string& id,
-                                                    sink& s)
+static log_channel* create_and_register_log_channel(const std::string& id, sink& s)
 {
   assert(!id.empty() && "Empty id string");
 
   srslog_instance& instance = srslog_instance::get();
 
   auto& p = instance.get_channel_repo().emplace(
-      std::piecewise_construct,
-      std::forward_as_tuple(id),
-      std::forward_as_tuple(id, s, instance.get_backend()));
+      std::piecewise_construct, std::forward_as_tuple(id), std::forward_as_tuple(id, s, instance.get_backend()));
 
   return &p;
 }
 
-static log_channel* create_and_register_log_channel(const std::string& id,
-                                                    log_channel_config config,
-                                                    sink& s)
+static log_channel* create_and_register_log_channel(const std::string& id, log_channel_config config, sink& s)
 {
   assert(!id.empty() && "Empty id string");
 
   srslog_instance& instance = srslog_instance::get();
 
-  auto& p = instance.get_channel_repo().emplace(
-      std::piecewise_construct,
-      std::forward_as_tuple(id),
-      std::forward_as_tuple(id, s, instance.get_backend(), std::move(config)));
+  auto& p =
+      instance.get_channel_repo().emplace(std::piecewise_construct,
+                                          std::forward_as_tuple(id),
+                                          std::forward_as_tuple(id, s, instance.get_backend(), std::move(config)));
 
   return &p;
 }
@@ -373,16 +377,11 @@ sink* srslog::create_file_sink(const std::string& path, size_t max_size)
       .get_sink_repo()
       .emplace(std::piecewise_construct,
                std::forward_as_tuple(path),
-               std::forward_as_tuple(new file_sink(
-                   path,
-                   max_size,
-                   std::unique_ptr<log_formatter>(new text_formatter))))
+               std::forward_as_tuple(new file_sink(path, max_size, std::unique_ptr<log_formatter>(new text_formatter))))
       .get();
 }
 
-basic_logger* srslog::create_basic_logger(const std::string& id,
-                                          sink& s,
-                                          bool should_print_context)
+basic_logger* srslog::create_basic_logger(const std::string& id, sink& s, bool should_print_context)
 {
   assert(!id.empty() && "Empty id string");
 
@@ -400,24 +399,16 @@ basic_logger* srslog::create_basic_logger(const std::string& id,
   // without any id clashes.
 
   log_channel* error = create_and_register_log_channel(
-      build_logger_name(id, basic_logger_chan_tags[0]),
-      {id, basic_logger_chan_tags[0], should_print_context},
-      s);
+      build_logger_name(id, basic_logger_chan_tags[0]), {id, basic_logger_chan_tags[0], should_print_context}, s);
   assert(error && "Could not create channel");
   log_channel* warning = create_and_register_log_channel(
-      build_logger_name(id, basic_logger_chan_tags[1]),
-      {id, basic_logger_chan_tags[1], should_print_context},
-      s);
+      build_logger_name(id, basic_logger_chan_tags[1]), {id, basic_logger_chan_tags[1], should_print_context}, s);
   assert(warning && "Could not create channel");
   log_channel* info = create_and_register_log_channel(
-      build_logger_name(id, basic_logger_chan_tags[2]),
-      {id, basic_logger_chan_tags[2], should_print_context},
-      s);
+      build_logger_name(id, basic_logger_chan_tags[2]), {id, basic_logger_chan_tags[2], should_print_context}, s);
   assert(info && "Could not create channel");
   log_channel* debug = create_and_register_log_channel(
-      build_logger_name(id, basic_logger_chan_tags[3]),
-      {id, basic_logger_chan_tags[3], should_print_context},
-      s);
+      build_logger_name(id, basic_logger_chan_tags[3]), {id, basic_logger_chan_tags[3], should_print_context}, s);
   assert(debug && "Could not create channel");
 
   return create_logger<basic_logger>(id, *error, *warning, *info, *debug);
diff --git a/lib/src/upper/pdcp.cc b/lib/src/upper/pdcp.cc
index 6fc9aad20..2b56aa381 100644
--- a/lib/src/upper/pdcp.cc
+++ b/lib/src/upper/pdcp.cc
@@ -109,22 +109,19 @@ void pdcp::add_bearer(uint32_t lcid, pdcp_config_t cfg)
       logger.error("Can not configure PDCP entity");
       return;
     }
-    
+
     if (not pdcp_array.insert(std::make_pair(lcid, std::move(entity))).second) {
       logger.error("Error inserting PDCP entity in to array.");
       return;
     }
-    logger.info("Add %s (lcid=%d, bearer_id=%d, sn_len=%dbits)",
-                rrc->get_rb_name(lcid).c_str(),
-                lcid,
-                cfg.bearer_id,
-                cfg.sn_len);
+    logger.info(
+        "Add %s (lcid=%d, bearer_id=%d, sn_len=%dbits)", rrc->get_rb_name(lcid), lcid, cfg.bearer_id, cfg.sn_len);
     {
       std::lock_guard<std::mutex> lock(cache_mutex);
       valid_lcids_cached.insert(lcid);
     }
   } else {
-    logger.info("Bearer %s already configured.", rrc->get_rb_name(lcid).c_str());
+    logger.info("Bearer %s already configured.", rrc->get_rb_name(lcid));
   }
 }
 
@@ -132,27 +129,20 @@ void pdcp::add_bearer_mrb(uint32_t lcid, pdcp_config_t cfg)
 {
   if (not valid_mch_lcid(lcid)) {
     std::unique_ptr<pdcp_entity_lte> entity;
-    entity.reset(new pdcp_entity_lte{rlc, rrc, gw, task_sched, logger, lcid});    
-    if(not entity->configure(cfg)){
+    entity.reset(new pdcp_entity_lte{rlc, rrc, gw, task_sched, logger, lcid});
+    if (not entity->configure(cfg)) {
       logger.error("Can not configure PDCP entity");
-      return; 
+      return;
     }
 
-    if (not pdcp_array_mrb
-                .insert(std::make_pair(
-                    lcid,
-                    std::move(entity)))
-                .second) {
+    if (not pdcp_array_mrb.insert(std::make_pair(lcid, std::move(entity))).second) {
       logger.error("Error inserting PDCP entity in to array.");
       return;
     }
-    logger.info("Add %s (lcid=%d, bearer_id=%d, sn_len=%dbits)",
-                rrc->get_rb_name(lcid).c_str(),
-                lcid,
-                cfg.bearer_id,
-                cfg.sn_len);
+    logger.info(
+        "Add %s (lcid=%d, bearer_id=%d, sn_len=%dbits)", rrc->get_rb_name(lcid), lcid, cfg.bearer_id, cfg.sn_len);
   } else {
-    logger.warning("Bearer %s already configured. Reconfiguration not supported", rrc->get_rb_name(lcid).c_str());
+    logger.warning("Bearer %s already configured. Reconfiguration not supported", rrc->get_rb_name(lcid));
   }
 }
 
@@ -164,9 +154,9 @@ void pdcp::del_bearer(uint32_t lcid)
   }
   if (valid_lcid(lcid)) {
     pdcp_array.erase(lcid);
-    logger.warning("Deleted PDCP bearer %s", rrc->get_rb_name(lcid).c_str());
+    logger.warning("Deleted PDCP bearer %s", rrc->get_rb_name(lcid));
   } else {
-    logger.warning("Can't delete bearer %s. Bearer doesn't exist.", rrc->get_rb_name(lcid).c_str());
+    logger.warning("Can't delete bearer %s. Bearer doesn't exist.", rrc->get_rb_name(lcid));
   }
 }
 
@@ -189,7 +179,7 @@ void pdcp::change_lcid(uint32_t old_lcid, uint32_t new_lcid)
     logger.warning("Changed LCID of PDCP bearer from %d to %d", old_lcid, new_lcid);
   } else {
     logger.error("Can't change PDCP of bearer %s from %d to %d. Bearer doesn't exist or new LCID already occupied.",
-                 rrc->get_rb_name(old_lcid).c_str(),
+                 rrc->get_rb_name(old_lcid),
                  old_lcid,
                  new_lcid);
   }
diff --git a/lib/src/upper/pdcp_entity_lte.cc b/lib/src/upper/pdcp_entity_lte.cc
index 8d28c298e..fd73ad056 100644
--- a/lib/src/upper/pdcp_entity_lte.cc
+++ b/lib/src/upper/pdcp_entity_lte.cc
@@ -79,7 +79,7 @@ bool pdcp_entity_lte::configure(const pdcp_config_t& cnfg_)
   // Queue Helpers
   maximum_allocated_sns_window = (1u << cfg.sn_len) / 2u;
 
-  logger.info("Init %s with bearer ID: %d", rrc->get_rb_name(lcid).c_str(), cfg.bearer_id);
+  logger.info("Init %s with bearer ID: %d", rrc->get_rb_name(lcid), cfg.bearer_id);
   logger.info("SN len bits: %d, SN len bytes: %d, reordering window: %d, Maximum SN: %d, discard timer: %d ms",
               cfg.sn_len,
               cfg.hdr_len_bytes,
@@ -104,7 +104,7 @@ bool pdcp_entity_lte::configure(const pdcp_config_t& cnfg_)
 // Reestablishment procedure: 36.323 5.2
 void pdcp_entity_lte::reestablish()
 {
-  logger.info("Re-establish %s with bearer ID: %d", rrc->get_rb_name(lcid).c_str(), cfg.bearer_id);
+  logger.info("Re-establish %s with bearer ID: %d", rrc->get_rb_name(lcid), cfg.bearer_id);
   // For SRBs
   if (is_srb()) {
     st.next_pdcp_tx_sn = 0;
@@ -126,7 +126,7 @@ void pdcp_entity_lte::reestablish()
 void pdcp_entity_lte::reset()
 {
   if (active) {
-    logger.debug("Reset %s", rrc->get_rb_name(lcid).c_str());
+    logger.debug("Reset %s", rrc->get_rb_name(lcid));
   }
   active = false;
 }
@@ -135,7 +135,7 @@ void pdcp_entity_lte::reset()
 void pdcp_entity_lte::write_sdu(unique_byte_buffer_t sdu, int upper_sn)
 {
   if (rlc->sdu_queue_is_full(lcid)) {
-    logger.info(sdu->msg, sdu->N_bytes, "Dropping %s SDU due to full queue", rrc->get_rb_name(lcid).c_str());
+    logger.info(sdu->msg, sdu->N_bytes, "Dropping %s SDU due to full queue", rrc->get_rb_name(lcid));
     return;
   }
 
@@ -157,7 +157,7 @@ void pdcp_entity_lte::write_sdu(unique_byte_buffer_t sdu, int upper_sn)
   if (!rlc->rb_is_um(lcid) and is_drb()) {
     if (not store_sdu(used_sn, sdu)) {
       // Could not store the SDU, discarding
-      logger.info("Could not store SDU. Discarding %d\n", used_sn);
+      logger.warning("Could not store SDU. Discarding SN=%d", used_sn);
       return;
     }
   }
@@ -190,7 +190,7 @@ void pdcp_entity_lte::write_sdu(unique_byte_buffer_t sdu, int upper_sn)
   logger.info(sdu->msg,
               sdu->N_bytes,
               "TX %s PDU, SN=%d, integrity=%s, encryption=%s",
-              rrc->get_rb_name(lcid).c_str(),
+              rrc->get_rb_name(lcid),
               used_sn,
               srsran_direction_text[integrity_direction],
               srsran_direction_text[encryption_direction]);
@@ -242,7 +242,7 @@ void pdcp_entity_lte::write_pdu(unique_byte_buffer_t pdu)
   logger.info(pdu->msg,
               pdu->N_bytes,
               "%s Rx PDU SN=%d (%d B, integrity=%s, encryption=%s)",
-              rrc->get_rb_name(lcid).c_str(),
+              rrc->get_rb_name(lcid),
               sn,
               pdu->N_bytes,
               srsran_direction_text[integrity_direction],
@@ -302,7 +302,7 @@ void pdcp_entity_lte::handle_srb_pdu(srsran::unique_byte_buffer_t pdu)
     cipher_decrypt(&pdu->msg[cfg.hdr_len_bytes], pdu->N_bytes - cfg.hdr_len_bytes, count, &pdu->msg[cfg.hdr_len_bytes]);
   }
 
-  logger.debug(pdu->msg, pdu->N_bytes, "%s Rx SDU SN=%d", rrc->get_rb_name(lcid).c_str(), sn);
+  logger.debug(pdu->msg, pdu->N_bytes, "%s Rx SDU SN=%d", rrc->get_rb_name(lcid), sn);
 
   // Extract MAC
   uint8_t mac[4];
@@ -311,7 +311,7 @@ void pdcp_entity_lte::handle_srb_pdu(srsran::unique_byte_buffer_t pdu)
   // Perfrom integrity checks
   if (integrity_direction == DIRECTION_RX || integrity_direction == DIRECTION_TXRX) {
     if (not integrity_verify(pdu->msg, pdu->N_bytes, count, mac)) {
-      logger.error(pdu->msg, pdu->N_bytes, "%s Dropping PDU", rrc->get_rb_name(lcid).c_str());
+      logger.error(pdu->msg, pdu->N_bytes, "%s Dropping PDU", rrc->get_rb_name(lcid));
       return; // Discard
     }
   }
@@ -349,7 +349,7 @@ void pdcp_entity_lte::handle_um_drb_pdu(srsran::unique_byte_buffer_t pdu)
     cipher_decrypt(pdu->msg, pdu->N_bytes, count, pdu->msg);
   }
 
-  logger.debug(pdu->msg, pdu->N_bytes, "%s Rx PDU SN=%d", rrc->get_rb_name(lcid).c_str(), sn);
+  logger.debug(pdu->msg, pdu->N_bytes, "%s Rx PDU SN=%d", rrc->get_rb_name(lcid), sn);
 
   st.next_pdcp_rx_sn = sn + 1;
   if (st.next_pdcp_rx_sn > maximum_pdcp_sn) {
@@ -413,7 +413,7 @@ void pdcp_entity_lte::handle_am_drb_pdu(srsran::unique_byte_buffer_t pdu)
 
   // Decrypt
   cipher_decrypt(pdu->msg, pdu->N_bytes, count, pdu->msg);
-  logger.debug(pdu->msg, pdu->N_bytes, "%s Rx SDU SN=%d", rrc->get_rb_name(lcid).c_str(), sn);
+  logger.debug(pdu->msg, pdu->N_bytes, "%s Rx SDU SN=%d", rrc->get_rb_name(lcid), sn);
 
   // Update info on last PDU submitted to upper layers
   st.last_submitted_pdcp_rx_sn = sn;
@@ -697,7 +697,7 @@ bool pdcp_entity_lte::store_sdu(uint32_t sn, const unique_byte_buffer_t& sdu)
 // Discard Timer Callback (discardTimer)
 void pdcp_entity_lte::discard_callback::operator()(uint32_t timer_id)
 {
-  parent->logger.debug("Discard timer expired for PDU with SN = %d", discard_sn);
+  parent->logger.info("Discard timer for SN=%d expired", discard_sn);
 
   // Notify the RLC of the discard. It's the RLC to actually discard, if no segment was transmitted yet.
   parent->rlc->discard_sdu(parent->lcid, discard_sn);
diff --git a/lib/src/upper/pdcp_entity_nr.cc b/lib/src/upper/pdcp_entity_nr.cc
index 1986083b2..db6e2dba1 100644
--- a/lib/src/upper/pdcp_entity_nr.cc
+++ b/lib/src/upper/pdcp_entity_nr.cc
@@ -46,7 +46,7 @@ pdcp_entity_nr::~pdcp_entity_nr() {}
 // Reestablishment procedure: 38.323 5.2
 void pdcp_entity_nr::reestablish()
 {
-  logger.info("Re-establish %s with bearer ID: %d", rrc->get_rb_name(lcid).c_str(), cfg.bearer_id);
+  logger.info("Re-establish %s with bearer ID: %d", rrc->get_rb_name(lcid), cfg.bearer_id);
   // TODO
 }
 
@@ -62,7 +62,7 @@ bool pdcp_entity_nr::configure(const pdcp_config_t& cnfg_)
   if (static_cast<uint32_t>(cfg.t_reordering) > 0) {
     reordering_timer.set(static_cast<uint32_t>(cfg.t_reordering), *reordering_fnc);
   }
-  active               = true;
+  active = true;
   return true;
 }
 
@@ -70,7 +70,7 @@ bool pdcp_entity_nr::configure(const pdcp_config_t& cnfg_)
 void pdcp_entity_nr::reset()
 {
   active = false;
-  logger.debug("Reset %s", rrc->get_rb_name(lcid).c_str());
+  logger.debug("Reset %s", rrc->get_rb_name(lcid));
 }
 
 // SDAP/RRC interface
@@ -80,7 +80,7 @@ void pdcp_entity_nr::write_sdu(unique_byte_buffer_t sdu, int sn)
   logger.info(sdu->msg,
               sdu->N_bytes,
               "TX %s SDU, integrity=%s, encryption=%s",
-              rrc->get_rb_name(lcid).c_str(),
+              rrc->get_rb_name(lcid),
               srsran_direction_text[integrity_direction],
               srsran_direction_text[encryption_direction]);
 
@@ -136,7 +136,7 @@ void pdcp_entity_nr::write_pdu(unique_byte_buffer_t pdu)
   logger.info(pdu->msg,
               pdu->N_bytes,
               "RX %s PDU (%d B), integrity=%s, encryption=%s",
-              rrc->get_rb_name(lcid).c_str(),
+              rrc->get_rb_name(lcid),
               pdu->N_bytes,
               srsran_direction_text[integrity_direction],
               srsran_direction_text[encryption_direction]);
diff --git a/lib/src/upper/rlc.cc b/lib/src/upper/rlc.cc
index eb08bd0bf..8fd146474 100644
--- a/lib/src/upper/rlc.cc
+++ b/lib/src/upper/rlc.cc
@@ -157,7 +157,7 @@ void rlc::reestablish()
 void rlc::reestablish(uint32_t lcid)
 {
   if (valid_lcid(lcid)) {
-    logger.info("Reestablishing %s", rrc->get_rb_name(lcid).c_str());
+    logger.info("Reestablishing %s", rrc->get_rb_name(lcid));
     rlc_array.at(lcid)->reestablish();
   } else {
     logger.warning("RLC LCID %d doesn't exist.", lcid);
@@ -451,7 +451,7 @@ void rlc::add_bearer(uint32_t lcid, const rlc_config_t& cnfg)
       logger.error("Error inserting RLC entity in to array.");
       goto delete_and_exit;
     }
-    logger.info("Added radio bearer %s in %s", rrc->get_rb_name(lcid).c_str(), to_string(cnfg.rlc_mode).c_str());
+    logger.info("Added radio bearer %s in %s", rrc->get_rb_name(lcid), to_string(cnfg.rlc_mode).c_str());
     rlc_entity = NULL;
   }
 
@@ -463,7 +463,7 @@ void rlc::add_bearer(uint32_t lcid, const rlc_config_t& cnfg)
     }
   }
 
-  logger.info("Configured radio bearer %s in %s", rrc->get_rb_name(lcid).c_str(), to_string(cnfg.rlc_mode).c_str());
+  logger.info("Configured radio bearer %s in %s", rrc->get_rb_name(lcid), to_string(cnfg.rlc_mode).c_str());
 
 delete_and_exit:
   if (rlc_entity) {
@@ -510,9 +510,9 @@ void rlc::del_bearer(uint32_t lcid)
     it->second->stop();
     delete (it->second);
     rlc_array.erase(it);
-    logger.warning("Deleted RLC bearer %s", rrc->get_rb_name(lcid).c_str());
+    logger.warning("Deleted RLC bearer %s", rrc->get_rb_name(lcid));
   } else {
-    logger.error("Can't delete bearer %s. Bearer doesn't exist.", rrc->get_rb_name(lcid).c_str());
+    logger.error("Can't delete bearer %s. Bearer doesn't exist.", rrc->get_rb_name(lcid));
   }
 }
 
@@ -525,9 +525,9 @@ void rlc::del_bearer_mrb(uint32_t lcid)
     it->second->stop();
     delete (it->second);
     rlc_array_mrb.erase(it);
-    logger.warning("Deleted RLC MRB bearer %s", rrc->get_rb_name(lcid).c_str());
+    logger.warning("Deleted RLC MRB bearer %s", rrc->get_rb_name(lcid));
   } else {
-    logger.error("Can't delete bearer %s. Bearer doesn't exist.", rrc->get_rb_name(lcid).c_str());
+    logger.error("Can't delete bearer %s. Bearer doesn't exist.", rrc->get_rb_name(lcid));
   }
 }
 
@@ -554,7 +554,7 @@ void rlc::change_lcid(uint32_t old_lcid, uint32_t new_lcid)
     }
   } else {
     logger.error("Can't change LCID of bearer %s from %d to %d. Bearer doesn't exist or new LCID already occupied.",
-                 rrc->get_rb_name(old_lcid).c_str(),
+                 rrc->get_rb_name(old_lcid),
                  old_lcid,
                  new_lcid);
   }
@@ -565,26 +565,26 @@ void rlc::suspend_bearer(uint32_t lcid)
 {
   if (valid_lcid(lcid)) {
     if (rlc_array.at(lcid)->suspend()) {
-      logger.info("Suspended radio bearer %s", rrc->get_rb_name(lcid).c_str());
+      logger.info("Suspended radio bearer %s", rrc->get_rb_name(lcid));
     } else {
       logger.error("Error suspending RLC entity: bearer already suspended.");
     }
   } else {
-    logger.error("Suspending bearer: bearer %s not configured.", rrc->get_rb_name(lcid).c_str());
+    logger.error("Suspending bearer: bearer %s not configured.", rrc->get_rb_name(lcid));
   }
 }
 
 void rlc::resume_bearer(uint32_t lcid)
 {
-  logger.info("Resuming radio bearer %s", rrc->get_rb_name(lcid).c_str());
+  logger.info("Resuming radio bearer %s", rrc->get_rb_name(lcid));
   if (valid_lcid(lcid)) {
     if (rlc_array.at(lcid)->resume()) {
-      logger.info("Resumed radio bearer %s", rrc->get_rb_name(lcid).c_str());
+      logger.info("Resumed radio bearer %s", rrc->get_rb_name(lcid));
     } else {
       logger.error("Error resuming RLC entity: bearer not suspended.");
     }
   } else {
-    logger.error("Resuming bearer: bearer %s not configured.", rrc->get_rb_name(lcid).c_str());
+    logger.error("Resuming bearer: bearer %s not configured.", rrc->get_rb_name(lcid));
   }
 }
 
diff --git a/lib/src/upper/rlc_tm.cc b/lib/src/upper/rlc_tm.cc
index 53874a80f..6ab1e7a50 100644
--- a/lib/src/upper/rlc_tm.cc
+++ b/lib/src/upper/rlc_tm.cc
@@ -20,6 +20,7 @@
  */
 
 #include "srsran/upper/rlc_tm.h"
+#include "srsran/common/lte_common.h"
 #include "srsran/interfaces/ue_pdcp_interfaces.h"
 #include "srsran/interfaces/ue_rrc_interfaces.h"
 
@@ -91,14 +92,14 @@ void rlc_tm::write_sdu(unique_byte_buffer_t sdu)
       logger.info(msg_ptr,
                   nof_bytes,
                   "%s Tx SDU, queue size=%d, bytes=%d",
-                  rrc->get_rb_name(lcid).c_str(),
+                  rrc->get_rb_name(lcid),
                   ul_queue.size(),
                   ul_queue.size_bytes());
     } else {
       logger.warning(ret.error()->msg,
                      ret.error()->N_bytes,
                      "[Dropped SDU] %s Tx SDU, queue size=%d, bytes=%d",
-                     rrc->get_rb_name(lcid).c_str(),
+                     rrc->get_rb_name(lcid),
                      ul_queue.size(),
                      ul_queue.size_bytes());
     }
@@ -146,8 +147,7 @@ int rlc_tm::read_pdu(uint8_t* payload, uint32_t nof_bytes)
 {
   uint32_t pdu_size = ul_queue.size_tail_bytes();
   if (pdu_size > nof_bytes) {
-    logger.info(
-        "%s Tx PDU size larger than MAC opportunity (%d > %d)", rrc->get_rb_name(lcid).c_str(), pdu_size, nof_bytes);
+    logger.info("%s Tx PDU size larger than MAC opportunity (%d > %d)", rrc->get_rb_name(lcid), pdu_size, nof_bytes);
     return -1;
   }
   unique_byte_buffer_t buf;
@@ -155,12 +155,12 @@ int rlc_tm::read_pdu(uint8_t* payload, uint32_t nof_bytes)
     pdu_size = buf->N_bytes;
     memcpy(payload, buf->msg, buf->N_bytes);
     logger.debug("%s Complete SDU scheduled for tx. Stack latency: %" PRIu64 " us",
-                 rrc->get_rb_name(lcid).c_str(),
+                 rrc->get_rb_name(lcid),
                  (uint64_t)buf->get_latency_us().count());
     logger.info(payload,
                 pdu_size,
                 "%s Tx %s PDU, queue size=%d, bytes=%d",
-                rrc->get_rb_name(lcid).c_str(),
+                rrc->get_rb_name(lcid),
                 srsran::to_string(rlc_mode_t::tm).c_str(),
                 ul_queue.size(),
                 ul_queue.size_bytes());
@@ -186,7 +186,7 @@ void rlc_tm::write_pdu(uint8_t* payload, uint32_t nof_bytes)
     buf->set_timestamp();
     metrics.num_rx_pdu_bytes += nof_bytes;
     metrics.num_rx_pdus++;
-    if (rrc->get_rb_name(lcid) == "SRB0") {
+    if (srsran::srb_to_lcid(srsran::lte_srb::srb0) == lcid) {
       rrc->write_pdu(lcid, std::move(buf));
     } else {
       pdcp->write_pdu(lcid, std::move(buf));
diff --git a/lib/test/common/timer_test.cc b/lib/test/common/timer_test.cc
index f63ecac90..99de7a891 100644
--- a/lib/test/common/timer_test.cc
+++ b/lib/test/common/timer_test.cc
@@ -19,20 +19,13 @@
  *
  */
 
+#include "srsran/common/test_common.h"
 #include "srsran/common/timers.h"
 #include <iostream>
 #include <random>
 #include <srsran/common/tti_sync_cv.h>
 #include <thread>
 
-#define TESTASSERT(cond)                                                                                               \
-  do {                                                                                                                 \
-    if (!(cond)) {                                                                                                     \
-      std::cout << "[" << __FUNCTION__ << "][Line " << __LINE__ << "]: FAIL at " << (#cond) << std::endl;              \
-      return -1;                                                                                                       \
-    }                                                                                                                  \
-  } while (0)
-
 using namespace srsran;
 
 int timers_test1()
@@ -51,8 +44,7 @@ int timers_test1()
 
     // TEST: Run multiple times with the same duration
     bool callback_called = false;
-    t.set(dur, [&callback_called](int) { callback_called = true; });
-    TESTASSERT(timers.get_cur_time() == 0);
+    t.set(dur, [&callback_called](int tid) { callback_called = true; });
     for (uint32_t runs = 0; runs < 3; ++runs) {
       callback_called = false;
       TESTASSERT(not t.is_running());
@@ -66,7 +58,6 @@ int timers_test1()
       TESTASSERT(not t.is_running() and t.is_expired());
       TESTASSERT(callback_called);
     }
-    TESTASSERT(timers.get_cur_time() == 3 * dur);
 
     // TEST: interrupt a timer. check if callback was called
     callback_called = false;
@@ -330,10 +321,7 @@ int timers_test5()
     std::string string = "test string";
     timers.defer_callback(2, [&vals, string]() {
       vals.push_back(2);
-      if (string != "test string") {
-        ERROR("string was not captured correctly");
-        exit(-1);
-      }
+      srsran_assert(string == "test string", "string was not captured correctly");
     });
   }
   timers.defer_callback(6, [&vals]() { vals.push_back(3); });
diff --git a/lib/test/mac/mac_pdu_nr_test.cc b/lib/test/mac/mac_pdu_nr_test.cc
index 5b4a0d38a..7998a0d34 100644
--- a/lib/test/mac/mac_pdu_nr_test.cc
+++ b/lib/test/mac/mac_pdu_nr_test.cc
@@ -633,6 +633,31 @@ int mac_dl_sch_pdu_unpack_and_pack_test6()
   return SRSRAN_SUCCESS;
 }
 
+int mac_ul_sch_pdu_unpack_test6()
+{
+  // Malformed MAC PDU
+  uint8_t tv[] = {0x04, 0xe7, 0x00, 0x80, 0x04, 0x45, 0x00, 0x00, 0xe4, 0x4b, 0x9d, 0x40, 0x00, 0x40, 0x01, 0x69, 0x28,
+                  0xc0, 0xa8, 0x02, 0x02, 0xc0, 0xa8, 0x02, 0x01, 0x08, 0x00, 0x3a, 0xbc, 0x65, 0x4a, 0x00, 0x01, 0x94,
+                  0x85, 0x70, 0x60, 0x00, 0x00, 0x00, 0x00, 0xaa, 0x1b, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x11,
+                  0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, 0x21, 0x22,
+                  0x23, 0x24, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, 0x31, 0x32, 0x33,
+                  0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f, 0x40, 0x41, 0x42, 0x43, 0x44,
+                  0x45, 0x46, 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55,
+                  0x56, 0x57, 0x58, 0x59, 0x5a, 0x5b, 0x5c, 0x5d, 0x5e, 0x5f, 0x60, 0x61, 0x62, 0x63, 0x64, 0x65, 0x66,
+                  0x67, 0x68, 0x69, 0x6a, 0x6b, 0x6c, 0x6d, 0x6e, 0x6f, 0x70, 0x71, 0x72, 0x73, 0x74, 0x75, 0x76, 0x77,
+                  0x78, 0x79, 0x7a, 0x7b, 0x7c, 0x7d, 0x7e, 0x7f, 0x80, 0x81, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88,
+                  0x89, 0x8a, 0x8b, 0x8c, 0x8d, 0x8e, 0x8f, 0x90, 0x91, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97};
+
+  if (pcap_handle) {
+    pcap_handle->write_ul_crnti_nr(tv, sizeof(tv), PCAP_CRNTI, true, PCAP_TTI);
+  }
+
+  srsran::mac_sch_pdu_nr pdu(true);
+  TESTASSERT(pdu.unpack(tv, sizeof(tv)) == SRSRAN_ERROR);
+
+  return SRSRAN_SUCCESS;
+}
+
 int main(int argc, char** argv)
 {
 #if PCAP
@@ -721,6 +746,11 @@ int main(int argc, char** argv)
     return SRSRAN_ERROR;
   }
 
+  if (mac_ul_sch_pdu_unpack_test6()) {
+    fprintf(stderr, "mac_ul_sch_pdu_unpack_test6() failed.\n");
+    return SRSRAN_ERROR;
+  }
+
   if (pcap_handle) {
     pcap_handle->close();
   }
diff --git a/lib/test/phy/phy_dl_nr_test.c b/lib/test/phy/phy_dl_nr_test.c
index 1fb1c7606..704d30b46 100644
--- a/lib/test/phy/phy_dl_nr_test.c
+++ b/lib/test/phy/phy_dl_nr_test.c
@@ -42,6 +42,8 @@ static uint32_t            mcs       = 30; // Set to 30 for steering
 static srsran_sch_cfg_nr_t pdsch_cfg = {};
 static uint32_t            nof_slots = 10;
 static uint32_t            rv_idx    = 0;
+static uint32_t            delay_n   = 4;      // Integer delay
+static float               cfo_hz    = 100.0f; // CFO Hz
 
 static void usage(char* prog)
 {
@@ -55,13 +57,15 @@ static void usage(char* prog)
          srsran_mcs_table_to_str(pdsch_cfg.sch_cfg.mcs_table));
   printf("\t-R Reserve RE: [rb_begin] [rb_end] [rb_stride] [sc_mask] [symbol_mask]\n");
   printf("\t-L Provide number of layers [Default %d]\n", carrier.max_mimo_layers);
+  printf("\t-D Delay signal an integer number of samples [Default %d samples]\n", delay_n);
+  printf("\t-C Frequency shift (CFO) signal in Hz [Default %+.0f Hz]\n", cfo_hz);
   printf("\t-v [set srsran_verbose to debug, default none]\n");
 }
 
 static int parse_args(int argc, char** argv)
 {
   int opt;
-  while ((opt = getopt(argc, argv, "rRPpmnTLv")) != -1) {
+  while ((opt = getopt(argc, argv, "rRPpmnTLDCv")) != -1) {
     switch (opt) {
       case 'P':
         carrier.nof_prb = (uint32_t)strtol(argv[optind], NULL, 10);
@@ -102,6 +106,12 @@ static int parse_args(int argc, char** argv)
       case 'L':
         carrier.max_mimo_layers = (uint32_t)strtol(argv[optind], NULL, 10);
         break;
+      case 'D':
+        delay_n = (uint32_t)strtol(argv[optind], NULL, 10);
+        break;
+      case 'C':
+        cfo_hz = strtof(argv[optind], NULL);
+        break;
       case 'v':
         srsran_verbose++;
         break;
@@ -183,35 +193,39 @@ static int work_ue_dl(srsran_ue_dl_nr_t* ue_dl, srsran_slot_cfg_t* slot, srsran_
 
 int main(int argc, char** argv)
 {
-  int                   ret                      = SRSRAN_ERROR;
-  srsran_enb_dl_nr_t    enb_dl                   = {};
-  srsran_ue_dl_nr_t     ue_dl                    = {};
-  srsran_pdsch_res_nr_t pdsch_res[SRSRAN_MAX_TB] = {};
-  srsran_random_t       rand_gen                 = srsran_random_init(1234);
-  srsran_slot_cfg_t     slot                     = {};
-  struct timeval        t[3]                     = {};
-  uint64_t              pdsch_encode_us          = 0;
-  uint64_t              pdsch_decode_us          = 0;
-  uint64_t              nof_bits                 = 0;
+  int                   ret             = SRSRAN_ERROR;
+  srsran_enb_dl_nr_t    enb_dl          = {};
+  srsran_ue_dl_nr_t     ue_dl           = {};
+  srsran_pdsch_res_nr_t pdsch_res       = {};
+  srsran_random_t       rand_gen        = srsran_random_init(1234);
+  srsran_slot_cfg_t     slot            = {};
+  struct timeval        t[3]            = {};
+  uint64_t              pdsch_encode_us = 0;
+  uint64_t              pdsch_decode_us = 0;
+  uint64_t              nof_bits        = 0;
 
   uint8_t* data_tx[SRSRAN_MAX_TB]        = {};
   uint8_t* data_rx[SRSRAN_MAX_CODEWORDS] = {};
-  cf_t*    buffer                        = NULL;
+  cf_t*    buffer_gnb[SRSRAN_MAX_PORTS]  = {};
+  cf_t*    buffer_ue[SRSRAN_MAX_PORTS]   = {};
 
-  buffer = srsran_vec_cf_malloc(SRSRAN_SF_LEN_PRB(carrier.nof_prb));
-  if (buffer == NULL) {
+  uint32_t sf_len = SRSRAN_SF_LEN_PRB(carrier.nof_prb);
+  buffer_gnb[0]   = srsran_vec_cf_malloc(sf_len);
+  buffer_ue[0]    = srsran_vec_cf_malloc(sf_len);
+  if (buffer_gnb[0] == NULL || buffer_ue[0] == NULL) {
     ERROR("Error malloc");
     goto clean_exit;
   }
 
-  srsran_ue_dl_nr_args_t ue_dl_args        = {};
-  ue_dl_args.nof_rx_antennas               = 1;
-  ue_dl_args.pdsch.sch.disable_simd        = false;
-  ue_dl_args.pdsch.sch.decoder_use_flooded = false;
-  ue_dl_args.pdsch.measure_evm             = true;
-  ue_dl_args.pdcch.disable_simd            = false;
-  ue_dl_args.pdcch.measure_evm             = true;
-  ue_dl_args.nof_max_prb                   = carrier.nof_prb;
+  srsran_ue_dl_nr_args_t ue_dl_args          = {};
+  ue_dl_args.nof_rx_antennas                 = 1;
+  ue_dl_args.pdsch.sch.disable_simd          = false;
+  ue_dl_args.pdsch.sch.decoder_use_flooded   = false;
+  ue_dl_args.pdsch.measure_evm               = true;
+  ue_dl_args.pdsch.disable_zero_re_around_dc = true;
+  ue_dl_args.pdcch.disable_simd              = false;
+  ue_dl_args.pdcch.measure_evm               = true;
+  ue_dl_args.nof_max_prb                     = carrier.nof_prb;
 
   srsran_enb_dl_nr_args_t enb_dl_args = {};
   enb_dl_args.nof_tx_antennas         = 1;
@@ -248,12 +262,12 @@ int main(int argc, char** argv)
     search_space->nof_candidates[L] = srsran_pdcch_nr_max_candidates_coreset(coreset, L);
   }
 
-  if (srsran_ue_dl_nr_init(&ue_dl, &buffer, &ue_dl_args)) {
+  if (srsran_ue_dl_nr_init(&ue_dl, buffer_ue, &ue_dl_args)) {
     ERROR("Error UE DL");
     goto clean_exit;
   }
 
-  if (srsran_enb_dl_nr_init(&enb_dl, &buffer, &enb_dl_args)) {
+  if (srsran_enb_dl_nr_init(&enb_dl, buffer_gnb, &enb_dl_args)) {
     ERROR("Error UE DL");
     goto clean_exit;
   }
@@ -291,7 +305,7 @@ int main(int argc, char** argv)
       goto clean_exit;
     }
 
-    pdsch_res[i].payload = data_rx[i];
+    pdsch_res.tb[i].payload = data_rx[i];
   }
 
   srsran_softbuffer_tx_t softbuffer_tx = {};
@@ -318,6 +332,7 @@ int main(int argc, char** argv)
   pdsch_cfg.grant.nof_layers                       = carrier.max_mimo_layers;
   pdsch_cfg.grant.dci_format                       = srsran_dci_format_nr_1_0;
   pdsch_cfg.grant.nof_dmrs_cdm_groups_without_data = 1;
+  pdsch_cfg.grant.beta_dmrs                        = srsran_convert_dB_to_amplitude(3);
   pdsch_cfg.grant.rnti_type                        = srsran_rnti_type_c;
   pdsch_cfg.grant.rnti                             = 0x4601;
   pdsch_cfg.grant.tb[0].rv                         = rv_idx;
@@ -390,13 +405,29 @@ int main(int argc, char** argv)
         get_time_interval(t);
         pdsch_encode_us += (size_t)(t[0].tv_sec * 1e6 + t[0].tv_usec);
 
+        // Emulate channel delay
+        if (delay_n >= sf_len) {
+          ERROR("Delay exceeds SF length");
+          goto clean_exit;
+        }
+        srsran_vec_cf_copy(&buffer_ue[0][0], &buffer_gnb[0][delay_n], sf_len - delay_n);
+        srsran_vec_cf_copy(&buffer_ue[0][sf_len - delay_n], &buffer_gnb[0][0], delay_n);
+
+        // Emulate channel CFO
+        if (isnormal(cfo_hz) && ue_dl.fft[0].cfg.symbol_sz > 0) {
+          srsran_vec_apply_cfo(buffer_ue[0],
+                               cfo_hz / (ue_dl.fft[0].cfg.symbol_sz * SRSRAN_SUBC_SPACING_NR(carrier.numerology)),
+                               buffer_ue[0],
+                               sf_len);
+        }
+
         for (uint32_t tb = 0; tb < SRSRAN_MAX_TB; tb++) {
           pdsch_cfg.grant.tb[tb].softbuffer.rx = &softbuffer_rx;
           srsran_softbuffer_rx_reset(pdsch_cfg.grant.tb[tb].softbuffer.rx);
         }
 
         gettimeofday(&t[1], NULL);
-        if (work_ue_dl(&ue_dl, &slot, pdsch_res) < SRSRAN_SUCCESS) {
+        if (work_ue_dl(&ue_dl, &slot, &pdsch_res) < SRSRAN_SUCCESS) {
           ERROR("Error running UE DL");
           goto clean_exit;
         }
@@ -404,14 +435,14 @@ int main(int argc, char** argv)
         get_time_interval(t);
         pdsch_decode_us += (size_t)(t[0].tv_sec * 1e6 + t[0].tv_usec);
 
-        if (pdsch_res->evm > 0.001f) {
-          ERROR("Error PDSCH EVM is too high %f", pdsch_res->evm);
+        if (pdsch_res.evm[0] > 0.02f) {
+          ERROR("Error PDSCH EVM is too high %f", pdsch_res.evm[0]);
           goto clean_exit;
         }
 
         // Check CRC only for RV=0
         if (rv_idx == 0) {
-          if (!pdsch_res[0].crc) {
+          if (!pdsch_res.tb[0].crc) {
             ERROR("Failed to match CRC; n_prb=%d; mcs=%d; TBS=%d;", n_prb, mcs, pdsch_cfg.grant.tb[0].tbs);
             goto clean_exit;
           }
@@ -426,7 +457,7 @@ int main(int argc, char** argv)
           }
         }
 
-        INFO("n_prb=%d; mcs=%d; TBS=%d; EVM=%f; PASSED!", n_prb, mcs, pdsch_cfg.grant.tb[0].tbs, pdsch_res[0].evm);
+        INFO("n_prb=%d; mcs=%d; TBS=%d; EVM=%f; PASSED!", n_prb, mcs, pdsch_cfg.grant.tb[0].tbs, pdsch_res.evm[0]);
 
         // Count the Tx/Rx'd number of bits
         nof_bits += pdsch_cfg.grant.tb[0].tbs;
@@ -456,8 +487,11 @@ clean_exit:
       free(data_rx[i]);
     }
   }
-  if (buffer) {
-    free(buffer);
+  if (buffer_gnb[0]) {
+    free(buffer_gnb[0]);
+  }
+  if (buffer_ue[0]) {
+    free(buffer_ue[0]);
   }
   srsran_softbuffer_tx_free(&softbuffer_tx);
   srsran_softbuffer_rx_free(&softbuffer_rx);
diff --git a/lib/test/srslog/text_formatter_test.cpp b/lib/test/srslog/text_formatter_test.cpp
index 69433cbf3..e930936ee 100644
--- a/lib/test/srslog/text_formatter_test.cpp
+++ b/lib/test/srslog/text_formatter_test.cpp
@@ -46,7 +46,7 @@ static bool when_fully_filled_log_entry_then_everything_is_formatted()
   fmt::dynamic_format_arg_store<fmt::printf_context> store;
   text_formatter{}.format(build_log_entry_metadata(&store), buffer);
   std::string result   = fmt::to_string(buffer);
-  std::string expected = "00:00:00.050000 [ABC ] [Z] [   10] Text 88\n";
+  std::string expected = "00:00:00.050000 [ABC    ] [Z] [   10] Text 88\n";
 
   ASSERT_EQ(result, expected);
 
@@ -78,7 +78,7 @@ static bool when_log_entry_without_tag_is_passed_then_tag_is_not_formatted()
   fmt::memory_buffer buffer;
   text_formatter{}.format(std::move(entry), buffer);
   std::string result   = fmt::to_string(buffer);
-  std::string expected = "00:00:00.050000 [ABC ] [   10] Text 88\n";
+  std::string expected = "00:00:00.050000 [ABC    ] [   10] Text 88\n";
 
   ASSERT_EQ(result, expected);
 
@@ -94,7 +94,7 @@ static bool when_log_entry_without_context_is_passed_then_context_is_not_formatt
   fmt::memory_buffer buffer;
   text_formatter{}.format(std::move(entry), buffer);
   std::string result   = fmt::to_string(buffer);
-  std::string expected = "00:00:00.050000 [ABC ] [Z] Text 88\n";
+  std::string expected = "00:00:00.050000 [ABC    ] [Z] Text 88\n";
 
   ASSERT_EQ(result, expected);
 
@@ -111,7 +111,7 @@ static bool when_log_entry_with_hex_dump_is_passed_then_hex_dump_is_formatted()
   fmt::memory_buffer buffer;
   text_formatter{}.format(std::move(entry), buffer);
   std::string result   = fmt::to_string(buffer);
-  std::string expected = "00:00:00.050000 [ABC ] [Z] [   10] Text 88\n"
+  std::string expected = "00:00:00.050000 [ABC    ] [Z] [   10] Text 88\n"
                          "    0000: 00 01 02 03 04 05 06 07 08 09 0a 0b 0c 0d 0e 0f\n"
                          "    0010: 10 11 12 13\n";
 
@@ -185,7 +185,7 @@ static bool when_log_entry_with_only_context_is_passed_then_context_is_formatted
   fmt::memory_buffer buffer;
   text_formatter{}.format_ctx(ctx, std::move(entry), buffer);
   std::string result   = fmt::to_string(buffer);
-  std::string expected = "00:00:00.050000 [ABC ] [Z] [   10] Context dump for "
+  std::string expected = "00:00:00.050000 [ABC    ] [Z] [   10] Context dump for "
                          "\"Complex Context\"\n"
                          "  > List: sector_list\n"
                          "    > Set: sector_metrics\n"
@@ -227,7 +227,7 @@ static bool when_log_entry_with_context_and_message_is_passed_then_context_is_fo
   fmt::memory_buffer buffer;
   text_formatter{}.format_ctx(ctx, std::move(entry), buffer);
   std::string result   = fmt::to_string(buffer);
-  std::string expected = "00:00:00.050000 [ABC ] [Z] [   10] [[sector_metrics_type: event, "
+  std::string expected = "00:00:00.050000 [ABC    ] [Z] [   10] [[sector_metrics_type: event, "
                          "sector_metrics_sector_id: 1, [ue_container_Throughput: 1.2 MB/s, "
                          "ue_container_Address: 10.20.30.40, [RF_SNR: 5.1 dB, RF_PWR: -11 "
                          "dBm][RF_SNR: 10.1 dB, RF_PWR: -20 dBm]][ue_container_Throughput: 10.2 "
diff --git a/lib/test/upper/pdcp_base_test.h b/lib/test/upper/pdcp_base_test.h
index da5f5007b..dd3ab7d9a 100644
--- a/lib/test/upper/pdcp_base_test.h
+++ b/lib/test/upper/pdcp_base_test.h
@@ -85,7 +85,7 @@ public:
   void write_pdu_pcch(srsran::unique_byte_buffer_t pdu) {}
   void write_pdu_mch(uint32_t lcid, srsran::unique_byte_buffer_t pdu) {}
 
-  std::string get_rb_name(uint32_t lcid) { return "None"; }
+  const char* get_rb_name(uint32_t lcid) { return "None"; }
 
   srslog::basic_logger& logger;
 
diff --git a/lib/test/upper/rlc_am_test.cc b/lib/test/upper/rlc_am_test.cc
index bc417c8d3..72b6227a1 100644
--- a/lib/test/upper/rlc_am_test.cc
+++ b/lib/test/upper/rlc_am_test.cc
@@ -80,7 +80,7 @@ public:
   // RRC interface
   void max_retx_attempted() { max_retx_triggered = true; }
 
-  std::string get_rb_name(uint32_t lcid) { return std::string(""); }
+  const char* get_rb_name(uint32_t lcid) { return ""; }
 
   std::vector<unique_byte_buffer_t> sdus;
   rlc_pcap*                         pcap               = nullptr;
diff --git a/lib/test/upper/rlc_common_test.cc b/lib/test/upper/rlc_common_test.cc
index 8c53abeda..8cd5fb7f8 100644
--- a/lib/test/upper/rlc_common_test.cc
+++ b/lib/test/upper/rlc_common_test.cc
@@ -63,7 +63,7 @@ public:
 
   // RRC interface
   void        max_retx_attempted() {}
-  std::string get_rb_name(uint32_t lcid) { return std::string("TestRB"); }
+  const char* get_rb_name(uint32_t lcid) { return "TestRB"; }
   void        set_expected_sdu_len(uint32_t len) { expected_sdu_len = len; }
 
   unique_byte_buffer_t sdus[MAX_NBUFS];
diff --git a/lib/test/upper/rlc_stress_test.cc b/lib/test/upper/rlc_stress_test.cc
index 96d82e6b9..c807d9b4b 100644
--- a/lib/test/upper/rlc_stress_test.cc
+++ b/lib/test/upper/rlc_stress_test.cc
@@ -389,7 +389,7 @@ public:
     std::this_thread::sleep_for(std::chrono::seconds(1));
     exit(1);
   }
-  std::string get_rb_name(uint32_t rx_lcid) { return std::string("DRB1"); }
+  const char* get_rb_name(uint32_t rx_lcid) { return "DRB1"; }
 
   int get_nof_rx_pdus() { return rx_pdus; }
 
diff --git a/lib/test/upper/rlc_test_common.h b/lib/test/upper/rlc_test_common.h
index b7cb56e46..244728ae6 100644
--- a/lib/test/upper/rlc_test_common.h
+++ b/lib/test/upper/rlc_test_common.h
@@ -65,7 +65,7 @@ public:
 
   // RRC interface
   void        max_retx_attempted() {}
-  std::string get_rb_name(uint32_t lcid) { return std::string(""); }
+  const char* get_rb_name(uint32_t lcid) { return ""; }
   void        set_expected_sdu_len(uint32_t len) { expected_sdu_len = len; }
 
   uint32_t get_num_sdus() { return sdus.size(); }
diff --git a/srsenb/hdr/common/common_enb.h b/srsenb/hdr/common/common_enb.h
index 56fe80378..8ad528d9c 100644
--- a/srsenb/hdr/common/common_enb.h
+++ b/srsenb/hdr/common/common_enb.h
@@ -26,7 +26,7 @@
                               INCLUDES
 *******************************************************************************/
 
-#include <memory>
+#include "srsran/common/lte_common.h"
 #include <stdint.h>
 
 namespace srsenb {
@@ -34,28 +34,21 @@ namespace srsenb {
 #define SRSENB_RRC_MAX_N_PLMN_IDENTITIES 6
 
 #define SRSENB_N_SRB 3
-#define SRSENB_N_DRB 8
-#define SRSENB_N_RADIO_BEARERS 11
 #define SRSENB_MAX_UES 64
+const uint32_t MAX_ERAB_ID   = 15;
+const uint32_t MAX_NOF_ERABS = 16;
 
-enum rb_id_t {
-  RB_ID_SRB0 = 0,
-  RB_ID_SRB1,
-  RB_ID_SRB2,
-  RB_ID_DRB1,
-  RB_ID_DRB2,
-  RB_ID_DRB3,
-  RB_ID_DRB4,
-  RB_ID_DRB5,
-  RB_ID_DRB6,
-  RB_ID_DRB7,
-  RB_ID_DRB8,
-  RB_ID_N_ITEMS,
-};
-inline const char* to_string(rb_id_t rb_id)
+using srsran::lte_drb;
+using srsran::lte_srb;
+using srsran::srb_to_lcid;
+inline const char* get_rb_name(uint32_t lcid)
 {
-  const static char* names[] = {"SRB0", "SRB1", "SRB2", "DRB1", "DRB2", "DRB3", "DRB4", "DRB5", "DRB6", "DRB7", "DRB8"};
-  return (rb_id < RB_ID_N_ITEMS) ? names[rb_id] : "invalid bearer id";
+  return (srsran::is_lte_srb(lcid)) ? srsran::get_srb_name(srsran::lte_lcid_to_srb(lcid))
+                                    : srsran::get_drb_name(static_cast<srsran::lte_drb>(lcid - srsran::MAX_LTE_SRB_ID));
+}
+constexpr uint32_t drb_to_lcid(lte_drb drb_id)
+{
+  return srb_to_lcid(lte_srb::srb2) + static_cast<uint32_t>(drb_id);
 }
 
 // Cat 3 UE - Max number of DL-SCH transport block bits received within a TTI
diff --git a/srsenb/hdr/enb.h b/srsenb/hdr/enb.h
index c2a404414..c0acf2258 100644
--- a/srsenb/hdr/enb.h
+++ b/srsenb/hdr/enb.h
@@ -103,6 +103,7 @@ struct general_args_t {
   std::string eia_pref_list;
   std::string eea_pref_list;
   uint32_t    max_mac_dl_kos;
+  uint32_t    max_mac_ul_kos;
 };
 
 struct all_args_t {
diff --git a/srsenb/hdr/stack/rrc/rrc.h b/srsenb/hdr/stack/rrc/rrc.h
index 0e841c7b0..6741661fb 100644
--- a/srsenb/hdr/stack/rrc/rrc.h
+++ b/srsenb/hdr/stack/rrc/rrc.h
@@ -77,7 +77,9 @@ public:
   // rrc_interface_mac
   int      add_user(uint16_t rnti, const sched_interface::ue_cfg_t& init_ue_cfg) override;
   void     upd_user(uint16_t new_rnti, uint16_t old_rnti) override;
-  void     set_activity_user(uint16_t rnti, bool ack_info) override;
+  void     set_activity_user(uint16_t rnti) override;
+  void     set_radiolink_dl_state(uint16_t rnti, bool crc_res) override;
+  void     set_radiolink_ul_state(uint16_t rnti, bool crc_res) override;
   bool     is_paging_opportunity(uint32_t tti, uint32_t* payload_len) override;
   uint8_t* read_pdu_bcch_dlsch(const uint8_t cc_idx, const uint32_t sib_index) override;
 
@@ -86,33 +88,38 @@ public:
   void max_retx_attempted(uint16_t rnti) override;
 
   // rrc_interface_s1ap
-  void write_dl_info(uint16_t rnti, srsran::unique_byte_buffer_t sdu) override;
-  void release_ue(uint16_t rnti) override;
-  bool setup_ue_ctxt(uint16_t rnti, const asn1::s1ap::init_context_setup_request_s& msg) override;
-  bool modify_ue_ctxt(uint16_t rnti, const asn1::s1ap::ue_context_mod_request_s& msg) override;
-  bool setup_ue_erabs(uint16_t rnti, const asn1::s1ap::erab_setup_request_s& msg) override;
-  void modify_erabs(uint16_t                                 rnti,
-                    const asn1::s1ap::erab_modify_request_s& msg,
-                    std::vector<uint16_t>*                   erabs_modified,
-                    std::vector<uint16_t>*                   erabs_failed_to_modify) override;
-  bool modify_ue_erab(uint16_t                                   rnti,
-                      uint8_t                                    erab_id,
-                      const asn1::s1ap::erab_level_qos_params_s& qos_params,
-                      const asn1::unbounded_octstring<true>*     nas_pdu);
-  bool release_erabs(uint32_t rnti) override;
-  void release_erabs(uint32_t                              rnti,
-                     const asn1::s1ap::erab_release_cmd_s& msg,
-                     std::vector<uint16_t>*                erabs_released,
-                     std::vector<uint16_t>*                erabs_failed_to_release) override;
-  void add_paging_id(uint32_t ueid, const asn1::s1ap::ue_paging_id_c& UEPagingID) override;
-  void ho_preparation_complete(uint16_t                     rnti,
-                               bool                         is_success,
-                               const asn1::s1ap::ho_cmd_s&  msg,
-                               srsran::unique_byte_buffer_t rrc_container) override;
-  uint16_t
-       start_ho_ue_resource_alloc(const asn1::s1ap::ho_request_s&                                   msg,
-                                  const asn1::s1ap::sourceenb_to_targetenb_transparent_container_s& container) override;
-  void set_erab_status(uint16_t rnti, const asn1::s1ap::bearers_subject_to_status_transfer_list_l& erabs) override;
+  void     write_dl_info(uint16_t rnti, srsran::unique_byte_buffer_t sdu) override;
+  void     release_ue(uint16_t rnti) override;
+  bool     setup_ue_ctxt(uint16_t rnti, const asn1::s1ap::init_context_setup_request_s& msg) override;
+  bool     modify_ue_ctxt(uint16_t rnti, const asn1::s1ap::ue_context_mod_request_s& msg) override;
+  bool     has_erab(uint16_t rnti, uint32_t erab_id) const override;
+  int      get_erab_addr_in(uint16_t rnti, uint16_t erab_id, transp_addr_t& addr_in, uint32_t& teid_in) const override;
+  void     set_aggregate_max_bitrate(uint16_t rnti, const asn1::s1ap::ue_aggregate_maximum_bitrate_s& bitrate) override;
+  int      setup_erab(uint16_t                                           rnti,
+                      uint16_t                                           erab_id,
+                      const asn1::s1ap::erab_level_qos_params_s&         qos_params,
+                      srsran::const_span<uint8_t>                        nas_pdu,
+                      const asn1::bounded_bitstring<1, 160, true, true>& addr,
+                      uint32_t                                           gtpu_teid_out,
+                      asn1::s1ap::cause_c&                               cause) override;
+  int      modify_erab(uint16_t                                   rnti,
+                       uint16_t                                   erab_id,
+                       const asn1::s1ap::erab_level_qos_params_s& qos_params,
+                       srsran::const_span<uint8_t>                nas_pdu,
+                       asn1::s1ap::cause_c&                       cause) override;
+  bool     release_erabs(uint32_t rnti) override;
+  int      release_erab(uint16_t rnti, uint16_t erab_id) override;
+  void     add_paging_id(uint32_t ueid, const asn1::s1ap::ue_paging_id_c& UEPagingID) override;
+  void     ho_preparation_complete(uint16_t                     rnti,
+                                   rrc::ho_prep_result          result,
+                                   const asn1::s1ap::ho_cmd_s&  msg,
+                                   srsran::unique_byte_buffer_t rrc_container) override;
+  uint16_t start_ho_ue_resource_alloc(const asn1::s1ap::ho_request_s&                                   msg,
+                                      const asn1::s1ap::sourceenb_to_targetenb_transparent_container_s& container,
+                                      asn1::s1ap::cause_c& failure_cause) override;
+  void     set_erab_status(uint16_t rnti, const asn1::s1ap::bearers_subject_to_status_transfer_list_l& erabs) override;
+
+  int notify_ue_erab_updates(uint16_t rnti, srsran::const_byte_span nas_pdu) override;
 
   // rrc_interface_pdcp
   void write_pdu(uint16_t rnti, uint32_t lcid, srsran::unique_byte_buffer_t pdu) override;
@@ -188,6 +195,7 @@ private:
   typedef struct {
     uint16_t                     rnti;
     uint32_t                     lcid;
+    uint32_t                     arg;
     srsran::unique_byte_buffer_t pdu;
   } rrc_pdu;
 
@@ -196,7 +204,8 @@ private:
   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_MAC_KO_USER = 0xffff0006;
+  const static uint32_t LCID_RADLINK_DL  = 0xffff0006;
+  const static uint32_t LCID_RADLINK_UL  = 0xffff0007;
 
   bool                                running = false;
   srsran::dyn_blocking_queue<rrc_pdu> rx_pdu_queue;
diff --git a/srsenb/hdr/stack/rrc/rrc_bearer_cfg.h b/srsenb/hdr/stack/rrc/rrc_bearer_cfg.h
index 87644636c..b8666f8a2 100644
--- a/srsenb/hdr/stack/rrc/rrc_bearer_cfg.h
+++ b/srsenb/hdr/stack/rrc/rrc_bearer_cfg.h
@@ -92,15 +92,17 @@ public:
                 const asn1::s1ap::erab_level_qos_params_s&         qos,
                 const asn1::bounded_bitstring<1, 160, true, true>& addr,
                 uint32_t                                           teid_out,
-                const asn1::unbounded_octstring<true>*             nas_pdu);
-  bool release_erab(uint8_t erab_id);
+                srsran::const_span<uint8_t>                        nas_pdu,
+                asn1::s1ap::cause_c&                               cause);
+  int  release_erab(uint8_t erab_id);
   void release_erabs();
-  bool modify_erab(uint8_t                                    erab_id,
+  int  modify_erab(uint8_t                                    erab_id,
                    const asn1::s1ap::erab_level_qos_params_s& qos,
-                   const asn1::unbounded_octstring<true>*     nas_pdu);
+                   srsran::const_span<uint8_t>                nas_pdu,
+                   asn1::s1ap::cause_c&                       cause);
 
   // Methods to apply bearer updates
-  void                       add_gtpu_bearer(uint32_t erab_id);
+  int                        add_gtpu_bearer(uint32_t erab_id);
   srsran::expected<uint32_t> add_gtpu_bearer(uint32_t                                erab_id,
                                              uint32_t                                teid_out,
                                              uint32_t                                addr,
diff --git a/srsenb/hdr/stack/rrc/rrc_config.h b/srsenb/hdr/stack/rrc/rrc_config.h
index 278400281..f7d2d1175 100644
--- a/srsenb/hdr/stack/rrc/rrc_config.h
+++ b/srsenb/hdr/stack/rrc/rrc_config.h
@@ -69,6 +69,7 @@ struct rrc_cfg_t {
   cell_list_t                                                                             cell_list;
   cell_list_t                                                                             cell_list_nr;
   uint32_t                                                                                max_mac_dl_kos;
+  uint32_t                                                                                max_mac_ul_kos;
 };
 
 constexpr uint32_t UE_PCELL_CC_IDX = 0;
diff --git a/srsenb/hdr/stack/rrc/rrc_mobility.h b/srsenb/hdr/stack/rrc/rrc_mobility.h
index 7f87626dc..f928f5abe 100644
--- a/srsenb/hdr/stack/rrc/rrc_mobility.h
+++ b/srsenb/hdr/stack/rrc/rrc_mobility.h
@@ -39,24 +39,25 @@ public:
     uint16_t crnti;
     uint16_t temp_crnti;
   };
-  struct ho_cancel_ev {};
+  struct ho_cancel_ev {
+    asn1::s1ap::cause_c cause;
+
+    ho_cancel_ev(const asn1::s1ap::cause_c& cause_) : cause(cause_) {}
+  };
 
   rrc_mobility(srsenb::rrc::ue* outer_ue);
 
   bool fill_conn_recfg_no_ho_cmd(asn1::rrc::rrc_conn_recfg_r8_ies_s* conn_recfg);
   void handle_ue_meas_report(const asn1::rrc::meas_report_s& msg, srsran::unique_byte_buffer_t pdu);
-  void handle_ho_preparation_complete(bool                         is_success,
+  void handle_ho_preparation_complete(rrc::ho_prep_result          result,
                                       const asn1::s1ap::ho_cmd_s&  msg,
                                       srsran::unique_byte_buffer_t container);
   bool is_ho_running() const { return not is_in_state<idle_st>(); }
 
   // S1-Handover
   bool start_s1_tenb_ho(const asn1::s1ap::ho_request_s&                                   msg,
-                        const asn1::s1ap::sourceenb_to_targetenb_transparent_container_s& container);
-
-  static uint16_t
-  start_ho_ue_resource_alloc(const asn1::s1ap::ho_request_s&                                   msg,
-                             const asn1::s1ap::sourceenb_to_targetenb_transparent_container_s& container);
+                        const asn1::s1ap::sourceenb_to_targetenb_transparent_container_s& container,
+                        asn1::s1ap::cause_c&                                              cause);
 
 private:
   // helper methods
@@ -72,7 +73,10 @@ private:
                                    const enb_cell_common&    target_cell,
                                    uint32_t                  src_dl_earfcn,
                                    uint32_t                  src_pci);
-  bool apply_ho_prep_cfg(const asn1::rrc::ho_prep_info_r8_ies_s& ho_prep, const asn1::s1ap::ho_request_s& ho_req_msg);
+  bool apply_ho_prep_cfg(const asn1::rrc::ho_prep_info_r8_ies_s& ho_prep,
+                         const asn1::s1ap::ho_request_s&         ho_req_msg,
+                         std::vector<asn1::s1ap::erab_item_s>&   erabs_failed_to_setup,
+                         asn1::s1ap::cause_c&                    cause);
 
   rrc::ue*              rrc_ue  = nullptr;
   rrc*                  rrc_enb = nullptr;
@@ -81,19 +85,24 @@ private:
   // vars
   asn1::rrc::meas_cfg_s                current_meas_cfg;
   asn1::rrc::rrc_conn_recfg_complete_s pending_recfg_complete;
+  asn1::s1ap::cause_c                  failure_cause;
 
   // events
   struct ho_meas_report_ev {
-    uint32_t                                target_eci = 0;
-    const asn1::rrc::meas_obj_to_add_mod_s* meas_obj   = nullptr;
+    uint32_t                                target_eci      = 0;
+    const asn1::rrc::meas_obj_to_add_mod_s* meas_obj        = nullptr;
+    bool                                    direct_fwd_path = false;
   };
   struct ho_req_rx_ev {
     const asn1::s1ap::ho_request_s*                                   ho_req_msg;
     const asn1::s1ap::sourceenb_to_targetenb_transparent_container_s* transparent_container;
   };
-  using unsuccessful_outcome_ev = std::false_type;
-  using recfg_complete_ev       = asn1::rrc::rrc_conn_recfg_complete_s;
-  using status_transfer_ev      = asn1::s1ap::bearers_subject_to_status_transfer_list_l;
+  struct ho_failure_ev {
+    asn1::s1ap::cause_c cause;
+    ho_failure_ev(const asn1::s1ap::cause_c& cause_) : cause(cause_) {}
+  };
+  using recfg_complete_ev  = asn1::rrc::rrc_conn_recfg_complete_s;
+  using status_transfer_ev = asn1::s1ap::bearers_subject_to_status_transfer_list_l;
 
   // states
   struct idle_st {};
@@ -105,6 +114,7 @@ private:
     void enter(rrc_mobility* f, const ho_meas_report_ev& meas_report);
   };
   struct s1_target_ho_st {
+    asn1::s1ap::cause_c   failure_cause;
     std::vector<uint32_t> pending_tunnels;
   };
   struct wait_recfg_comp {};
@@ -123,9 +133,9 @@ private:
     explicit s1_source_ho_st(rrc_mobility* parent_);
 
   private:
-    void handle_ho_cmd(wait_ho_cmd& s, const ho_cmd_msg& ho_cmd);
-    void handle_ho_cancel(const ho_cancel_ev& ev);
-    bool start_enb_status_transfer(const asn1::s1ap::ho_cmd_s& s1ap_ho_cmd);
+    void                handle_ho_cmd(wait_ho_cmd& s, const ho_cmd_msg& ho_cmd);
+    void                handle_ho_cancel(const ho_cancel_ev& ev);
+    asn1::s1ap::cause_c start_enb_status_transfer(const asn1::s1ap::ho_cmd_s& s1ap_ho_cmd);
 
     rrc*                  rrc_enb;
     rrc::ue*              rrc_ue;
@@ -154,6 +164,7 @@ private:
   void handle_crnti_ce(intraenb_ho_st& s, const user_crnti_upd_ev& ev);
   void handle_recfg_complete(intraenb_ho_st& s, const recfg_complete_ev& ev);
   void handle_ho_requested(idle_st& s, const ho_req_rx_ev& ho_req);
+  void handle_ho_failure(const ho_failure_ev& ev);
   void handle_status_transfer(s1_target_ho_st& s, const status_transfer_ev& ev);
   void defer_recfg_complete(s1_target_ho_st& s, const recfg_complete_ev& ev);
   void handle_recfg_complete(wait_recfg_comp& s, const recfg_complete_ev& ev);
@@ -181,6 +192,7 @@ protected:
   row< intraenb_ho_st,  idle_st,           recfg_complete_ev,   &fsm::handle_recfg_complete                            >,
   // +----------------+-------------------+---------------------+----------------------------+-------------------------+
   row< s1_target_ho_st, wait_recfg_comp,   status_transfer_ev,  &fsm::handle_status_transfer                           >,
+  to_state<             idle_st,           ho_failure_ev,       &fsm::handle_ho_failure                                >,
   upd< s1_target_ho_st,                    recfg_complete_ev,   &fsm::defer_recfg_complete                             >,
   row< wait_recfg_comp, idle_st,           recfg_complete_ev,   &fsm::handle_recfg_complete                            >
   // +----------------+-------------------+---------------------+----------------------------+-------------------------+
diff --git a/srsenb/hdr/stack/rrc/rrc_ue.h b/srsenb/hdr/stack/rrc/rrc_ue.h
index 5bbcbde31..7d5deee84 100644
--- a/srsenb/hdr/stack/rrc/rrc_ue.h
+++ b/srsenb/hdr/stack/rrc/rrc_ue.h
@@ -51,7 +51,8 @@ public:
   void        set_activity_timeout(const activity_timeout_type_t type);
   void        set_rlf_timeout();
   void        set_activity();
-  void        mac_ko_activity();
+  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();
@@ -78,9 +79,9 @@ public:
   void send_connection_reject(procedure_result_code cause);
   void send_connection_release();
   void send_connection_reest_rej(procedure_result_code cause);
-  void send_connection_reconf(srsran::unique_byte_buffer_t           sdu             = {},
-                              bool                                   phy_cfg_updated = true,
-                              const asn1::unbounded_octstring<true>* nas_pdu         = nullptr);
+  void send_connection_reconf(srsran::unique_byte_buffer_t sdu             = {},
+                              bool                         phy_cfg_updated = true,
+                              srsran::const_byte_span      nas_pdu         = {});
   void send_security_mode_command();
   void send_ue_cap_enquiry();
   void send_ue_info_req();
@@ -116,16 +117,23 @@ public:
 
   void set_bitrates(const asn1::s1ap::ue_aggregate_maximum_bitrate_s& rates);
 
+  /// Helper to check UE ERABs
+  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 setup_erabs(const asn1::s1ap::erab_to_be_setup_list_bearer_su_req_l& e);
   bool release_erabs();
-  bool release_erab(uint32_t erab_id);
-  bool modify_erab(uint16_t                                   erab_id,
+  int  release_erab(uint32_t erab_id);
+  int  setup_erab(uint16_t                                           erab_id,
+                  const asn1::s1ap::erab_level_qos_params_s&         qos_params,
+                  srsran::const_span<uint8_t>                        nas_pdu,
+                  const asn1::bounded_bitstring<1, 160, true, true>& addr,
+                  uint32_t                                           gtpu_teid_out,
+                  asn1::s1ap::cause_c&                               cause);
+  int  modify_erab(uint16_t                                   erab_id,
                    const asn1::s1ap::erab_level_qos_params_s& qos_params,
-                   const asn1::unbounded_octstring<true>*     nas_pdu);
-
-  void notify_s1ap_ue_ctxt_setup_complete();
-  void notify_s1ap_ue_erab_setup_response(const asn1::s1ap::erab_to_be_setup_list_bearer_su_req_l& e);
+                   srsran::const_span<uint8_t>                nas_pdu,
+                   asn1::s1ap::cause_c&                       cause);
 
   // Getters for PUCCH resources
   int  get_cqi(uint16_t* pmi_idx, uint16_t* n_pucch, uint32_t ue_cc_idx);
@@ -187,7 +195,9 @@ private:
 
   const static uint32_t UE_PCELL_CC_IDX = 0;
 
-  uint32_t consecutive_kos = 0;
+  // consecutive KO counter for DL and UL
+  uint32_t consecutive_kos_dl = 0;
+  uint32_t consecutive_kos_ul = 0;
 
   ue_cell_ded_list     ue_cell_list;
   bearer_cfg_handler   bearer_list;
diff --git a/srsenb/hdr/stack/upper/gtpu.h b/srsenb/hdr/stack/upper/gtpu.h
index 01c41ad41..4c8afea02 100644
--- a/srsenb/hdr/stack/upper/gtpu.h
+++ b/srsenb/hdr/stack/upper/gtpu.h
@@ -64,7 +64,7 @@ public:
 
   struct tunnel {
     uint16_t rnti      = SRSRAN_INVALID_RNTI;
-    uint32_t lcid      = SRSENB_N_RADIO_BEARERS;
+    uint32_t lcid      = srsran::MAX_NOF_BEARERS;
     uint32_t teid_in   = 0;
     uint32_t teid_out  = 0;
     uint32_t spgw_addr = 0;
diff --git a/srsenb/hdr/stack/upper/pdcp.h b/srsenb/hdr/stack/upper/pdcp.h
index 30178389b..c83654621 100644
--- a/srsenb/hdr/stack/upper/pdcp.h
+++ b/srsenb/hdr/stack/upper/pdcp.h
@@ -108,7 +108,7 @@ private:
     void        write_pdu_bcch_dlsch(srsran::unique_byte_buffer_t pdu);
     void        write_pdu_pcch(srsran::unique_byte_buffer_t pdu);
     void        write_pdu_mch(uint32_t lcid, srsran::unique_byte_buffer_t pdu) {}
-    std::string get_rb_name(uint32_t lcid);
+    const char* get_rb_name(uint32_t lcid);
   };
 
   class user_interface
diff --git a/srsenb/hdr/stack/upper/pdcp_nr.h b/srsenb/hdr/stack/upper/pdcp_nr.h
index c269d2839..2c3897d38 100644
--- a/srsenb/hdr/stack/upper/pdcp_nr.h
+++ b/srsenb/hdr/stack/upper/pdcp_nr.h
@@ -96,7 +96,7 @@ private:
     void        write_pdu_bcch_dlsch(srsran::unique_byte_buffer_t pdu) final;
     void        write_pdu_pcch(srsran::unique_byte_buffer_t pdu) final;
     void        write_pdu_mch(uint32_t lcid, srsran::unique_byte_buffer_t pdu) final {}
-    std::string get_rb_name(uint32_t lcid) final;
+    const char* get_rb_name(uint32_t lcid) final;
   };
 
   class user_interface
diff --git a/srsenb/hdr/stack/upper/rlc.h b/srsenb/hdr/stack/upper/rlc.h
index 40504eed3..98d3000b3 100644
--- a/srsenb/hdr/stack/upper/rlc.h
+++ b/srsenb/hdr/stack/upper/rlc.h
@@ -68,7 +68,7 @@ public:
   void        write_sdu(uint16_t rnti, uint32_t lcid, srsran::unique_byte_buffer_t sdu);
   void        discard_sdu(uint16_t rnti, uint32_t lcid, uint32_t discard_sn);
   bool        rb_is_um(uint16_t rnti, uint32_t lcid);
-  std::string get_rb_name(uint32_t lcid);
+  const char* get_rb_name(uint32_t lcid);
   bool        sdu_queue_is_full(uint16_t rnti, uint32_t lcid);
 
   // rlc_interface_mac
@@ -88,7 +88,7 @@ private:
     void        write_pdu_pcch(srsran::unique_byte_buffer_t sdu);
     void        write_pdu_mch(uint32_t lcid, srsran::unique_byte_buffer_t sdu) {}
     void        max_retx_attempted();
-    std::string get_rb_name(uint32_t lcid);
+    const char* get_rb_name(uint32_t lcid);
     uint16_t    rnti;
 
     srsenb::pdcp_interface_rlc*  pdcp;
diff --git a/srsenb/hdr/stack/upper/rlc_nr.h b/srsenb/hdr/stack/upper/rlc_nr.h
index 9a4e2c3a5..6d1ab8258 100644
--- a/srsenb/hdr/stack/upper/rlc_nr.h
+++ b/srsenb/hdr/stack/upper/rlc_nr.h
@@ -56,7 +56,7 @@ public:
   void        write_sdu(uint16_t rnti, uint32_t lcid, srsran::unique_byte_buffer_t sdu);
   bool        rb_is_um(uint16_t rnti, uint32_t lcid);
   bool        sdu_queue_is_full(uint16_t rnti, uint32_t lcid);
-  std::string get_rb_name(uint32_t lcid);
+  const char* get_rb_name(uint32_t lcid);
 
   // rlc_interface_mac_nr
   int read_pdu(uint16_t rnti, uint32_t lcid, uint8_t* payload, uint32_t nof_bytes);
@@ -76,7 +76,7 @@ private:
     void        write_pdu_pcch(srsran::unique_byte_buffer_t sdu);
     void        write_pdu_mch(uint32_t lcid, srsran::unique_byte_buffer_t sdu) {}
     void        max_retx_attempted() final;
-    std::string get_rb_name(uint32_t lcid) final;
+    const char* get_rb_name(uint32_t lcid) final;
     uint16_t    rnti;
 
     srsenb::pdcp_interface_rlc_nr* m_pdcp = nullptr;
diff --git a/srsenb/hdr/stack/upper/s1ap.h b/srsenb/hdr/stack/upper/s1ap.h
index 86b56a75f..f8301771a 100644
--- a/srsenb/hdr/stack/upper/s1ap.h
+++ b/srsenb/hdr/stack/upper/s1ap.h
@@ -25,6 +25,7 @@
 #include <map>
 
 #include "srsenb/hdr/common/common_enb.h"
+#include "srsran/adt/circular_map.h"
 #include "srsran/common/buffer_pool.h"
 #include "srsran/common/common.h"
 #include "srsran/common/s1ap_pcap.h"
@@ -57,7 +58,12 @@ struct ue_ctxt_t {
 
 class s1ap : public s1ap_interface_rrc
 {
+  using s1ap_proc_id_t = asn1::s1ap::s1ap_elem_procs_o::init_msg_c::types_opts::options;
+
 public:
+  using erab_id_list   = srsran::bounded_vector<uint16_t, ASN1_S1AP_MAXNOOF_ERABS>;
+  using erab_item_list = srsran::bounded_vector<asn1::s1ap::erab_item_s, ASN1_S1AP_MAXNOOF_ERABS>;
+
   static const uint32_t ts1_reloc_prep_timeout_ms    = 10000;
   static const uint32_t ts1_reloc_overall_timeout_ms = 10000;
 
@@ -83,29 +89,34 @@ public:
   bool user_exists(uint16_t rnti) override;
   void user_mod(uint16_t old_rnti, uint16_t new_rnti) override;
   bool user_release(uint16_t rnti, asn1::s1ap::cause_radio_network_e cause_radio) override;
-  void ue_ctxt_setup_complete(uint16_t rnti, const asn1::s1ap::init_context_setup_resp_s& res) override;
-  void ue_erab_setup_complete(uint16_t rnti, const asn1::s1ap::erab_setup_resp_s& res) override;
+  void ue_ctxt_setup_complete(uint16_t rnti) override;
   bool is_mme_connected() override;
   bool send_ho_required(uint16_t                     rnti,
                         uint32_t                     target_eci,
                         srsran::plmn_id_t            target_plmn,
                         srsran::span<uint32_t>       fwd_erabs,
-                        srsran::unique_byte_buffer_t rrc_container) override;
+                        srsran::unique_byte_buffer_t rrc_container,
+                        bool                         has_direct_fwd_path) override;
   bool send_enb_status_transfer_proc(uint16_t rnti, std::vector<bearer_status_info>& bearer_status_list) override;
-  bool send_ho_failure(uint32_t mme_ue_s1ap_id);
   bool send_ho_req_ack(const asn1::s1ap::ho_request_s&                msg,
                        uint16_t                                       rnti,
                        uint32_t                                       enb_cc_idx,
                        srsran::unique_byte_buffer_t                   ho_cmd,
-                       srsran::span<asn1::s1ap::erab_admitted_item_s> admitted_bearers) override;
-  void send_ho_notify(uint16_t rnti, uint64_t target_eci) override;
-  void send_ho_cancel(uint16_t rnti) override;
+                       srsran::span<asn1::s1ap::erab_admitted_item_s> admitted_bearers,
+                       srsran::const_span<asn1::s1ap::erab_item_s>    not_admitted_bearers) override;
+  void send_ho_cancel(uint16_t rnti, const asn1::s1ap::cause_c& cause) override;
   bool release_erabs(uint16_t rnti, const std::vector<uint16_t>& erabs_successfully_released) override;
   bool send_error_indication(const asn1::s1ap::cause_c& cause,
                              srsran::optional<uint32_t> enb_ue_s1ap_id = {},
                              srsran::optional<uint32_t> mme_ue_s1ap_id = {});
   bool send_ue_cap_info_indication(uint16_t rnti, srsran::unique_byte_buffer_t ue_radio_cap) override;
 
+  /// Target eNB Handover
+  /// Section 8.4.2 - Handover Resource Allocation
+  void send_ho_failure(uint32_t mme_ue_s1ap_id, const asn1::s1ap::cause_c& cause);
+  /// Section 8.4.3 - Handover Notification
+  void send_ho_notify(uint16_t rnti, uint64_t target_eci) override;
+
   // Stack interface
   bool
        handle_mme_rx_msg(srsran::unique_byte_buffer_t pdu, const sockaddr_in& from, const sctp_sndrcvinfo& sri, int flags);
@@ -162,8 +173,8 @@ private:
   bool handle_uectxtreleasecommand(const asn1::s1ap::ue_context_release_cmd_s& msg);
   bool handle_s1setupfailure(const asn1::s1ap::s1_setup_fail_s& msg);
   bool handle_erabsetuprequest(const asn1::s1ap::erab_setup_request_s& msg);
-  bool handle_erabreleasecommand(const asn1::s1ap::erab_release_cmd_s& msg);
   bool handle_erabmodifyrequest(const asn1::s1ap::erab_modify_request_s& msg);
+  bool handle_erabreleasecommand(const asn1::s1ap::erab_release_cmd_s& msg);
   bool handle_uecontextmodifyrequest(const asn1::s1ap::ue_context_mod_request_s& msg);
 
   // handover
@@ -212,7 +223,8 @@ private:
       srsran::proc_outcome_t init(uint32_t                     target_eci_,
                                   srsran::plmn_id_t            target_plmn_,
                                   srsran::span<uint32_t>       fwd_erabs,
-                                  srsran::unique_byte_buffer_t rrc_container);
+                                  srsran::unique_byte_buffer_t rrc_container,
+                                  bool                         has_direct_fwd_path);
       srsran::proc_outcome_t step() { return srsran::proc_outcome_t::yield; }
       srsran::proc_outcome_t react(ts1_reloc_prep_expired e);
       srsran::proc_outcome_t react(const asn1::s1ap::ho_prep_fail_s& msg);
@@ -243,18 +255,18 @@ private:
                                bool                                  has_tmsi,
                                uint32_t                              m_tmsi = 0,
                                uint8_t                               mmec   = 0);
-    bool send_initial_ctxt_setup_response(const asn1::s1ap::init_context_setup_resp_s& res_);
-    bool send_initial_ctxt_setup_failure();
-    bool send_erab_setup_response(const asn1::s1ap::erab_setup_resp_s& res_);
-    bool send_erab_release_response(const std::vector<uint16_t>& erabs_successfully_released,
-                                    const std::vector<uint16_t>& erabs_failed_to_release);
-    bool send_erab_modify_response(const std::vector<uint16_t>& erabs_successfully_released,
-                                   const std::vector<uint16_t>& erabs_failed_to_release);
+    void ue_ctxt_setup_complete();
+    bool send_erab_setup_response(const erab_id_list& erabs_setup, const erab_item_list& erabs_failed);
+    bool send_erab_release_response(const erab_id_list& erabs_released, const erab_item_list& erabs_failed);
+    bool send_erab_modify_response(const erab_id_list& erabs_modified, const erab_item_list& erabs_failed);
     bool send_erab_release_indication(const std::vector<uint16_t>& erabs_successfully_released);
     bool send_ue_cap_info_indication(srsran::unique_byte_buffer_t ue_radio_cap);
 
     bool was_uectxtrelease_requested() const { return release_requested; }
 
+    void
+    set_state(s1ap_proc_id_t state, const erab_id_list& erabs_updated, const erab_item_list& erabs_failed_to_update);
+
     ue_ctxt_t ctxt      = {};
     uint16_t  stream_id = 1;
 
@@ -262,8 +274,9 @@ private:
     bool send_ho_required(uint32_t                     target_eci_,
                           srsran::plmn_id_t            target_plmn_,
                           srsran::span<uint32_t>       fwd_erabs,
-                          srsran::unique_byte_buffer_t rrc_container);
-    //! TS 36.413, Section 8.4.6 - eNB Status Transfer procedure
+                          srsran::unique_byte_buffer_t rrc_container,
+                          bool                         has_direct_fwd_path);
+    void get_erab_addr(uint16_t erab_id, transp_addr_t& transp_addr, asn1::fixed_octstring<4, true>& gtpu_teid_id);
 
     // args
     s1ap*                 s1ap_ptr;
@@ -274,6 +287,11 @@ private:
     srsran::unique_timer ts1_reloc_prep;    ///< TS1_{RELOCprep} - max time for HO preparation
     srsran::unique_timer ts1_reloc_overall; ///< TS1_{RELOCOverall}
 
+    // Procedure state
+    s1ap_proc_id_t                                                           current_state;
+    erab_id_list                                                             updated_erabs;
+    srsran::bounded_vector<asn1::s1ap::erab_item_s, ASN1_S1AP_MAXNOOF_ERABS> failed_cfg_erabs;
+
   public:
     // user procedures
     srsran::proc_t<ho_prep_proc_t> ho_prep_proc;
diff --git a/srsenb/rr.conf.example b/srsenb/rr.conf.example
index a78d5e11b..77362484e 100644
--- a/srsenb/rr.conf.example
+++ b/srsenb/rr.conf.example
@@ -61,7 +61,8 @@ cell_list =
     dl_earfcn = 3350;
     //ul_earfcn = 21400;
     ho_active = false;
-    //meas_gap_period = 0;
+    //meas_gap_period = 0; // 0 (inactive), 40 or 80
+    //allowed_meas_bw = 6;
 
     // CA cells
     scell_list = (
@@ -75,6 +76,8 @@ cell_list =
         eci = 0x19C02;
         dl_earfcn = 2850;
         pci = 2;
+        //direct_forward_path_available = false;
+        //allowed_meas_bw = 6;
       }
     );
 
diff --git a/srsenb/src/enb_cfg_parser.cc b/srsenb/src/enb_cfg_parser.cc
index a9ecdaa0f..9ded45a09 100644
--- a/srsenb/src/enb_cfg_parser.cc
+++ b/srsenb/src/enb_cfg_parser.cc
@@ -684,15 +684,14 @@ static int parse_meas_cell_list(rrc_meas_cfg_t* meas_cfg, Setting& root)
 {
   meas_cfg->meas_cells.resize(root.getLength());
   for (uint32_t i = 0; i < meas_cfg->meas_cells.size(); ++i) {
-    meas_cfg->meas_cells[i].earfcn   = root[i]["dl_earfcn"];
-    meas_cfg->meas_cells[i].pci      = (unsigned int)root[i]["pci"] % SRSRAN_NUM_PCI;
-    meas_cfg->meas_cells[i].eci      = (unsigned int)root[i]["eci"];
-    meas_cfg->meas_cells[i].q_offset = 0; // LIBLTE_RRC_Q_OFFSET_RANGE_DB_0; // TODO
-                                          //    // TODO: TEMP
-                                          //    printf("PARSER: neighbor cell: {dl_earfcn=%d pci=%d cell_idx=0x%x}\n",
-                                          //           meas_cfg->meas_cells[i].earfcn,
-                                          //           meas_cfg->meas_cells[i].pci,
-                                          //           meas_cfg->meas_cells[i].eci);
+    auto& cell    = meas_cfg->meas_cells[i];
+    cell.earfcn   = root[i]["dl_earfcn"];
+    cell.pci      = (unsigned int)root[i]["pci"] % SRSRAN_NUM_PCI;
+    cell.eci      = (unsigned int)root[i]["eci"];
+    cell.q_offset = 0; // LIBLTE_RRC_Q_OFFSET_RANGE_DB_0; // TODO
+    parse_default_field(cell.direct_forward_path_available, root[i], "direct_forward_path_available", false);
+    parse_default_field(cell.allowed_meas_bw, root[i], "allowed_meas_bw", 6u);
+    srsran_assert(srsran::is_lte_cell_nof_prb(cell.allowed_meas_bw), "Invalid measurement Bandwidth");
   }
   return 0;
 }
@@ -761,6 +760,8 @@ static int parse_cell_list(all_args_t* args, rrc_cfg_t* rrc_cfg, Setting& root)
     parse_default_field(cell_cfg.meas_cfg.meas_gap_period, cellroot, "meas_gap_period", 0u);
     HANDLEPARSERCODE(parse_default_field(cell_cfg.target_ul_sinr_db, cellroot, "target_ul_sinr", -1));
     HANDLEPARSERCODE(parse_default_field(cell_cfg.enable_phr_handling, cellroot, "enable_phr_handling", false));
+    parse_default_field(cell_cfg.meas_cfg.allowed_meas_bw, cellroot, "allowed_meas_bw", 6u);
+    srsran_assert(srsran::is_lte_cell_nof_prb(cell_cfg.meas_cfg.allowed_meas_bw), "Invalid measurement Bandwidth");
 
     if (cellroot.exists("ho_active") and cellroot["ho_active"]) {
       HANDLEPARSERCODE(parse_meas_cell_list(&cell_cfg.meas_cfg, cellroot["meas_cell_list"]));
@@ -920,7 +921,7 @@ int set_derived_args(all_args_t* args_, rrc_cfg_t* rrc_cfg_, phy_cfg_t* phy_cfg_
   // Check for a forced  DL EARFCN or frequency (only valid for a single cell config (Xico's favorite feature))
   if (rrc_cfg_->cell_list.size() == 1) {
     auto& cfg = rrc_cfg_->cell_list.at(0);
-    if (args_->enb.dl_earfcn > 0) {
+    if (args_->enb.dl_earfcn > 0 and args_->enb.dl_earfcn != cfg.dl_earfcn) {
       cfg.dl_earfcn = args_->enb.dl_earfcn;
       ERROR("Force DL EARFCN for cell PCI=%d to %d", cfg.pci, cfg.dl_earfcn);
     }
@@ -1155,6 +1156,7 @@ int set_derived_args(all_args_t* args_, rrc_cfg_t* rrc_cfg_, phy_cfg_t* phy_cfg_
 
   // Set max number of KOs
   rrc_cfg_->max_mac_dl_kos = args_->general.max_mac_dl_kos;
+  rrc_cfg_->max_mac_ul_kos = args_->general.max_mac_ul_kos;
 
   // Set sync queue capacity to 1 for ZMQ
   if (args_->rf.device_name == "zmq") {
diff --git a/srsenb/src/main.cc b/srsenb/src/main.cc
index 2e8473231..3875b75ae 100644
--- a/srsenb/src/main.cc
+++ b/srsenb/src/main.cc
@@ -223,7 +223,9 @@ void parse_args(all_args_t* args, int argc, char* argv[])
     ("expert.eea_pref_list", bpo::value<string>(&args->general.eea_pref_list)->default_value("EEA0, EEA2, EEA1"), "Ordered preference list for the selection of encryption algorithm (EEA) (default: EEA0, EEA2, EEA1).")
     ("expert.eia_pref_list", bpo::value<string>(&args->general.eia_pref_list)->default_value("EIA2, EIA1, EIA0"), "Ordered preference list for the selection of integrity algorithm (EIA) (default: EIA2, EIA1, EIA0).")
     ("expert.max_nof_ues", bpo::value<uint32_t>(&args->stack.mac.max_nof_ues)->default_value(8), "Maximum number of connected UEs")
-    ("expert.max_mac_dl_kos", bpo::value<uint32_t>(&args->general.max_mac_dl_kos)->default_value(100), "Maximum number of consecutive KOs before triggering the UE's release")
+    ("expert.max_mac_dl_kos", bpo::value<uint32_t>(&args->general.max_mac_dl_kos)->default_value(100), "Maximum number of consecutive KOs in DL before triggering the UE's release")
+    ("expert.max_mac_ul_kos", bpo::value<uint32_t>(&args->general.max_mac_ul_kos)->default_value(100), "Maximum number of consecutive KOs in UL before triggering the UE's release")
+
 
     // eMBMS section
     ("embms.enable", bpo::value<bool>(&args->stack.embms.enable)->default_value(false), "Enables MBMS in the eNB")
diff --git a/srsenb/src/phy/nr/worker_pool.cc b/srsenb/src/phy/nr/worker_pool.cc
index 6fac19960..a93f82641 100644
--- a/srsenb/src/phy/nr/worker_pool.cc
+++ b/srsenb/src/phy/nr/worker_pool.cc
@@ -30,7 +30,7 @@ bool worker_pool::init(const phy_args_t& args, phy_common* common, srslog::sink&
   // Add workers to workers pool and start threads
   srslog::basic_levels log_level = srslog::str_to_basic_level(args.log.phy_level);
   for (uint32_t i = 0; i < args.nof_phy_threads; i++) {
-    auto& log = srslog::fetch_basic_logger(fmt::format("PHY{}", i), log_sink);
+    auto& log = srslog::fetch_basic_logger(fmt::format("PHY{}-NR", i), log_sink);
     log.set_level(log_level);
     log.set_hex_dump_max_size(args.log.phy_hex_limit);
 
diff --git a/srsenb/src/stack/gnb_stack_nr.cc b/srsenb/src/stack/gnb_stack_nr.cc
index e4bc66328..beee28083 100644
--- a/srsenb/src/stack/gnb_stack_nr.cc
+++ b/srsenb/src/stack/gnb_stack_nr.cc
@@ -25,11 +25,11 @@
 
 namespace srsenb {
 
-gnb_stack_nr::gnb_stack_nr() : task_sched{512, 128}, thread("gNB"), rlc_logger(srslog::fetch_basic_logger("RLC"))
+gnb_stack_nr::gnb_stack_nr() : task_sched{512, 128}, thread("gNB"), rlc_logger(srslog::fetch_basic_logger("RLC-NR"))
 {
   m_mac.reset(new mac_nr());
-  m_rlc.reset(new rlc_nr("RLC"));
-  m_pdcp.reset(new pdcp_nr(&task_sched, "PDCP"));
+  m_rlc.reset(new rlc_nr("RLC-NR"));
+  m_pdcp.reset(new pdcp_nr(&task_sched, "PDCP-NR"));
   m_rrc.reset(new rrc_nr(task_sched.get_timer_handler()));
   m_sdap.reset(new sdap());
   m_gw.reset(new srsue::gw());
diff --git a/srsenb/src/stack/mac/mac.cc b/srsenb/src/stack/mac/mac.cc
index 387b1c5fb..f4e169f17 100644
--- a/srsenb/src/stack/mac/mac.cc
+++ b/srsenb/src/stack/mac/mac.cc
@@ -310,14 +310,8 @@ int mac::ack_info(uint32_t tti_rx, uint16_t rnti, uint32_t enb_cc_idx, uint32_t
   int nof_bytes = scheduler.dl_ack_info(tti_rx, rnti, enb_cc_idx, tb_idx, ack);
   ue_db[rnti]->metrics_tx(ack, nof_bytes);
 
-  if (ack) {
-    if (nof_bytes > 64) { // do not count RLC status messages only
-      rrc_h->set_activity_user(rnti, true);
-      logger.info("DL activity rnti=0x%x, n_bytes=%d", rnti, nof_bytes);
-    }
-  } else {
-    rrc_h->set_activity_user(rnti, false);
-  }
+  rrc_h->set_radiolink_dl_state(rnti, ack);
+
   return SRSRAN_SUCCESS;
 }
 
@@ -333,6 +327,8 @@ int mac::crc_info(uint32_t tti_rx, uint16_t rnti, uint32_t enb_cc_idx, uint32_t
   ue_db[rnti]->set_tti(tti_rx);
   ue_db[rnti]->metrics_rx(crc, nof_bytes);
 
+  rrc_h->set_radiolink_ul_state(rnti, crc);
+
   // Scheduler uses eNB's CC mapping
   return scheduler.ul_crc_info(tti_rx, rnti, enb_cc_idx, crc);
 }
diff --git a/srsenb/src/stack/mac/mac_nr.cc b/srsenb/src/stack/mac/mac_nr.cc
index c941f8936..73b905017 100644
--- a/srsenb/src/stack/mac/mac_nr.cc
+++ b/srsenb/src/stack/mac/mac_nr.cc
@@ -29,9 +29,7 @@
 
 namespace srsenb {
 
-mac_nr::mac_nr() : logger(srslog::fetch_basic_logger("MAC"))
-{
-}
+mac_nr::mac_nr() : logger(srslog::fetch_basic_logger("MAC-NR")) {}
 
 mac_nr::~mac_nr()
 {
@@ -239,7 +237,9 @@ int mac_nr::handle_pdu(srsran::unique_byte_buffer_t pdu)
   logger.info(pdu->msg, pdu->N_bytes, "Handling MAC PDU (%d B)", pdu->N_bytes);
 
   ue_rx_pdu.init_rx(true);
-  ue_rx_pdu.unpack(pdu->msg, pdu->N_bytes);
+  if (ue_rx_pdu.unpack(pdu->msg, pdu->N_bytes) != SRSRAN_SUCCESS) {
+    return SRSRAN_ERROR;
+  }
 
   for (uint32_t i = 0; i < ue_rx_pdu.get_num_subpdus(); ++i) {
     srsran::mac_sch_subpdu_nr subpdu = ue_rx_pdu.get_subpdu(i);
diff --git a/srsenb/src/stack/mac/sched_phy_ch/sf_cch_allocator.cc b/srsenb/src/stack/mac/sched_phy_ch/sf_cch_allocator.cc
index aaa26fd9e..a29fdd11c 100644
--- a/srsenb/src/stack/mac/sched_phy_ch/sf_cch_allocator.cc
+++ b/srsenb/src/stack/mac/sched_phy_ch/sf_cch_allocator.cc
@@ -37,6 +37,9 @@ void sf_cch_allocator::init(const sched_cell_params_t& cell_params_)
 {
   cc_cfg           = &cell_params_;
   pucch_cfg_common = cc_cfg->pucch_cfg_common;
+  dci_record_list.reserve(16);
+  last_dci_dfs.reserve(16);
+  temp_dci_dfs.reserve(16);
 }
 
 void sf_cch_allocator::new_tti(tti_point tti_rx_)
diff --git a/srsenb/src/stack/mac/ue.cc b/srsenb/src/stack/mac/ue.cc
index aba545bdb..9d4e301ef 100644
--- a/srsenb/src/stack/mac/ue.cc
+++ b/srsenb/src/stack/mac/ue.cc
@@ -392,9 +392,9 @@ void ue::process_pdu(uint8_t* pdu, uint32_t nof_bytes, srsran::pdu_queue::channe
       // Indicate scheduler to update BSR counters
       // sched->ul_recv_len(rnti, mac_msg_ul.get()->get_sdu_lcid(), mac_msg_ul.get()->get_payload_size());
 
-      // Indicate RRC about successful activity if valid RLC message is received
-      if (mac_msg_ul.get()->get_payload_size() > 64) { // do not count RLC status messages only
-        rrc->set_activity_user(rnti, true);
+      // Indicate DRB activity in UL to RRC
+      if (mac_msg_ul.get()->get_sdu_lcid() > 2) {
+        rrc->set_activity_user(rnti);
         logger.debug("UL activity rnti=0x%x, n_bytes=%d", rnti, nof_bytes);
       }
 
@@ -526,6 +526,13 @@ void ue::allocate_sdu(srsran::sch_pdu* pdu, uint32_t lcid, uint32_t total_sdu_le
         if (n > 0) { // new SDU could be added
           sdu_len -= n;
           logger.debug("SDU:   rnti=0x%x, lcid=%d, nbytes=%d, rem_len=%d", rnti, lcid, n, sdu_len);
+
+          // Indicate DRB activity in DL to RRC
+          if (lcid > 2) {
+            rrc->set_activity_user(rnti);
+            logger.debug("DL activity rnti=0x%x, n_bytes=%d", rnti, sdu_len);
+          }
+
         } else {
           logger.debug("Could not add SDU lcid=%d nbytes=%d, space=%d", lcid, sdu_len, sdu_space);
           pdu->del_subh();
diff --git a/srsenb/src/stack/rrc/mac_controller.cc b/srsenb/src/stack/rrc/mac_controller.cc
index b9d16d535..c8b106007 100644
--- a/srsenb/src/stack/rrc/mac_controller.cc
+++ b/srsenb/src/stack/rrc/mac_controller.cc
@@ -144,7 +144,7 @@ int mac_controller::handle_crnti_ce(uint32_t temp_crnti)
   set_drb_activation(false);
 
   // Re-activate SRBs UL (needed for ReconfComplete)
-  for (uint32_t i = rb_id_t::RB_ID_SRB1; i <= rb_id_t::RB_ID_SRB2; ++i) {
+  for (uint32_t i = srb_to_lcid(lte_srb::srb1); i <= srb_to_lcid(lte_srb::srb2); ++i) {
     current_sched_ue_cfg.ue_bearers[i] = next_sched_ue_cfg.ue_bearers[i];
   }
 
@@ -293,7 +293,7 @@ void mac_controller::handle_intraenb_ho_cmd(const asn1::rrc::rrc_conn_recfg_r8_i
   set_drb_activation(false);
 
   // Stop any SRB UL (including SRs)
-  for (uint32_t i = rb_id_t::RB_ID_SRB1; i <= rb_id_t::RB_ID_SRB2; ++i) {
+  for (uint32_t i = srb_to_lcid(lte_srb::srb1); i <= srb_to_lcid(lte_srb::srb2); ++i) {
     next_sched_ue_cfg.ue_bearers[i].direction = sched_interface::ue_bearer_cfg_t::DL;
   }
 
@@ -324,7 +324,7 @@ void mac_controller::set_scell_activation(const std::bitset<SRSRAN_MAX_CARRIERS>
 void mac_controller::set_drb_activation(bool active)
 {
   for (const drb_to_add_mod_s& drb : bearer_list.get_established_drbs()) {
-    current_sched_ue_cfg.ue_bearers[drb.drb_id + rb_id_t::RB_ID_SRB2].direction =
+    current_sched_ue_cfg.ue_bearers[drb_to_lcid((lte_drb)drb.drb_id)].direction =
         active ? sched_interface::ue_bearer_cfg_t::BOTH : sched_interface::ue_bearer_cfg_t::IDLE;
   }
 }
diff --git a/srsenb/src/stack/rrc/rrc.cc b/srsenb/src/stack/rrc/rrc.cc
index 0381945e7..83dc1c3a1 100644
--- a/srsenb/src/stack/rrc/rrc.cc
+++ b/srsenb/src/stack/rrc/rrc.cc
@@ -99,7 +99,7 @@ void rrc::stop()
 {
   if (running) {
     running   = false;
-    rrc_pdu p = {0, LCID_EXIT, nullptr};
+    rrc_pdu p = {0, LCID_EXIT, false, nullptr};
     rx_pdu_queue.push_blocking(std::move(p));
   }
   users.clear();
@@ -135,14 +135,29 @@ uint8_t* rrc::read_pdu_bcch_dlsch(const uint8_t cc_idx, const uint32_t sib_index
   return nullptr;
 }
 
-void rrc::set_activity_user(uint16_t rnti, bool ack_info)
+void rrc::set_radiolink_dl_state(uint16_t rnti, bool crc_res)
 {
-  rrc_pdu p;
-  if (ack_info) {
-    p = {rnti, LCID_ACT_USER, nullptr};
-  } else {
-    p = {rnti, LCID_MAC_KO_USER, nullptr};
+  // embed parameters in arg value
+  rrc_pdu p = {rnti, LCID_RADLINK_DL, crc_res, nullptr};
+
+  if (not rx_pdu_queue.try_push(std::move(p))) {
+    logger.error("Failed to push UE activity command to RRC queue");
+  }
+}
+
+void rrc::set_radiolink_ul_state(uint16_t rnti, bool crc_res)
+{
+  // embed parameters in arg value
+  rrc_pdu p = {rnti, LCID_RADLINK_UL, crc_res, nullptr};
+
+  if (not rx_pdu_queue.try_push(std::move(p))) {
+    logger.error("Failed to push UE activity command to RRC queue");
   }
+}
+
+void rrc::set_activity_user(uint16_t rnti)
+{
+  rrc_pdu p = {rnti, LCID_ACT_USER, false, nullptr};
 
   if (not rx_pdu_queue.try_push(std::move(p))) {
     logger.error("Failed to push UE activity command to RRC queue");
@@ -151,7 +166,7 @@ void rrc::set_activity_user(uint16_t rnti, bool ack_info)
 
 void rrc::rem_user_thread(uint16_t rnti)
 {
-  rrc_pdu p = {rnti, LCID_REM_USER, nullptr};
+  rrc_pdu p = {rnti, LCID_REM_USER, false, nullptr};
   if (not rx_pdu_queue.try_push(std::move(p))) {
     logger.error("Failed to push UE remove command to RRC queue");
   }
@@ -164,7 +179,7 @@ uint32_t rrc::get_nof_users()
 
 void rrc::max_retx_attempted(uint16_t rnti)
 {
-  rrc_pdu p = {rnti, LCID_RTX_USER, nullptr};
+  rrc_pdu p = {rnti, LCID_RTX_USER, false, nullptr};
   if (not rx_pdu_queue.try_push(std::move(p))) {
     logger.error("Failed to push max Retx event to RRC queue");
   }
@@ -254,7 +269,7 @@ void rrc::send_rrc_connection_reject(uint16_t rnti)
   char buf[32] = {};
   sprintf(buf, "SRB0 - rnti=0x%x", rnti);
   log_rrc_message(buf, Tx, pdu.get(), dl_ccch_msg, dl_ccch_msg.msg.c1().type().to_string());
-  rlc->write_sdu(rnti, RB_ID_SRB0, std::move(pdu));
+  rlc->write_sdu(rnti, srb_to_lcid(lte_srb::srb0), std::move(pdu));
 }
 
 /*******************************************************************************
@@ -262,7 +277,7 @@ void rrc::send_rrc_connection_reject(uint16_t rnti)
 *******************************************************************************/
 void rrc::write_pdu(uint16_t rnti, uint32_t lcid, srsran::unique_byte_buffer_t pdu)
 {
-  rrc_pdu p = {rnti, lcid, std::move(pdu)};
+  rrc_pdu p = {rnti, lcid, false, std::move(pdu)};
   if (not rx_pdu_queue.try_push(std::move(p))) {
     logger.error("Failed to push Release command to RRC queue");
   }
@@ -299,7 +314,7 @@ void rrc::write_dl_info(uint16_t rnti, srsran::unique_byte_buffer_t sdu)
 
 void rrc::release_ue(uint16_t rnti)
 {
-  rrc_pdu p = {rnti, LCID_REL_USER, nullptr};
+  rrc_pdu p = {rnti, LCID_REL_USER, false, nullptr};
   if (not rx_pdu_queue.try_push(std::move(p))) {
     logger.error("Failed to push Release command to RRC queue");
   }
@@ -309,7 +324,6 @@ bool rrc::setup_ue_ctxt(uint16_t rnti, const asn1::s1ap::init_context_setup_requ
 {
   logger.info("Adding initial context for 0x%x", rnti);
   auto user_it = users.find(rnti);
-
   if (user_it == users.end()) {
     logger.warning("Unrecognised rnti: 0x%x", rnti);
     return false;
@@ -332,9 +346,9 @@ bool rrc::modify_ue_ctxt(uint16_t rnti, const asn1::s1ap::ue_context_mod_request
   return user_it->second->handle_ue_ctxt_mod_req(msg);
 }
 
-bool rrc::setup_ue_erabs(uint16_t rnti, const asn1::s1ap::erab_setup_request_s& msg)
+bool rrc::release_erabs(uint32_t rnti)
 {
-  logger.info("Setting up erab(s) for 0x%x", rnti);
+  logger.info("Releasing E-RABs for 0x%x", rnti);
   auto user_it = users.find(rnti);
 
   if (user_it == users.end()) {
@@ -342,109 +356,97 @@ bool rrc::setup_ue_erabs(uint16_t rnti, const asn1::s1ap::erab_setup_request_s&
     return false;
   }
 
-  if (msg.protocol_ies.ueaggregate_maximum_bitrate_present) {
-    // UEAggregateMaximumBitrate
-    user_it->second->set_bitrates(msg.protocol_ies.ueaggregate_maximum_bitrate.value);
-  }
-
-  // Setup E-RABs
-  user_it->second->setup_erabs(msg.protocol_ies.erab_to_be_setup_list_bearer_su_req.value);
-
-  return true;
+  bool ret = user_it->second->release_erabs();
+  return ret;
 }
 
-bool rrc::release_erabs(uint32_t rnti)
+int rrc::release_erab(uint16_t rnti, uint16_t erab_id)
 {
-  logger.info("Releasing E-RABs for 0x%x", rnti);
+  logger.info("Releasing E-RAB id=%d for 0x%x", erab_id, rnti);
   auto user_it = users.find(rnti);
 
   if (user_it == users.end()) {
     logger.warning("Unrecognised rnti: 0x%x", rnti);
-    return false;
+    return SRSRAN_ERROR;
   }
 
-  bool ret = user_it->second->release_erabs();
-  return ret;
+  return user_it->second->release_erab(erab_id);
 }
 
-void rrc::release_erabs(uint32_t                              rnti,
-                        const asn1::s1ap::erab_release_cmd_s& msg,
-                        std::vector<uint16_t>*                erabs_released,
-                        std::vector<uint16_t>*                erabs_failed_to_release)
+int rrc::notify_ue_erab_updates(uint16_t rnti, srsran::const_byte_span nas_pdu)
 {
-  logger.info("Releasing E-RAB for 0x%x", rnti);
   auto user_it = users.find(rnti);
-
   if (user_it == users.end()) {
     logger.warning("Unrecognised rnti: 0x%x", rnti);
-    return;
+    return SRSRAN_ERROR;
   }
-
-  for (uint32_t i = 0; i < msg.protocol_ies.erab_to_be_released_list.value.size(); i++) {
-    const asn1::s1ap::erab_item_s& erab_to_release =
-        msg.protocol_ies.erab_to_be_released_list.value[i].value.erab_item();
-    bool ret = user_it->second->release_erab(erab_to_release.erab_id);
-    if (ret) {
-      erabs_released->push_back(erab_to_release.erab_id);
-    } else {
-      erabs_failed_to_release->push_back(erab_to_release.erab_id);
-    }
-  }
-  const asn1::unbounded_octstring<true>* nas_pdu =
-      msg.protocol_ies.nas_pdu_present ? &msg.protocol_ies.nas_pdu.value : nullptr;
   user_it->second->send_connection_reconf(nullptr, false, nas_pdu);
+  return SRSRAN_SUCCESS;
+}
 
-  return;
+bool rrc::has_erab(uint16_t rnti, uint32_t erab_id) const
+{
+  auto user_it = users.find(rnti);
+  if (user_it == users.end()) {
+    logger.warning("Unrecognised rnti: 0x%x", rnti);
+    return false;
+  }
+  return user_it->second->has_erab(erab_id);
 }
 
-void rrc::modify_erabs(uint16_t                                 rnti,
-                       const asn1::s1ap::erab_modify_request_s& msg,
-                       std::vector<uint16_t>*                   erabs_modified,
-                       std::vector<uint16_t>*                   erabs_failed_to_modify)
+int rrc::get_erab_addr_in(uint16_t rnti, uint16_t erab_id, transp_addr_t& addr_in, uint32_t& teid_in) const
 {
-  logger.info("Modifying E-RABs for 0x%x", rnti);
   auto user_it = users.find(rnti);
+  if (user_it == users.end()) {
+    logger.warning("Unrecognised rnti: 0x%x", rnti);
+    return SRSRAN_ERROR;
+  }
+  return user_it->second->get_erab_addr_in(erab_id, addr_in, teid_in);
+}
 
+void rrc::set_aggregate_max_bitrate(uint16_t rnti, const asn1::s1ap::ue_aggregate_maximum_bitrate_s& bitrate)
+{
+  auto user_it = users.find(rnti);
   if (user_it == users.end()) {
     logger.warning("Unrecognised rnti: 0x%x", rnti);
     return;
   }
+  user_it->second->set_bitrates(bitrate);
+}
 
-  // Iterate over bearers
-  for (uint32_t i = 0; i < msg.protocol_ies.erab_to_be_modified_list_bearer_mod_req.value.size(); i++) {
-    const asn1::s1ap::erab_to_be_modified_item_bearer_mod_req_s& erab_to_mod =
-        msg.protocol_ies.erab_to_be_modified_list_bearer_mod_req.value[i]
-            .value.erab_to_be_modified_item_bearer_mod_req();
-
-    uint32_t                            erab_id    = erab_to_mod.erab_id;
-    asn1::s1ap::erab_level_qos_params_s qos_params = erab_to_mod.erab_level_qos_params;
-
-    bool ret = modify_ue_erab(rnti, erab_id, qos_params, &erab_to_mod.nas_pdu);
-    if (ret) {
-      erabs_modified->push_back(erab_to_mod.erab_id);
-    } else {
-      erabs_failed_to_modify->push_back(erab_to_mod.erab_id);
-    }
+int rrc::setup_erab(uint16_t                                           rnti,
+                    uint16_t                                           erab_id,
+                    const asn1::s1ap::erab_level_qos_params_s&         qos_params,
+                    srsran::const_span<uint8_t>                        nas_pdu,
+                    const asn1::bounded_bitstring<1, 160, true, true>& addr,
+                    uint32_t                                           gtpu_teid_out,
+                    asn1::s1ap::cause_c&                               cause)
+{
+  logger.info("Setting up erab id=%d for 0x%x", erab_id, rnti);
+  auto user_it = users.find(rnti);
+  if (user_it == users.end()) {
+    logger.warning("Unrecognised rnti: 0x%x", rnti);
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::unknown_erab_id;
+    return SRSRAN_ERROR;
   }
-
-  return;
+  return user_it->second->setup_erab(erab_id, qos_params, nas_pdu, addr, gtpu_teid_out, cause);
 }
 
-bool rrc::modify_ue_erab(uint16_t                                   rnti,
-                         uint8_t                                    erab_id,
-                         const asn1::s1ap::erab_level_qos_params_s& qos_params,
-                         const asn1::unbounded_octstring<true>*     nas_pdu)
+int rrc::modify_erab(uint16_t                                   rnti,
+                     uint16_t                                   erab_id,
+                     const asn1::s1ap::erab_level_qos_params_s& qos_params,
+                     srsran::const_span<uint8_t>                nas_pdu,
+                     asn1::s1ap::cause_c&                       cause)
 {
   logger.info("Modifying E-RAB for 0x%x. E-RAB Id %d", rnti, erab_id);
   auto user_it = users.find(rnti);
-
   if (user_it == users.end()) {
     logger.warning("Unrecognised rnti: 0x%x", rnti);
-    return false;
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::unknown_erab_id;
+    return SRSRAN_ERROR;
   }
 
-  bool ret = user_it->second->modify_erab(erab_id, qos_params, nas_pdu);
-  return ret;
+  return user_it->second->modify_erab(erab_id, qos_params, nas_pdu, cause);
 }
 
 /*******************************************************************************
@@ -588,11 +590,11 @@ void rrc::read_pdu_pcch(uint8_t* payload, uint32_t buffer_size)
 *******************************************************************************/
 
 void rrc::ho_preparation_complete(uint16_t                     rnti,
-                                  bool                         is_success,
+                                  ho_prep_result               result,
                                   const asn1::s1ap::ho_cmd_s&  msg,
                                   srsran::unique_byte_buffer_t rrc_container)
 {
-  users.at(rnti)->mobility_handler->handle_ho_preparation_complete(is_success, msg, std::move(rrc_container));
+  users.at(rnti)->mobility_handler->handle_ho_preparation_complete(result, msg, std::move(rrc_container));
 }
 
 void rrc::set_erab_status(uint16_t rnti, const asn1::s1ap::bearers_subject_to_status_transfer_list_l& erabs)
@@ -657,7 +659,7 @@ void rrc::parse_ul_dcch(uint16_t rnti, uint32_t lcid, srsran::unique_byte_buffer
     if (user_it != users.end()) {
       user_it->second->parse_ul_dcch(lcid, std::move(pdu));
     } else {
-      logger.error("Processing %s: Unknown rnti=0x%x", srsenb::to_string((rb_id_t)lcid), rnti);
+      logger.error("Processing %s: Unknown rnti=0x%x", get_rb_name(lcid), rnti);
     }
   }
 }
@@ -1001,7 +1003,7 @@ void rrc::tti_clock()
   while (rx_pdu_queue.try_pop(p)) {
     // print Rx PDU
     if (p.pdu != nullptr) {
-      logger.info(p.pdu->msg, p.pdu->N_bytes, "Rx %s PDU", to_string((rb_id_t)p.lcid));
+      logger.info(p.pdu->msg, p.pdu->N_bytes, "Rx %s PDU", get_rb_name(p.lcid));
     }
 
     // check if user exists
@@ -1013,11 +1015,11 @@ void rrc::tti_clock()
 
     // handle queue cmd
     switch (p.lcid) {
-      case RB_ID_SRB0:
+      case srb_to_lcid(lte_srb::srb0):
         parse_ul_ccch(p.rnti, std::move(p.pdu));
         break;
-      case RB_ID_SRB1:
-      case RB_ID_SRB2:
+      case srb_to_lcid(lte_srb::srb1):
+      case srb_to_lcid(lte_srb::srb2):
         parse_ul_dcch(p.rnti, p.lcid, std::move(p.pdu));
         break;
       case LCID_REM_USER:
@@ -1029,8 +1031,11 @@ void rrc::tti_clock()
       case LCID_ACT_USER:
         user_it->second->set_activity();
         break;
-      case LCID_MAC_KO_USER:
-        user_it->second->mac_ko_activity();
+      case LCID_RADLINK_DL:
+        user_it->second->set_radiolink_dl_state(p.arg);
+        break;
+      case LCID_RADLINK_UL:
+        user_it->second->set_radiolink_ul_state(p.arg);
         break;
       case LCID_RTX_USER:
         user_it->second->max_retx_reached();
diff --git a/srsenb/src/stack/rrc/rrc_bearer_cfg.cc b/srsenb/src/stack/rrc/rrc_bearer_cfg.cc
index 76c44f166..959dc3809 100644
--- a/srsenb/src/stack/rrc/rrc_bearer_cfg.cc
+++ b/srsenb/src/stack/rrc/rrc_bearer_cfg.cc
@@ -156,8 +156,6 @@ bool security_cfg_handler::set_security_capabilities(const asn1::s1ap::ue_securi
   }
 
   if (not integ_algo_found || not enc_algo_found) {
-    // TODO: if no security algorithm found abort radio connection and issue
-    // encryption-and-or-integrity-protection-algorithms-not-supported message
     logger.error("Did not find a matching integrity or encryption algorithm with the UE");
     return false;
   }
@@ -217,10 +215,12 @@ int bearer_cfg_handler::add_erab(uint8_t
                                  const asn1::s1ap::erab_level_qos_params_s&         qos,
                                  const asn1::bounded_bitstring<1, 160, true, true>& addr,
                                  uint32_t                                           teid_out,
-                                 const asn1::unbounded_octstring<true>*             nas_pdu)
+                                 srsran::const_span<uint8_t>                        nas_pdu,
+                                 asn1::s1ap::cause_c&                               cause)
 {
   if (erab_id < 5) {
     logger->error("ERAB id=%d is invalid", erab_id);
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::unknown_erab_id;
     return SRSRAN_ERROR;
   }
   uint8_t lcid  = erab_id - 2; // Map e.g. E-RAB 5 to LCID 3 (==DRB1)
@@ -229,10 +229,12 @@ int bearer_cfg_handler::add_erab(uint8_t
   auto qci_it = cfg->qci_cfg.find(qos.qci);
   if (qci_it == cfg->qci_cfg.end() or not qci_it->second.configured) {
     logger->error("QCI=%d not configured", qos.qci);
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::not_supported_qci_value;
     return SRSRAN_ERROR;
   }
-  if (lcid < 3 or lcid > 10) {
-    logger->error("DRB logical channel ids must be within 3 and 10");
+  if (not srsran::is_lte_drb(lcid)) {
+    logger->error("E-RAB=%d logical channel id=%d is invalid", erab_id, lcid);
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::unknown_erab_id;
     return SRSRAN_ERROR;
   }
   const rrc_cfg_qci_t& qci_cfg = qci_it->second;
@@ -244,11 +246,37 @@ int bearer_cfg_handler::add_erab(uint8_t
 
   if (addr.length() > 32) {
     logger->error("Only addresses with length <= 32 are supported");
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::invalid_qos_combination;
+    return SRSRAN_ERROR;
+  }
+  if (qos.gbr_qos_info_present and not qci_cfg.configured) {
+    logger->warning("Provided E-RAB id=%d QoS not supported", erab_id);
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::invalid_qos_combination;
+    return SRSRAN_ERROR;
+  }
+  if (qos.gbr_qos_info_present) {
+    uint64_t req_bitrate =
+        std::max(qos.gbr_qos_info.erab_guaranteed_bitrate_dl, qos.gbr_qos_info.erab_guaranteed_bitrate_ul);
+    int16_t  pbr_kbps = qci_cfg.lc_cfg.prioritised_bit_rate.to_number();
+    uint64_t pbr      = pbr_kbps < 0 ? std::numeric_limits<uint64_t>::max() : pbr_kbps * 1000u;
+    if (req_bitrate > pbr) {
+      logger->warning("Provided E-RAB id=%d QoS not supported (guaranteed bitrates)", erab_id);
+      cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::invalid_qos_combination;
+      return SRSRAN_ERROR;
+    }
+  }
+  if (qos.alloc_retention_prio.pre_emption_cap.value == asn1::s1ap::pre_emption_cap_opts::may_trigger_pre_emption and
+      qos.alloc_retention_prio.prio_level < qci_cfg.lc_cfg.prio) {
+    logger->warning("Provided E-RAB id=%d QoS not supported (priority %d < %d)",
+                    erab_id,
+                    qos.alloc_retention_prio.prio_level,
+                    qci_cfg.lc_cfg.prio);
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::invalid_qos_combination;
     return SRSRAN_ERROR;
   }
 
-  if (nas_pdu != nullptr and nas_pdu->size() > 0) {
-    erab_info_list[erab_id].assign(nas_pdu->data(), nas_pdu->data() + nas_pdu->size());
+  if (not nas_pdu.empty()) {
+    erab_info_list[erab_id].assign(nas_pdu.begin(), nas_pdu.end());
     logger->info(
         &erab_info_list[erab_id][0], erab_info_list[erab_id].size(), "setup_erab nas_pdu -> erab_info rnti 0x%x", rnti);
   }
@@ -271,12 +299,12 @@ int bearer_cfg_handler::add_erab(uint8_t
   return SRSRAN_SUCCESS;
 }
 
-bool bearer_cfg_handler::release_erab(uint8_t erab_id)
+int bearer_cfg_handler::release_erab(uint8_t erab_id)
 {
   auto it = erabs.find(erab_id);
   if (it == erabs.end()) {
     logger->warning("The user rnti=0x%x does not contain ERAB-ID=%d", rnti, erab_id);
-    return false;
+    return SRSRAN_ERROR;
   }
 
   uint8_t drb_id = erab_id - 4;
@@ -286,7 +314,7 @@ bool bearer_cfg_handler::release_erab(uint8_t erab_id)
   erabs.erase(it);
   erab_info_list.erase(erab_id);
 
-  return true;
+  return SRSRAN_SUCCESS;
 }
 
 void bearer_cfg_handler::release_erabs()
@@ -298,24 +326,25 @@ void bearer_cfg_handler::release_erabs()
   }
 }
 
-bool bearer_cfg_handler::modify_erab(uint8_t                                    erab_id,
-                                     const asn1::s1ap::erab_level_qos_params_s& qos,
-                                     const asn1::unbounded_octstring<true>*     nas_pdu)
+int bearer_cfg_handler::modify_erab(uint8_t                                    erab_id,
+                                    const asn1::s1ap::erab_level_qos_params_s& qos,
+                                    srsran::const_span<uint8_t>                nas_pdu,
+                                    asn1::s1ap::cause_c&                       cause)
 {
   logger->info("Modifying E-RAB %d", erab_id);
   std::map<uint8_t, erab_t>::iterator erab_it = erabs.find(erab_id);
   if (erab_it == erabs.end()) {
     logger->error("Could not find E-RAB to modify");
-    return false;
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::unknown_erab_id;
+    return SRSRAN_ERROR;
   }
   auto     address  = erab_it->second.address;
   uint32_t teid_out = erab_it->second.teid_out;
   release_erab(erab_id);
-  add_erab(erab_id, qos, address, teid_out, nas_pdu);
-  return true;
+  return add_erab(erab_id, qos, address, teid_out, nas_pdu, cause);
 }
 
-void bearer_cfg_handler::add_gtpu_bearer(uint32_t erab_id)
+int bearer_cfg_handler::add_gtpu_bearer(uint32_t erab_id)
 {
   auto it = erabs.find(erab_id);
   if (it != erabs.end()) {
@@ -323,10 +352,11 @@ void bearer_cfg_handler::add_gtpu_bearer(uint32_t erab_id)
         add_gtpu_bearer(erab_id, it->second.teid_out, it->second.address.to_number(), nullptr);
     if (teidin.has_value()) {
       it->second.teid_in = teidin.value();
-      return;
+      return SRSRAN_SUCCESS;
     }
   }
   logger->error("Adding erab_id=%d to GTPU", erab_id);
+  return SRSRAN_ERROR;
 }
 
 srsran::expected<uint32_t> bearer_cfg_handler::add_gtpu_bearer(uint32_t                                erab_id,
diff --git a/srsenb/src/stack/rrc/rrc_mobility.cc b/srsenb/src/stack/rrc/rrc_mobility.cc
index a47b225f9..8109fa92e 100644
--- a/srsenb/src/stack/rrc/rrc_mobility.cc
+++ b/srsenb/src/stack/rrc/rrc_mobility.cc
@@ -147,7 +147,8 @@ std::string to_string(const cells_to_add_mod_s& obj)
  * @return rnti of created ue
  */
 uint16_t rrc::start_ho_ue_resource_alloc(const asn1::s1ap::ho_request_s&                                   msg,
-                                         const asn1::s1ap::sourceenb_to_targetenb_transparent_container_s& container)
+                                         const asn1::s1ap::sourceenb_to_targetenb_transparent_container_s& container,
+                                         asn1::s1ap::cause_c&                                              cause)
 {
   // TODO: Decision Making on whether the same QoS of the source eNB can be provided by target eNB
 
@@ -156,6 +157,7 @@ uint16_t rrc::start_ho_ue_resource_alloc(const asn1::s1ap::ho_request_s&
   const enb_cell_common* target_cell = cell_common_list->get_cell_id(rrc_details::eci_to_cellid(target_eci));
   if (target_cell == nullptr) {
     logger.error("The S1-handover target cell_id=0x%x does not exist", rrc_details::eci_to_cellid(target_eci));
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::ho_target_not_allowed;
     return SRSRAN_INVALID_RNTI;
   }
 
@@ -171,6 +173,7 @@ uint16_t rrc::start_ho_ue_resource_alloc(const asn1::s1ap::ho_request_s&
   uint16_t rnti                          = mac->reserve_new_crnti(ue_cfg);
   if (rnti == SRSRAN_INVALID_RNTI) {
     logger.error("Failed to allocate C-RNTI resources");
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::radio_res_not_available;
     return SRSRAN_INVALID_RNTI;
   }
 
@@ -186,9 +189,8 @@ uint16_t rrc::start_ho_ue_resource_alloc(const asn1::s1ap::ho_request_s&
   //    rrc_ptr->logger.error("Failed to setup e-RABs for rnti=0x%x", );
   //  }
 
-  // TODO: KeNB derivations
-  if (not ue_ptr->mobility_handler->start_s1_tenb_ho(msg, container)) {
-    rem_user_thread(rnti);
+  if (not ue_ptr->mobility_handler->start_s1_tenb_ho(msg, container, cause)) {
+    rem_user(rnti);
     return SRSRAN_INVALID_RNTI;
   }
   return rnti;
@@ -257,7 +259,8 @@ void rrc::ue::rrc_mobility::handle_ue_meas_report(const meas_report_s& msg, srsr
     auto                   meas_it  = std::find_if(meas_list_cfg.begin(), meas_list_cfg.end(), same_pci);
     const enb_cell_common* c        = rrc_enb->cell_common_list->get_pci(e.pci);
     if (meas_it != meas_list_cfg.end()) {
-      meas_ev.target_eci = meas_it->eci;
+      meas_ev.target_eci      = meas_it->eci;
+      meas_ev.direct_fwd_path = meas_it->direct_forward_path_available;
     } else if (c != nullptr) {
       meas_ev.target_eci = (rrc_enb->cfg.enb_id << 8u) + c->cell_cfg.cell_id;
     } else {
@@ -288,11 +291,6 @@ bool rrc::ue::rrc_mobility::start_ho_preparation(uint32_t target_eci,
                                                  uint8_t  measobj_id,
                                                  bool     fwd_direct_path_available)
 {
-  if (fwd_direct_path_available) {
-    Error("Direct tunnels not supported supported");
-    return false;
-  }
-
   srsran::plmn_id_t target_plmn =
       srsran::make_plmn_id_t(rrc_enb->cfg.sib1.cell_access_related_info.plmn_id_list[0].plmn_id);
   const ue_cell_ded*     src_cell_ded = rrc_ue->ue_cell_list.get_ue_cc_idx(UE_PCELL_CC_IDX);
@@ -399,7 +397,8 @@ bool rrc::ue::rrc_mobility::start_ho_preparation(uint32_t target_eci,
     fwd_erabs.push_back(erab_pair.first);
   }
 
-  return rrc_enb->s1ap->send_ho_required(rrc_ue->rnti, target_eci, target_plmn, fwd_erabs, std::move(buffer));
+  return rrc_enb->s1ap->send_ho_required(
+      rrc_ue->rnti, target_eci, target_plmn, fwd_erabs, std::move(buffer), fwd_direct_path_available);
 }
 
 /**
@@ -410,15 +409,31 @@ bool rrc::ue::rrc_mobility::start_ho_preparation(uint32_t target_eci,
  * @param is_success flag to whether an HandoverCommand or HandoverReject was received
  * @param container RRC container with HandoverCommand to send to UE
  */
-void rrc::ue::rrc_mobility::handle_ho_preparation_complete(bool                         is_success,
+void rrc::ue::rrc_mobility::handle_ho_preparation_complete(rrc::ho_prep_result          result,
                                                            const asn1::s1ap::ho_cmd_s&  msg,
                                                            srsran::unique_byte_buffer_t container)
 {
-  if (not is_success) {
+  if (result == rrc_interface_s1ap::ho_prep_result::failure) {
     logger.info("Received S1AP HandoverFailure. Aborting Handover...");
     trigger(srsran::failure_ev{});
     return;
   }
+  if (result == rrc_interface_s1ap::ho_prep_result::timeout) {
+    asn1::s1ap::cause_c cause;
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::ts1relocprep_expiry;
+    trigger(ho_cancel_ev{cause});
+    return;
+  }
+
+  // Check if any E-RAB that was not admitted. Cancel Handover, in such case.
+  if (msg.protocol_ies.erab_to_release_list_ho_cmd_present) {
+    get_logger().warning("E-RAB id=%d was not admitted in target eNB. Cancelling handover...",
+                         msg.protocol_ies.erab_to_release_list_ho_cmd.value[0].value.erab_item().erab_id);
+    asn1::s1ap::cause_c cause;
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::no_radio_res_available_in_target_cell;
+    trigger(ho_cancel_ev{cause});
+  }
+
   /* unpack RRC HOCmd struct and perform sanity checks */
   asn1::rrc::ho_cmd_s rrchocmd;
   {
@@ -426,14 +441,18 @@ void rrc::ue::rrc_mobility::handle_ho_preparation_complete(bool
     if (rrchocmd.unpack(bref) != asn1::SRSASN_SUCCESS) {
       get_logger().warning("Unpacking of RRC HOCommand was unsuccessful");
       get_logger().warning(container->msg, container->N_bytes, "Received container:");
-      trigger(ho_cancel_ev{});
+      asn1::s1ap::cause_c cause;
+      cause.set_protocol().value = asn1::s1ap::cause_protocol_opts::transfer_syntax_error;
+      trigger(ho_cancel_ev{cause});
       return;
     }
   }
   if (rrchocmd.crit_exts.type().value != c1_or_crit_ext_opts::c1 or
       rrchocmd.crit_exts.c1().type().value != ho_cmd_s::crit_exts_c_::c1_c_::types_opts::ho_cmd_r8) {
     get_logger().warning("Only handling r8 Handover Commands");
-    trigger(ho_cancel_ev{});
+    asn1::s1ap::cause_c cause;
+    cause.set_protocol().value = asn1::s1ap::cause_protocol_opts::semantic_error;
+    trigger(ho_cancel_ev{cause});
     return;
   }
 
@@ -442,10 +461,15 @@ void rrc::ue::rrc_mobility::handle_ho_preparation_complete(bool
 
 bool rrc::ue::rrc_mobility::start_s1_tenb_ho(
     const asn1::s1ap::ho_request_s&                                   msg,
-    const asn1::s1ap::sourceenb_to_targetenb_transparent_container_s& container)
+    const asn1::s1ap::sourceenb_to_targetenb_transparent_container_s& container,
+    asn1::s1ap::cause_c&                                              cause)
 {
   trigger(ho_req_rx_ev{&msg, &container});
-  return is_in_state<s1_target_ho_st>();
+  if (not is_in_state<s1_target_ho_st>()) {
+    cause = failure_cause;
+    return false;
+  }
+  return true;
 }
 
 /**
@@ -547,8 +571,10 @@ rrc::ue::rrc_mobility::s1_source_ho_st::s1_source_ho_st(rrc_mobility* parent_) :
  *         - The eNB sends eNBStatusTransfer to MME
  *         - A GTPU forwarding tunnel is opened to forward buffered PDCP PDUs and incoming GTPU PDUs
  */
-bool rrc::ue::rrc_mobility::s1_source_ho_st::start_enb_status_transfer(const asn1::s1ap::ho_cmd_s& s1ap_ho_cmd)
+asn1::s1ap::cause_c
+rrc::ue::rrc_mobility::s1_source_ho_st::start_enb_status_transfer(const asn1::s1ap::ho_cmd_s& s1ap_ho_cmd)
 {
+  asn1::s1ap::cause_c                                 cause;
   std::vector<s1ap_interface_rrc::bearer_status_info> s1ap_bearers;
   s1ap_bearers.reserve(rrc_ue->bearer_list.get_erabs().size());
 
@@ -559,7 +585,8 @@ bool rrc::ue::rrc_mobility::s1_source_ho_st::start_enb_status_transfer(const asn
     srsran::pdcp_lte_state_t pdcp_state         = {};
     if (not rrc_enb->pdcp->get_bearer_state(rrc_ue->rnti, lcid, &pdcp_state)) {
       Error("PDCP bearer lcid=%d for rnti=0x%x was not found", lcid, rrc_ue->rnti);
-      return false;
+      cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::unknown_erab_id;
+      return cause;
     }
     b.dl_hfn     = pdcp_state.tx_hfn;
     b.pdcp_dl_sn = pdcp_state.next_pdcp_tx_sn;
@@ -570,7 +597,8 @@ bool rrc::ue::rrc_mobility::s1_source_ho_st::start_enb_status_transfer(const asn
 
   Info("PDCP Bearer list sent to S1AP to initiate the eNB Status Transfer");
   if (not rrc_enb->s1ap->send_enb_status_transfer_proc(rrc_ue->rnti, s1ap_bearers)) {
-    return false;
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::unknown_erab_id;
+    return cause;
   }
 
   // Setup GTPU forwarding tunnel
@@ -597,7 +625,7 @@ bool rrc::ue::rrc_mobility::s1_source_ho_st::start_enb_status_transfer(const asn
     }
   }
 
-  return true;
+  return cause;
 }
 
 void rrc::ue::rrc_mobility::s1_source_ho_st::enter(rrc_mobility* f, const ho_meas_report_ev& ev)
@@ -606,7 +634,7 @@ void rrc::ue::rrc_mobility::s1_source_ho_st::enter(rrc_mobility* f, const ho_mea
   logger.info("Starting S1 Handover of rnti=0x%x to cellid=0x%x.", rrc_ue->rnti, ev.target_eci);
   report = ev;
 
-  if (not parent_fsm()->start_ho_preparation(report.target_eci, report.meas_obj->meas_obj_id, false)) {
+  if (not parent_fsm()->start_ho_preparation(report.target_eci, report.meas_obj->meas_obj_id, ev.direct_fwd_path)) {
     trigger(srsran::failure_ev{});
   }
 }
@@ -626,20 +654,26 @@ void rrc::ue::rrc_mobility::s1_source_ho_st::handle_ho_cmd(wait_ho_cmd& s, const
     asn1::cbit_ref bref(&ho_cmd.ho_cmd->ho_cmd_msg[0], ho_cmd.ho_cmd->ho_cmd_msg.size());
     if (dl_dcch_msg.unpack(bref) != asn1::SRSASN_SUCCESS) {
       Warning("Unpacking of RRC DL-DCCH message with HO Command was unsuccessful.");
-      trigger(ho_cancel_ev{});
+      asn1::s1ap::cause_c cause;
+      cause.set_protocol().value = asn1::s1ap::cause_protocol_opts::transfer_syntax_error;
+      trigger(ho_cancel_ev{cause});
       return;
     }
   }
   if (dl_dcch_msg.msg.type().value != dl_dcch_msg_type_c::types_opts::c1 or
       dl_dcch_msg.msg.c1().type().value != dl_dcch_msg_type_c::c1_c_::types_opts::rrc_conn_recfg) {
     Warning("HandoverCommand is expected to contain an RRC Connection Reconf message inside");
-    trigger(ho_cancel_ev{});
+    asn1::s1ap::cause_c cause;
+    cause.set_protocol().value = asn1::s1ap::cause_protocol_opts::semantic_error;
+    trigger(ho_cancel_ev{cause});
     return;
   }
   asn1::rrc::rrc_conn_recfg_s& reconf = dl_dcch_msg.msg.c1().rrc_conn_recfg();
   if (not reconf.crit_exts.c1().rrc_conn_recfg_r8().mob_ctrl_info_present) {
     Warning("HandoverCommand is expected to have mobility control subfield");
-    trigger(ho_cancel_ev{});
+    asn1::s1ap::cause_c cause;
+    cause.set_protocol().value = asn1::s1ap::cause_protocol_opts::semantic_error;
+    trigger(ho_cancel_ev{cause});
     return;
   }
 
@@ -652,20 +686,23 @@ void rrc::ue::rrc_mobility::s1_source_ho_st::handle_ho_cmd(wait_ho_cmd& s, const
 
   // Send HO Command to UE
   if (not rrc_ue->send_dl_dcch(&dl_dcch_msg)) {
-    trigger(ho_cancel_ev{});
+    asn1::s1ap::cause_c cause;
+    cause.set_protocol().value = asn1::s1ap::cause_protocol_opts::unspecified;
+    trigger(ho_cancel_ev{cause});
     return;
   }
 
   /* Start S1AP eNBStatusTransfer Procedure */
-  if (not start_enb_status_transfer(*ho_cmd.s1ap_ho_cmd)) {
-    trigger(ho_cancel_ev{});
+  asn1::s1ap::cause_c cause = start_enb_status_transfer(*ho_cmd.s1ap_ho_cmd);
+  if (cause.type().value != asn1::s1ap::cause_c::types_opts::nulltype) {
+    trigger(ho_cancel_ev{cause});
   }
 }
 
 //! Called in Source ENB during S1-Handover when there was a Reestablishment Request
 void rrc::ue::rrc_mobility::s1_source_ho_st::handle_ho_cancel(const ho_cancel_ev& ev)
 {
-  rrc_enb->s1ap->send_ho_cancel(rrc_ue->rnti);
+  rrc_enb->s1ap->send_ho_cancel(rrc_ue->rnti, ev.cause);
 }
 
 /*************************************
@@ -680,27 +717,34 @@ void rrc::ue::rrc_mobility::s1_source_ho_st::handle_ho_cancel(const ho_cancel_ev
  */
 void rrc::ue::rrc_mobility::handle_ho_requested(idle_st& s, const ho_req_rx_ev& ho_req)
 {
-  const auto& rrc_container = ho_req.transparent_container->rrc_container;
+  asn1::s1ap::cause_c                  cause; // in case of failure
+  const auto&                          rrc_container = ho_req.transparent_container->rrc_container;
+  std::vector<asn1::s1ap::erab_item_s> not_admitted_erabs;
+  auto&                                fwd_tunnels = get_state<s1_target_ho_st>()->pending_tunnels;
+  fwd_tunnels.clear();
 
   /* TS 36.331 10.2.2. - Decode HandoverPreparationInformation */
   asn1::cbit_ref            bref{rrc_container.data(), rrc_container.size()};
   asn1::rrc::ho_prep_info_s hoprep;
   if (hoprep.unpack(bref) != asn1::SRSASN_SUCCESS) {
     rrc_enb->logger.error("Failed to decode HandoverPreparationinformation in S1AP SourceENBToTargetENBContainer");
-    trigger(srsran::failure_ev{});
+    cause.set_protocol().value = asn1::s1ap::cause_protocol_opts::transfer_syntax_error;
+    trigger(ho_failure_ev{cause});
     return;
   }
   if (hoprep.crit_exts.type().value != c1_or_crit_ext_opts::c1 or
       hoprep.crit_exts.c1().type().value != ho_prep_info_s::crit_exts_c_::c1_c_::types_opts::ho_prep_info_r8) {
     rrc_enb->logger.error("Only release 8 supported");
-    trigger(srsran::failure_ev{});
+    cause.set_protocol().value = asn1::s1ap::cause_protocol_opts::semantic_error;
+    trigger(ho_failure_ev{cause});
     return;
   }
   rrc_enb->log_rrc_message("HandoverPreparation", direction_t::fromS1AP, rrc_container, hoprep, "HandoverPreparation");
 
   /* Setup UE current state in TeNB based on HandoverPreparation message */
   const ho_prep_info_r8_ies_s& hoprep_r8 = hoprep.crit_exts.c1().ho_prep_info_r8();
-  if (not apply_ho_prep_cfg(hoprep_r8, *ho_req.ho_req_msg)) {
+  if (not apply_ho_prep_cfg(hoprep_r8, *ho_req.ho_req_msg, not_admitted_erabs, cause)) {
+    trigger(ho_failure_ev{cause});
     return;
   }
 
@@ -730,13 +774,15 @@ void rrc::ue::rrc_mobility::handle_ho_requested(idle_st& s, const ho_req_rx_ev&
   srsran::unique_byte_buffer_t ho_cmd_pdu = srsran::make_byte_buffer();
   if (ho_cmd_pdu == nullptr) {
     logger.error("Couldn't allocate PDU in %s().", __FUNCTION__);
-    trigger(srsran::failure_ev{});
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::unspecified;
+    trigger(ho_failure_ev{cause});
     return;
   }
   asn1::bit_ref bref2{ho_cmd_pdu->msg, ho_cmd_pdu->get_tailroom()};
   if (dl_dcch_msg.pack(bref2) != asn1::SRSASN_SUCCESS) {
     logger.error("Failed to pack HandoverCommand");
-    trigger(srsran::failure_ev{});
+    cause.set_protocol().value = asn1::s1ap::cause_protocol_opts::transfer_syntax_error;
+    trigger(ho_failure_ev{cause});
     return;
   }
   ho_cmd_pdu->N_bytes = bref2.distance_bytes();
@@ -749,7 +795,8 @@ void rrc::ue::rrc_mobility::handle_ho_requested(idle_st& s, const ho_req_rx_ev&
   bref2 = {ho_cmd_pdu->msg, ho_cmd_pdu->get_tailroom()};
   if (ho_cmd.pack(bref2) != asn1::SRSASN_SUCCESS) {
     logger.error("Failed to pack HandoverCommand");
-    trigger(srsran::failure_ev{});
+    cause.set_protocol().value = asn1::s1ap::cause_protocol_opts::transfer_syntax_error;
+    trigger(ho_failure_ev{cause});
     return;
   }
   ho_cmd_pdu->N_bytes = bref2.distance_bytes();
@@ -765,8 +812,6 @@ void rrc::ue::rrc_mobility::handle_ho_requested(idle_st& s, const ho_req_rx_ev&
 
   // Set admitted E-RABs
   std::vector<asn1::s1ap::erab_admitted_item_s> admitted_erabs;
-  auto&                                         fwd_tunnels = get_state<s1_target_ho_st>()->pending_tunnels;
-  fwd_tunnels.clear();
   for (const auto& erab : rrc_ue->bearer_list.get_erabs()) {
     admitted_erabs.emplace_back();
     asn1::s1ap::erab_admitted_item_s& admitted_erab = admitted_erabs.back();
@@ -775,8 +820,8 @@ void rrc::ue::rrc_mobility::handle_ho_requested(idle_st& s, const ho_req_rx_ev&
 
     // Establish GTPU Forwarding Paths
     if (ho_req.transparent_container->erab_info_list_present) {
-      auto& lst = ho_req.transparent_container->erab_info_list;
-      auto  it  = std::find_if(
+      const auto& lst = ho_req.transparent_container->erab_info_list;
+      const auto* it  = std::find_if(
           lst.begin(),
           lst.end(),
           [&erab](const asn1::s1ap::protocol_ie_single_container_s<asn1::s1ap::erab_info_list_ies_o>& fwd_erab) {
@@ -785,7 +830,7 @@ void rrc::ue::rrc_mobility::handle_ho_requested(idle_st& s, const ho_req_rx_ev&
       if (it == lst.end()) {
         continue;
       }
-      auto& fwd_erab = it->value.erab_info_list_item();
+      const auto& fwd_erab = it->value.erab_info_list_item();
 
       if (fwd_erab.dl_forwarding_present and
           fwd_erab.dl_forwarding.value == asn1::s1ap::dl_forwarding_opts::dl_forwarding_proposed) {
@@ -796,9 +841,13 @@ void rrc::ue::rrc_mobility::handle_ho_requested(idle_st& s, const ho_req_rx_ev&
         srsran::expected<uint32_t> dl_teid_in = rrc_ue->bearer_list.add_gtpu_bearer(
             erab.second.id, erab.second.teid_out, erab.second.address.to_number(), &props);
         if (not dl_teid_in.has_value()) {
-          logger.error("Failed to allocate GTPU TEID");
-          trigger(srsran::failure_ev{});
-          return;
+          logger.error("Failed to allocate GTPU TEID for E-RAB id=%d", fwd_erab.erab_id);
+          not_admitted_erabs.emplace_back();
+          not_admitted_erabs.back().erab_id = erab.second.id;
+          not_admitted_erabs.back().cause.set_radio_network().value =
+              asn1::s1ap::cause_radio_network_opts::no_radio_res_available_in_target_cell;
+          admitted_erabs.pop_back();
+          continue;
         }
         fwd_tunnels.push_back(dl_teid_in.value());
         srsran::uint32_to_uint8(dl_teid_in.value(), admitted_erabs.back().dl_g_tp_teid.data());
@@ -806,52 +855,85 @@ void rrc::ue::rrc_mobility::handle_ho_requested(idle_st& s, const ho_req_rx_ev&
     }
   }
 
+  /// If the target eNB does not admit at least one non-GBR E-RAB, ..., it shall send the HANDOVER FAILURE message ...
+  if (admitted_erabs.empty()) {
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::unspecified;
+    if (not not_admitted_erabs.empty()) {
+      cause = not_admitted_erabs[0].cause;
+    }
+    trigger(ho_failure_ev{cause});
+    return;
+  }
+
   // send S1AP HandoverRequestAcknowledge
   if (not rrc_enb->s1ap->send_ho_req_ack(*ho_req.ho_req_msg,
                                          rrc_ue->rnti,
                                          rrc_ue->ue_cell_list.get_ue_cc_idx(UE_PCELL_CC_IDX)->cell_common->enb_cc_idx,
                                          std::move(ho_cmd_pdu),
-                                         admitted_erabs)) {
-    trigger(srsran::failure_ev{});
+                                         admitted_erabs,
+                                         not_admitted_erabs)) {
+    cause.set_protocol().value = asn1::s1ap::cause_protocol_opts::transfer_syntax_error;
+    trigger(ho_failure_ev{cause});
     return;
   }
 }
 
-bool rrc::ue::rrc_mobility::apply_ho_prep_cfg(const ho_prep_info_r8_ies_s&    ho_prep,
-                                              const asn1::s1ap::ho_request_s& ho_req_msg)
+void rrc::ue::rrc_mobility::handle_ho_failure(const ho_failure_ev& ev)
+{
+  // Store Handover failure cause
+  failure_cause = ev.cause;
+}
+
+bool rrc::ue::rrc_mobility::apply_ho_prep_cfg(const ho_prep_info_r8_ies_s&          ho_prep,
+                                              const asn1::s1ap::ho_request_s&       ho_req_msg,
+                                              std::vector<asn1::s1ap::erab_item_s>& erabs_failed_to_setup,
+                                              asn1::s1ap::cause_c&                  cause)
 {
   const ue_cell_ded* target_cell     = rrc_ue->ue_cell_list.get_ue_cc_idx(UE_PCELL_CC_IDX);
   const cell_cfg_t&  target_cell_cfg = target_cell->cell_common->cell_cfg;
 
   // Establish ERABs/DRBs
   for (const auto& erab_item : ho_req_msg.protocol_ies.erab_to_be_setup_list_ho_req.value) {
-    auto& erab = erab_item.value.erab_to_be_setup_item_ho_req();
+    const auto& erab = erab_item.value.erab_to_be_setup_item_ho_req();
     if (erab.ext) {
       get_logger().warning("Not handling E-RABToBeSetupList extensions");
     }
     if (erab.transport_layer_address.length() > 32) {
       get_logger().error("IPv6 addresses not currently supported");
-      trigger(srsran::failure_ev{});
-      return false;
-    }
-
-    if (not erab.ie_exts_present or not erab.ie_exts.data_forwarding_not_possible_present or
-        erab.ie_exts.data_forwarding_not_possible.ext.value !=
-            asn1::s1ap::data_forwarding_not_possible_opts::data_forwarding_not_possible) {
-      get_logger().warning("Data Forwarding of E-RABs not supported");
+      erabs_failed_to_setup.emplace_back();
+      erabs_failed_to_setup.back().erab_id                     = erab.erab_id;
+      erabs_failed_to_setup.back().cause.set_transport().value = asn1::s1ap::cause_transport_opts::unspecified;
+      continue;
     }
 
     // Create E-RAB and associated main GTPU tunnel
-    uint32_t teid_out;
+    uint32_t teid_out = 0;
     srsran::uint8_to_uint32(erab.gtp_teid.data(), &teid_out);
-    rrc_ue->bearer_list.add_erab(
-        erab.erab_id, erab.erab_level_qos_params, erab.transport_layer_address, teid_out, nullptr);
-    rrc_ue->bearer_list.add_gtpu_bearer(erab.erab_id);
+    asn1::s1ap::cause_c erab_cause;
+    if (rrc_ue->bearer_list.add_erab(
+            erab.erab_id, erab.erab_level_qos_params, erab.transport_layer_address, teid_out, {}, erab_cause) !=
+        SRSRAN_SUCCESS) {
+      erabs_failed_to_setup.emplace_back();
+      erabs_failed_to_setup.back().erab_id = erab.erab_id;
+      erabs_failed_to_setup.back().cause   = erab_cause;
+      continue;
+    }
+    if (rrc_ue->bearer_list.add_gtpu_bearer(erab.erab_id) != SRSRAN_SUCCESS) {
+      erabs_failed_to_setup.emplace_back();
+      erabs_failed_to_setup.back().erab_id = erab.erab_id;
+      erabs_failed_to_setup.back().cause.set_radio_network().value =
+          asn1::s1ap::cause_radio_network_opts::no_radio_res_available_in_target_cell;
+      continue;
+    }
   }
 
   // Regenerate AS Keys
   // See TS 33.401, Sec. 7.2.8.4.3
-  rrc_ue->ue_security_cfg.set_security_capabilities(ho_req_msg.protocol_ies.ue_security_cap.value);
+  if (not rrc_ue->ue_security_cfg.set_security_capabilities(ho_req_msg.protocol_ies.ue_security_cap.value)) {
+    cause.set_radio_network().value =
+        asn1::s1ap::cause_radio_network_opts::encryption_and_or_integrity_protection_algorithms_not_supported;
+    return false;
+  }
   rrc_ue->ue_security_cfg.set_security_key(ho_req_msg.protocol_ies.security_context.value.next_hop_param);
   rrc_ue->ue_security_cfg.set_ncc(ho_req_msg.protocol_ies.security_context.value.next_hop_chaining_count);
   rrc_ue->ue_security_cfg.regenerate_keys_handover(target_cell_cfg.pci, target_cell_cfg.dl_earfcn);
diff --git a/srsenb/src/stack/rrc/rrc_nr.cc b/srsenb/src/stack/rrc/rrc_nr.cc
index daf84172e..07e2cfa8d 100644
--- a/srsenb/src/stack/rrc/rrc_nr.cc
+++ b/srsenb/src/stack/rrc/rrc_nr.cc
@@ -28,7 +28,7 @@ using namespace asn1::rrc_nr;
 
 namespace srsenb {
 
-rrc_nr::rrc_nr(srsran::timer_handler* timers_) : logger(srslog::fetch_basic_logger("RRC")), timers(timers_) {}
+rrc_nr::rrc_nr(srsran::timer_handler* timers_) : logger(srslog::fetch_basic_logger("RRC-NR")), timers(timers_) {}
 
 int rrc_nr::init(const rrc_nr_cfg_t&     cfg_,
                  phy_interface_stack_nr* phy_,
@@ -324,12 +324,12 @@ void rrc_nr::handle_pdu(uint16_t rnti, uint32_t lcid, srsran::unique_byte_buffer
   }
 
   if (users.count(rnti) == 1) {
-    switch (lcid) {
-      case srsenb::RB_ID_SRB0:
+    switch (static_cast<srsran::rb_id_nr_t>(lcid)) {
+      case srsran::rb_id_nr_t::NR_SRB0:
         //        parse_ul_ccch(rnti, std::move(pdu));
         break;
-      case srsenb::RB_ID_SRB1:
-      case srsenb::RB_ID_SRB2:
+      case srsran::rb_id_nr_t::NR_SRB1:
+      case srsran::rb_id_nr_t::NR_SRB2:
         //        parse_ul_dcch(p.rnti, p.lcid, std::move(p.pdu));
         break;
       default:
@@ -405,7 +405,7 @@ void rrc_nr::ue::send_dl_ccch(dl_ccch_msg_s* dl_ccch_msg)
   char buf[32] = {};
   sprintf(buf, "SRB0 - rnti=0x%x", rnti);
   parent->log_rrc_message(buf, Tx, pdu.get(), *dl_ccch_msg);
-  parent->rlc->write_sdu(rnti, RB_ID_SRB0, std::move(pdu));
+  parent->rlc->write_sdu(rnti, srsran::NR_SRB0, std::move(pdu));
 }
 
 } // namespace srsenb
diff --git a/srsenb/src/stack/rrc/rrc_ue.cc b/srsenb/src/stack/rrc/rrc_ue.cc
index 152746014..85b754311 100644
--- a/srsenb/src/stack/rrc/rrc_ue.cc
+++ b/srsenb/src/stack/rrc/rrc_ue.cc
@@ -97,15 +97,51 @@ void rrc::ue::set_activity()
 {
   // re-start activity timer with current timeout value
   activity_timer.run();
-  rlf_timer.stop();
-  consecutive_kos = 0;
   if (parent) {
     parent->logger.debug("Activity registered for rnti=0x%x (timeout_value=%dms)", rnti, activity_timer.duration());
   }
 }
 
-void rrc::ue::mac_ko_activity()
+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 (crc_res) {
+    consecutive_kos_dl = 0;
+    consecutive_kos_ul = 0;
+    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_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();
+  }
+}
+
+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 (crc_res) {
+    consecutive_kos_dl = 0;
+    consecutive_kos_ul = 0;
+    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_timer.is_running()) {
@@ -113,9 +149,9 @@ void rrc::ue::mac_ko_activity()
     return;
   }
 
-  consecutive_kos++;
-  if (consecutive_kos > parent->cfg.max_mac_dl_kos) {
-    parent->logger.info("Max KOs reached, triggering release rnti=0x%x", rnti);
+  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();
   }
 }
@@ -236,8 +272,6 @@ bool rrc::ue::is_idle()
 
 void rrc::ue::parse_ul_dcch(uint32_t lcid, srsran::unique_byte_buffer_t pdu)
 {
-  set_activity();
-
   ul_dcch_msg_s  ul_dcch_msg;
   asn1::cbit_ref bref(pdu->msg, pdu->N_bytes);
   if (ul_dcch_msg.unpack(bref) != asn1::SRSASN_SUCCESS or
@@ -246,8 +280,7 @@ void rrc::ue::parse_ul_dcch(uint32_t lcid, srsran::unique_byte_buffer_t pdu)
     return;
   }
 
-  parent->log_rrc_message(
-      srsenb::to_string((rb_id_t)lcid), Rx, pdu.get(), ul_dcch_msg, ul_dcch_msg.msg.c1().type().to_string());
+  parent->log_rrc_message(get_rb_name(lcid), Rx, pdu.get(), ul_dcch_msg, ul_dcch_msg.msg.c1().type().to_string());
 
   srsran::unique_byte_buffer_t original_pdu = std::move(pdu);
   pdu                                       = srsran::make_byte_buffer();
@@ -262,10 +295,12 @@ void rrc::ue::parse_ul_dcch(uint32_t lcid, srsran::unique_byte_buffer_t pdu)
     case ul_dcch_msg_type_c::c1_c_::types::rrc_conn_setup_complete:
       save_ul_message(std::move(original_pdu));
       handle_rrc_con_setup_complete(&ul_dcch_msg.msg.c1().rrc_conn_setup_complete(), std::move(pdu));
+      set_activity();
       break;
     case ul_dcch_msg_type_c::c1_c_::types::rrc_conn_reest_complete:
       save_ul_message(std::move(original_pdu));
       handle_rrc_con_reest_complete(&ul_dcch_msg.msg.c1().rrc_conn_reest_complete(), std::move(pdu));
+      set_activity();
       break;
     case ul_dcch_msg_type_c::c1_c_::types::ul_info_transfer:
       pdu->N_bytes = ul_dcch_msg.msg.c1()
@@ -301,7 +336,7 @@ void rrc::ue::parse_ul_dcch(uint32_t lcid, srsran::unique_byte_buffer_t pdu)
       break;
     case ul_dcch_msg_type_c::c1_c_::types::ue_cap_info:
       if (handle_ue_cap_info(&ul_dcch_msg.msg.c1().ue_cap_info())) {
-        notify_s1ap_ue_ctxt_setup_complete();
+        parent->s1ap->ue_ctxt_setup_complete(rnti);
         send_connection_reconf(std::move(pdu));
         state = RRC_STATE_WAIT_FOR_CON_RECONF_COMPLETE;
       } else {
@@ -510,7 +545,9 @@ void rrc::ue::handle_rrc_con_reest_req(rrc_conn_reest_request_s* msg)
                           old_rnti);
 
       // Cancel Handover in Target eNB if on-going
-      parent->users.at(old_rnti)->mobility_handler->trigger(rrc_mobility::ho_cancel_ev{});
+      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);
@@ -621,9 +658,9 @@ void rrc::ue::handle_rrc_con_reest_complete(rrc_conn_reest_complete_s* msg, srsr
   mac_ctrl.handle_con_reest_complete();
 
   // Activate security for SRB1
-  parent->pdcp->config_security(rnti, RB_ID_SRB1, ue_security_cfg.get_as_sec_cfg());
-  parent->pdcp->enable_integrity(rnti, RB_ID_SRB1);
-  parent->pdcp->enable_encryption(rnti, RB_ID_SRB1);
+  parent->pdcp->config_security(rnti, srb_to_lcid(lte_srb::srb1), ue_security_cfg.get_as_sec_cfg());
+  parent->pdcp->enable_integrity(rnti, srb_to_lcid(lte_srb::srb1));
+  parent->pdcp->enable_encryption(rnti, srb_to_lcid(lte_srb::srb1));
 
   // Reestablish current DRBs during ConnectionReconfiguration
   bearer_list = std::move(parent->users.at(old_reest_rnti)->bearer_list);
@@ -665,9 +702,9 @@ void rrc::ue::send_connection_reest_rej(procedure_result_code cause)
 /*
  * Connection Reconfiguration
  */
-void rrc::ue::send_connection_reconf(srsran::unique_byte_buffer_t           pdu,
-                                     bool                                   phy_cfg_updated,
-                                     const asn1::unbounded_octstring<true>* nas_pdu)
+void rrc::ue::send_connection_reconf(srsran::unique_byte_buffer_t pdu,
+                                     bool                         phy_cfg_updated,
+                                     srsran::const_byte_span      nas_pdu)
 {
   parent->logger.debug("RRC state %d", state);
 
@@ -707,13 +744,13 @@ void rrc::ue::send_connection_reconf(srsran::unique_byte_buffer_t           pdu,
   mac_ctrl.handle_con_reconf(recfg_r8, ue_capabilities);
 
   // Fill in NAS PDU - Only for RRC Connection Reconfiguration during E-RAB Release Command
-  if (nas_pdu != nullptr and nas_pdu->size() > 0 and !recfg_r8.ded_info_nas_list_present) {
+  if (nas_pdu.size() > 0 and !recfg_r8.ded_info_nas_list_present) {
     recfg_r8.ded_info_nas_list_present = true;
     recfg_r8.ded_info_nas_list.resize(recfg_r8.rr_cfg_ded.drb_to_release_list.size());
     // Add NAS PDU
     for (uint32_t idx = 0; idx < recfg_r8.rr_cfg_ded.drb_to_release_list.size(); idx++) {
-      recfg_r8.ded_info_nas_list[idx].resize(nas_pdu->size());
-      memcpy(recfg_r8.ded_info_nas_list[idx].data(), nas_pdu->data(), nas_pdu->size());
+      recfg_r8.ded_info_nas_list[idx].resize(nas_pdu.size());
+      memcpy(recfg_r8.ded_info_nas_list[idx].data(), nas_pdu.data(), nas_pdu.size());
     }
   }
 
@@ -799,8 +836,8 @@ void rrc::ue::handle_ue_info_resp(const asn1::rrc::ue_info_resp_r9_s& msg, srsra
 void rrc::ue::send_security_mode_command()
 {
   // Setup SRB1 security/integrity. Encryption is set on completion
-  parent->pdcp->config_security(rnti, RB_ID_SRB1, ue_security_cfg.get_as_sec_cfg());
-  parent->pdcp->enable_integrity(rnti, RB_ID_SRB1);
+  parent->pdcp->config_security(rnti, srb_to_lcid(lte_srb::srb1), ue_security_cfg.get_as_sec_cfg());
+  parent->pdcp->enable_integrity(rnti, srb_to_lcid(lte_srb::srb1));
 
   dl_dcch_msg_s        dl_dcch_msg;
   security_mode_cmd_s* comm = &dl_dcch_msg.msg.set_c1().set_security_mode_cmd();
@@ -816,7 +853,7 @@ void rrc::ue::handle_security_mode_complete(security_mode_complete_s* msg)
 {
   parent->logger.info("SecurityModeComplete transaction ID: %d", msg->rrc_transaction_id);
 
-  parent->pdcp->enable_encryption(rnti, RB_ID_SRB1);
+  parent->pdcp->enable_encryption(rnti, srb_to_lcid(lte_srb::srb1));
 }
 
 void rrc::ue::handle_security_mode_failure(security_mode_fail_s* msg)
@@ -924,47 +961,10 @@ void rrc::ue::send_connection_release()
 }
 
 /*
- * UE context
+ * UE Init Context Setup Request
  */
 void rrc::ue::handle_ue_init_ctxt_setup_req(const asn1::s1ap::init_context_setup_request_s& msg)
 {
-  if (msg.protocol_ies.add_cs_fallback_ind_present) {
-    parent->logger.warning("Not handling AdditionalCSFallbackIndicator");
-  }
-  if (msg.protocol_ies.csg_membership_status_present) {
-    parent->logger.warning("Not handling CSGMembershipStatus");
-  }
-  if (msg.protocol_ies.gummei_id_present) {
-    parent->logger.warning("Not handling GUMMEI_ID");
-  }
-  if (msg.protocol_ies.ho_restrict_list_present) {
-    parent->logger.warning("Not handling HandoverRestrictionList");
-  }
-  if (msg.protocol_ies.management_based_mdt_allowed_present) {
-    parent->logger.warning("Not handling ManagementBasedMDTAllowed");
-  }
-  if (msg.protocol_ies.management_based_mdtplmn_list_present) {
-    parent->logger.warning("Not handling ManagementBasedMDTPLMNList");
-  }
-  if (msg.protocol_ies.mme_ue_s1ap_id_minus2_present) {
-    parent->logger.warning("Not handling MME_UE_S1AP_ID_2");
-  }
-  if (msg.protocol_ies.registered_lai_present) {
-    parent->logger.warning("Not handling RegisteredLAI");
-  }
-  if (msg.protocol_ies.srvcc_operation_possible_present) {
-    parent->logger.warning("Not handling SRVCCOperationPossible");
-  }
-  if (msg.protocol_ies.subscriber_profile_idfor_rfp_present) {
-    parent->logger.warning("Not handling SubscriberProfileIDforRFP");
-  }
-  if (msg.protocol_ies.trace_activation_present) {
-    parent->logger.warning("Not handling TraceActivation");
-  }
-  if (msg.protocol_ies.ue_radio_cap_present) {
-    parent->logger.warning("Not handling UERadioCapability");
-  }
-
   set_bitrates(msg.protocol_ies.ueaggregate_maximum_bitrate.value);
   ue_security_cfg.set_security_capabilities(msg.protocol_ies.ue_security_cap.value);
   ue_security_cfg.set_security_key(msg.protocol_ies.security_key.value);
@@ -979,9 +979,6 @@ void rrc::ue::handle_ue_init_ctxt_setup_req(const asn1::s1ap::init_context_setup
 
   // Send RRC security mode command
   send_security_mode_command();
-
-  // Setup E-RABs
-  setup_erabs(msg.protocol_ies.erab_to_be_setup_list_ctxt_su_req.value);
 }
 
 bool rrc::ue::handle_ue_ctxt_mod_req(const asn1::s1ap::ue_context_mod_request_s& msg)
@@ -994,19 +991,6 @@ bool rrc::ue::handle_ue_ctxt_mod_req(const asn1::s1ap::ue_context_mod_request_s&
     }
   }
 
-  if (msg.protocol_ies.add_cs_fallback_ind_present) {
-    parent->logger.warning("Not handling AdditionalCSFallbackIndicator");
-  }
-  if (msg.protocol_ies.csg_membership_status_present) {
-    parent->logger.warning("Not handling CSGMembershipStatus");
-  }
-  if (msg.protocol_ies.registered_lai_present) {
-    parent->logger.warning("Not handling RegisteredLAI");
-  }
-  if (msg.protocol_ies.subscriber_profile_idfor_rfp_present) {
-    parent->logger.warning("Not handling SubscriberProfileIDforRFP");
-  }
-
   // UEAggregateMaximumBitrate
   if (msg.protocol_ies.ueaggregate_maximum_bitrate_present) {
     set_bitrates(msg.protocol_ies.ueaggregate_maximum_bitrate.value);
@@ -1025,23 +1009,6 @@ bool rrc::ue::handle_ue_ctxt_mod_req(const asn1::s1ap::ue_context_mod_request_s&
   return true;
 }
 
-void rrc::ue::notify_s1ap_ue_ctxt_setup_complete()
-{
-  asn1::s1ap::init_context_setup_resp_s res;
-
-  res.protocol_ies.erab_setup_list_ctxt_su_res.value.resize(bearer_list.get_erabs().size());
-  uint32_t i = 0;
-  for (const auto& erab : bearer_list.get_erabs()) {
-    res.protocol_ies.erab_setup_list_ctxt_su_res.value[i].load_info_obj(ASN1_S1AP_ID_ERAB_SETUP_ITEM_CTXT_SU_RES);
-    auto& item   = res.protocol_ies.erab_setup_list_ctxt_su_res.value[i].value.erab_setup_item_ctxt_su_res();
-    item.erab_id = erab.second.id;
-    srsran::uint32_to_uint8(erab.second.teid_in, item.gtp_teid.data());
-    i++;
-  }
-
-  parent->s1ap->ue_ctxt_setup_complete(rnti, res);
-}
-
 void rrc::ue::set_bitrates(const asn1::s1ap::ue_aggregate_maximum_bitrate_s& rates)
 {
   bitrates = rates;
@@ -1050,7 +1017,7 @@ void rrc::ue::set_bitrates(const asn1::s1ap::ue_aggregate_maximum_bitrate_s& rat
 bool rrc::ue::setup_erabs(const asn1::s1ap::erab_to_be_setup_list_ctxt_su_req_l& e)
 {
   for (const auto& item : e) {
-    auto& erab = item.value.erab_to_be_setup_item_ctxt_su_req();
+    const auto& erab = item.value.erab_to_be_setup_item_ctxt_su_req();
     if (erab.ext) {
       parent->logger.warning("Not handling E-RABToBeSetupListCtxtSURequest extensions");
     }
@@ -1062,40 +1029,17 @@ bool rrc::ue::setup_erabs(const asn1::s1ap::erab_to_be_setup_list_ctxt_su_req_l&
       return false;
     }
 
-    uint32_t teid_out;
+    uint32_t teid_out = 0;
     srsran::uint8_to_uint32(erab.gtp_teid.data(), &teid_out);
-    const asn1::unbounded_octstring<true>* nas_pdu = erab.nas_pdu_present ? &erab.nas_pdu : nullptr;
-    bearer_list.add_erab(erab.erab_id, erab.erab_level_qos_params, erab.transport_layer_address, teid_out, nas_pdu);
-    bearer_list.add_gtpu_bearer(erab.erab_id);
-  }
-  return true;
-}
-
-bool rrc::ue::setup_erabs(const asn1::s1ap::erab_to_be_setup_list_bearer_su_req_l& e)
-{
-  for (const auto& item : e) {
-    auto& erab = item.value.erab_to_be_setup_item_bearer_su_req();
-    if (erab.ext) {
-      parent->logger.warning("Not handling E-RABToBeSetupListBearerSUReq extensions");
-    }
-    if (erab.ie_exts_present) {
-      parent->logger.warning("Not handling E-RABToBeSetupListBearerSUReq extensions");
-    }
-    if (erab.transport_layer_address.length() > 32) {
-      parent->logger.error("IPv6 addresses not currently supported");
-      return false;
+    srsran::const_span<uint8_t> nas_pdu;
+    if (erab.nas_pdu_present) {
+      nas_pdu = erab.nas_pdu;
     }
-
-    uint32_t teid_out;
-    srsran::uint8_to_uint32(erab.gtp_teid.data(), &teid_out);
+    asn1::s1ap::cause_c cause;
     bearer_list.add_erab(
-        erab.erab_id, erab.erab_level_qos_params, erab.transport_layer_address, teid_out, &erab.nas_pdu);
+        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);
   }
-
-  // Work in progress
-  notify_s1ap_ue_erab_setup_response(e);
-  send_connection_reconf(nullptr, false);
   return true;
 }
 
@@ -1105,49 +1049,50 @@ bool rrc::ue::release_erabs()
   return true;
 }
 
-bool rrc::ue::release_erab(uint32_t erab_id)
+int rrc::ue::release_erab(uint32_t erab_id)
 {
   return bearer_list.release_erab(erab_id);
 }
 
-bool rrc::ue::modify_erab(uint16_t                                   erab_id,
-                          const asn1::s1ap::erab_level_qos_params_s& qos_params,
-                          const asn1::unbounded_octstring<true>*     nas_pdu)
+int rrc::ue::get_erab_addr_in(uint16_t erab_id, transp_addr_t& addr_in, uint32_t& teid_in) const
 {
-  bool ret = bearer_list.modify_erab(erab_id, qos_params, nas_pdu);
-  if (ret) {
-    send_connection_reconf(nullptr, false);
+  auto it = bearer_list.get_erabs().find(erab_id);
+  if (it == bearer_list.get_erabs().end()) {
+    parent->logger.error("E-RAB id=%d for rnti=0x%x not found", erab_id, rnti);
+    return SRSRAN_ERROR;
   }
-  return ret;
+  addr_in = it->second.address;
+  teid_in = it->second.teid_in;
+  return SRSRAN_SUCCESS;
 }
 
-void rrc::ue::notify_s1ap_ue_erab_setup_response(const asn1::s1ap::erab_to_be_setup_list_bearer_su_req_l& e)
+int rrc::ue::setup_erab(uint16_t                                           erab_id,
+                        const asn1::s1ap::erab_level_qos_params_s&         qos_params,
+                        srsran::const_span<uint8_t>                        nas_pdu,
+                        const asn1::bounded_bitstring<1, 160, true, true>& addr,
+                        uint32_t                                           gtpu_teid_out,
+                        asn1::s1ap::cause_c&                               cause)
 {
-  asn1::s1ap::erab_setup_resp_s res;
-
-  const auto& erabs = bearer_list.get_erabs();
-  for (const auto& erab : e) {
-    uint8_t id = erab.value.erab_to_be_setup_item_bearer_su_req().erab_id;
-    if (erabs.count(id)) {
-      res.protocol_ies.erab_setup_list_bearer_su_res_present = true;
-      res.protocol_ies.erab_setup_list_bearer_su_res.value.push_back({});
-      auto& item = res.protocol_ies.erab_setup_list_bearer_su_res.value.back();
-      item.load_info_obj(ASN1_S1AP_ID_ERAB_SETUP_ITEM_BEARER_SU_RES);
-      item.value.erab_setup_item_bearer_su_res().erab_id = id;
-      srsran::uint32_to_uint8(bearer_list.get_erabs().at(id).teid_in,
-                              &item.value.erab_setup_item_bearer_su_res().gtp_teid[0]);
-    } else {
-      res.protocol_ies.erab_failed_to_setup_list_bearer_su_res_present = true;
-      res.protocol_ies.erab_failed_to_setup_list_bearer_su_res.value.push_back({});
-      auto& item = res.protocol_ies.erab_failed_to_setup_list_bearer_su_res.value.back();
-      item.load_info_obj(ASN1_S1AP_ID_ERAB_ITEM);
-      item.value.erab_item().erab_id = id;
-      item.value.erab_item().cause.set_radio_network().value =
-          asn1::s1ap::cause_radio_network_opts::invalid_qos_combination;
-    }
+  if (bearer_list.get_erabs().count(erab_id) > 0) {
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::multiple_erab_id_instances;
+    return SRSRAN_ERROR;
   }
+  if (bearer_list.add_erab(erab_id, qos_params, addr, gtpu_teid_out, nas_pdu, cause) != SRSRAN_SUCCESS) {
+    return SRSRAN_ERROR;
+  }
+  if (bearer_list.add_gtpu_bearer(erab_id) != SRSRAN_SUCCESS) {
+    cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::radio_res_not_available;
+    return SRSRAN_ERROR;
+  }
+  return SRSRAN_SUCCESS;
+}
 
-  parent->s1ap->ue_erab_setup_complete(rnti, res);
+int rrc::ue::modify_erab(uint16_t                                   erab_id,
+                         const asn1::s1ap::erab_level_qos_params_s& qos_params,
+                         srsran::const_span<uint8_t>                nas_pdu,
+                         asn1::s1ap::cause_c&                       cause)
+{
+  return bearer_list.modify_erab(erab_id, qos_params, nas_pdu, cause);
 }
 
 //! Helper method to access Cell configuration based on UE Carrier Index
@@ -1210,7 +1155,7 @@ void rrc::ue::send_dl_ccch(dl_ccch_msg_s* dl_ccch_msg, std::string* octet_str)
       *octet_str = asn1::octstring_to_string(pdu->msg, pdu->N_bytes);
     }
 
-    parent->rlc->write_sdu(rnti, RB_ID_SRB0, std::move(pdu));
+    parent->rlc->write_sdu(rnti, srb_to_lcid(lte_srb::srb0), std::move(pdu));
   } else {
     parent->logger.error("Allocating pdu");
   }
@@ -1229,14 +1174,15 @@ bool rrc::ue::send_dl_dcch(const dl_dcch_msg_s* dl_dcch_msg, srsran::unique_byte
     }
     pdu->N_bytes = (uint32_t)bref.distance_bytes();
 
-    uint32_t lcid = RB_ID_SRB1;
+    lte_srb rb = lte_srb::srb1;
     if (dl_dcch_msg->msg.c1().type() == dl_dcch_msg_type_c::c1_c_::types_opts::dl_info_transfer) {
       // send messages with NAS on SRB2 if user is fully registered (after RRC reconfig complete)
-      lcid = parent->rlc->has_bearer(rnti, RB_ID_SRB2) && state == RRC_STATE_REGISTERED ? RB_ID_SRB2 : RB_ID_SRB1;
+      rb = (parent->rlc->has_bearer(rnti, srb_to_lcid(lte_srb::srb2)) && state == RRC_STATE_REGISTERED) ? lte_srb::srb2
+                                                                                                        : lte_srb::srb1;
     }
 
     char buf[32] = {};
-    sprintf(buf, "SRB%d - rnti=0x%x", lcid, rnti);
+    sprintf(buf, "%s - rnti=0x%x", srsran::get_srb_name(rb), rnti);
     parent->log_rrc_message(buf, Tx, pdu.get(), *dl_dcch_msg, dl_dcch_msg->msg.c1().type().to_string());
 
     // Encode the pdu as an octet string if the user passed a valid pointer.
@@ -1244,7 +1190,7 @@ bool rrc::ue::send_dl_dcch(const dl_dcch_msg_s* dl_dcch_msg, srsran::unique_byte
       *octet_str = asn1::octstring_to_string(pdu->msg, pdu->N_bytes);
     }
 
-    parent->pdcp->write_sdu(rnti, lcid, std::move(pdu));
+    parent->pdcp->write_sdu(rnti, srb_to_lcid(rb), std::move(pdu));
   } else {
     parent->logger.error("Allocating pdu");
     return false;
diff --git a/srsenb/src/stack/rrc/ue_meas_cfg.cc b/srsenb/src/stack/rrc/ue_meas_cfg.cc
index 5e00cab55..6a5a00fb8 100644
--- a/srsenb/src/stack/rrc/ue_meas_cfg.cc
+++ b/srsenb/src/stack/rrc/ue_meas_cfg.cc
@@ -68,7 +68,7 @@ std::pair<meas_obj_t*, cells_to_add_mod_s*> find_cell(meas_obj_to_add_mod_list_l
 }
 
 /// Add EARFCN to the MeasObjToAddModList
-std::pair<bool, meas_obj_t*> add_meas_obj(meas_obj_list& list, uint32_t dl_earfcn)
+std::pair<bool, meas_obj_t*> add_meas_obj(meas_obj_list& list, uint32_t dl_earfcn, uint32_t allowed_meas_bw)
 {
   meas_obj_t* obj = find_meas_obj(list, dl_earfcn);
   if (obj != nullptr) {
@@ -79,9 +79,9 @@ std::pair<bool, meas_obj_t*> add_meas_obj(meas_obj_list& list, uint32_t dl_earfc
   new_obj.meas_obj_id                = srsran::find_rrc_obj_id_gap(list);
   asn1::rrc::meas_obj_eutra_s& eutra = new_obj.meas_obj.set_meas_obj_eutra();
   eutra.carrier_freq                 = dl_earfcn;
-  eutra.allowed_meas_bw.value        = asn1::rrc::allowed_meas_bw_e::mbw6; // TODO: What value to add here?
-  eutra.neigh_cell_cfg.from_number(1);                                     // No MBSFN subframes present in neighbors
-  eutra.offset_freq_present = false;                                       // no offset
+  asn1::number_to_enum(eutra.allowed_meas_bw, allowed_meas_bw);
+  eutra.neigh_cell_cfg.from_number(1); // No MBSFN subframes present in neighbors
+  eutra.offset_freq_present = false;   // no offset
   obj                       = srsran::add_rrc_obj(list, new_obj);
   return {true, obj};
 }
@@ -118,7 +118,7 @@ std::tuple<bool, meas_obj_t*, cells_to_add_mod_s*> add_cell_enb_cfg(meas_obj_lis
     }
   } else {
     // no measobj has been found with same earfcn, create a new one
-    auto ret2 = add_meas_obj(meas_obj_list, cellcfg.earfcn);
+    auto ret2 = add_meas_obj(meas_obj_list, cellcfg.earfcn, cellcfg.allowed_meas_bw);
     ret.first = ret2.second;
 
     new_cell.cell_idx                   = 1;
@@ -339,7 +339,8 @@ bool fill_meascfg_enb_cfg(meas_cfg_s& meascfg, const ue_cell_ded_list& ue_cell_l
     return cc1->get_dl_earfcn() < cc2->get_dl_earfcn();
   });
   for (auto* cc : sorted_ue_cells) {
-    add_meas_obj(meascfg.meas_obj_to_add_mod_list, cc->get_dl_earfcn());
+    add_meas_obj(
+        meascfg.meas_obj_to_add_mod_list, cc->get_dl_earfcn(), cc->cell_common->cell_cfg.meas_cfg.allowed_meas_bw);
   }
 
   // Inserts all cells in meas_cell_list that are not PCell or SCells
diff --git a/srsenb/src/stack/upper/gtpu.cc b/srsenb/src/stack/upper/gtpu.cc
index e67d98874..750c9113e 100644
--- a/srsenb/src/stack/upper/gtpu.cc
+++ b/srsenb/src/stack/upper/gtpu.cc
@@ -66,7 +66,7 @@ gtpu_tunnel_manager::ue_lcid_tunnel_list* gtpu_tunnel_manager::find_rnti_tunnels
 
 srsran::span<gtpu_tunnel_manager::lcid_tunnel> gtpu_tunnel_manager::find_rnti_lcid_tunnels(uint16_t rnti, uint32_t lcid)
 {
-  if (lcid < SRSENB_N_SRB or lcid >= SRSENB_N_RADIO_BEARERS) {
+  if (not is_lte_rb(lcid)) {
     logger.warning("Searching for bearer with invalid lcid=%d", lcid);
     return {};
   }
@@ -82,7 +82,7 @@ srsran::span<gtpu_tunnel_manager::lcid_tunnel> gtpu_tunnel_manager::find_rnti_lc
 
 const gtpu_tunnel* gtpu_tunnel_manager::add_tunnel(uint16_t rnti, uint32_t lcid, uint32_t teidout, uint32_t spgw_addr)
 {
-  if (lcid < SRSENB_N_SRB or lcid >= SRSENB_N_RADIO_BEARERS) {
+  if (not is_lte_rb(lcid)) {
     logger.warning("Adding TEID with invalid lcid=%d", lcid);
     return nullptr;
   }
diff --git a/srsenb/src/stack/upper/pdcp.cc b/srsenb/src/stack/upper/pdcp.cc
index 4e14f72c5..db55cd9e6 100644
--- a/srsenb/src/stack/upper/pdcp.cc
+++ b/srsenb/src/stack/upper/pdcp.cc
@@ -244,9 +244,9 @@ void pdcp::user_interface_rrc::write_pdu_pcch(srsran::unique_byte_buffer_t pdu)
   ERROR("Error: Received PCCH from ue=%d", rnti);
 }
 
-std::string pdcp::user_interface_rrc::get_rb_name(uint32_t lcid)
+const char* pdcp::user_interface_rrc::get_rb_name(uint32_t lcid)
 {
-  return to_string((rb_id_t)lcid);
+  return srsenb::get_rb_name(lcid);
 }
 
 void pdcp::get_metrics(pdcp_metrics_t& m, const uint32_t nof_tti)
diff --git a/srsenb/src/stack/upper/pdcp_nr.cc b/srsenb/src/stack/upper/pdcp_nr.cc
index bb65cdf18..e47ea8fb5 100644
--- a/srsenb/src/stack/upper/pdcp_nr.cc
+++ b/srsenb/src/stack/upper/pdcp_nr.cc
@@ -21,6 +21,7 @@
 
 #include "srsenb/hdr/stack/upper/pdcp_nr.h"
 #include "lib/include/srsran/interfaces/nr_common_interface_types.h"
+#include "srsenb/hdr/common/common_enb.h"
 
 namespace srsenb {
 
@@ -181,9 +182,9 @@ void pdcp_nr::user_interface_rrc::write_pdu_pcch(srsran::unique_byte_buffer_t pd
   ERROR("Error: Received PCCH from ue=%d", rnti);
 }
 
-std::string pdcp_nr::user_interface_rrc::get_rb_name(uint32_t lcid)
+const char* pdcp_nr::user_interface_rrc::get_rb_name(uint32_t lcid)
 {
-  return srsran::to_string(static_cast<srsran::rb_id_nr_t>(lcid));
+  return srsenb::get_rb_name(lcid);
 }
 
 } // namespace srsenb
diff --git a/srsenb/src/stack/upper/rlc.cc b/srsenb/src/stack/upper/rlc.cc
index a1e72acf0..6e63f6c7b 100644
--- a/srsenb/src/stack/upper/rlc.cc
+++ b/srsenb/src/stack/upper/rlc.cc
@@ -69,7 +69,7 @@ void rlc::add_user(uint16_t rnti)
     obj->init(&users[rnti],
               &users[rnti],
               timers,
-              RB_ID_SRB0,
+              srb_to_lcid(lte_srb::srb0),
               [rnti, this](uint32_t lcid, uint32_t tx_queue, uint32_t retx_queue) {
                 update_bsr(rnti, lcid, tx_queue, retx_queue);
               });
@@ -271,7 +271,7 @@ void rlc::user_interface::max_retx_attempted()
 
 void rlc::user_interface::write_pdu(uint32_t lcid, srsran::unique_byte_buffer_t sdu)
 {
-  if (lcid == RB_ID_SRB0) {
+  if (lcid == srb_to_lcid(lte_srb::srb0)) {
     rrc->write_pdu(rnti, lcid, std::move(sdu));
   } else {
     pdcp->write_pdu(rnti, lcid, std::move(sdu));
@@ -303,9 +303,9 @@ void rlc::user_interface::write_pdu_pcch(srsran::unique_byte_buffer_t sdu)
   ERROR("Error: Received PCCH from ue=%d", rnti);
 }
 
-std::string rlc::user_interface::get_rb_name(uint32_t lcid)
+const char* rlc::user_interface::get_rb_name(uint32_t lcid)
 {
-  return to_string((rb_id_t)lcid);
+  return srsenb::get_rb_name(lcid);
 }
 
 } // namespace srsenb
diff --git a/srsenb/src/stack/upper/rlc_nr.cc b/srsenb/src/stack/upper/rlc_nr.cc
index 7169364ec..791a53e3d 100644
--- a/srsenb/src/stack/upper/rlc_nr.cc
+++ b/srsenb/src/stack/upper/rlc_nr.cc
@@ -211,7 +211,7 @@ void rlc_nr::user_interface::write_pdu_pcch(srsran::unique_byte_buffer_t sdu)
   ERROR("Error: Received PCCH from ue=%d", rnti);
 }
 
-std::string rlc_nr::user_interface::get_rb_name(uint32_t lcid)
+const char* rlc_nr::user_interface::get_rb_name(uint32_t lcid)
 {
   return srsran::to_string(static_cast<srsran::rb_id_nr_t>(lcid));
 }
diff --git a/srsenb/src/stack/upper/s1ap.cc b/srsenb/src/stack/upper/s1ap.cc
index fe2469cdc..4d9dc42f5 100644
--- a/srsenb/src/stack/upper/s1ap.cc
+++ b/srsenb/src/stack/upper/s1ap.cc
@@ -20,7 +20,6 @@
  */
 
 #include "srsenb/hdr/stack/upper/s1ap.h"
-#include "srsenb/hdr/common/common_enb.h"
 #include "srsran/adt/scope_exit.h"
 #include "srsran/common/bcd_helpers.h"
 #include "srsran/common/enb_events.h"
@@ -43,10 +42,21 @@ using srsran::uint32_to_uint8;
 #define procWarning(fmt, ...) s1ap_ptr->logger.warning("Proc \"%s\" - " fmt, name(), ##__VA_ARGS__)
 #define procInfo(fmt, ...) s1ap_ptr->logger.info("Proc \"%s\" - " fmt, name(), ##__VA_ARGS__)
 
+#define WarnUnsupportFeature(cond, featurename)                                                                        \
+  do {                                                                                                                 \
+    if (cond) {                                                                                                        \
+      logger.warning("Not handling feature - %s", featurename);                                                        \
+    }                                                                                                                  \
+  } while (0)
+
 using namespace asn1::s1ap;
 
 namespace srsenb {
 
+/*************************
+ *    Helper Functions
+ ************************/
+
 asn1::bounded_bitstring<1, 160, true, true> addr_to_asn1(const char* addr_str)
 {
   asn1::bounded_bitstring<1, 160, true, true> transport_layer_addr(32);
@@ -58,6 +68,56 @@ asn1::bounded_bitstring<1, 160, true, true> addr_to_asn1(const char* addr_str)
   return transport_layer_addr;
 }
 
+/// Helper to add ERAB items that are duplicates in the received S1AP message
+template <typename List>
+void add_repeated_erab_ids(const List&                                                   list,
+                           srsran::bounded_vector<erab_item_s, ASN1_S1AP_MAXNOOF_ERABS>& failed_cfg_erabs)
+{
+  for (auto it = list.begin(); it != list.end(); ++it) {
+    for (auto it2 = it + 1; it2 != list.end(); ++it2) {
+      if (equal_obj_id(*it, *it2)) {
+        failed_cfg_erabs.push_back(erab_item_s());
+        failed_cfg_erabs.back().erab_id                         = get_obj_id(*it);
+        failed_cfg_erabs.back().cause.set_radio_network().value = cause_radio_network_opts::multiple_erab_id_instances;
+      }
+    }
+  }
+
+  // Sort and remove duplications
+  std::sort(failed_cfg_erabs.begin(), failed_cfg_erabs.end(), &lower_obj_id<erab_item_s>);
+  failed_cfg_erabs.erase(std::unique(failed_cfg_erabs.begin(), failed_cfg_erabs.end(), &equal_obj_id<erab_item_s>),
+                         failed_cfg_erabs.end());
+}
+
+bool contains_erab_id(srsran::bounded_vector<erab_item_s, ASN1_S1AP_MAXNOOF_ERABS>& failed_cfg_erabs, uint16_t erab_id)
+{
+  erab_item_s dummy;
+  dummy.erab_id = erab_id;
+  return std::find_if(failed_cfg_erabs.begin(), failed_cfg_erabs.end(), [erab_id](const erab_item_s& e) {
+           return e.erab_id == erab_id;
+         }) != failed_cfg_erabs.end();
+}
+
+void sanitize_response_erab_lists(s1ap::erab_item_list& failed_cfg_erabs, s1ap::erab_id_list& erabs)
+{
+  // Sort and remove duplicates
+  std::sort(failed_cfg_erabs.begin(), failed_cfg_erabs.end(), &lower_obj_id<erab_item_s>);
+  failed_cfg_erabs.erase(std::unique(failed_cfg_erabs.begin(), failed_cfg_erabs.end(), &equal_obj_id<erab_item_s>),
+                         failed_cfg_erabs.end());
+  std::sort(erabs.begin(), erabs.end());
+  erabs.erase(std::unique(erabs.begin(), erabs.end()), erabs.end());
+}
+
+template <typename OutList>
+void fill_erab_failed_setup_list(OutList& output_list, const s1ap::erab_item_list& input_list)
+{
+  output_list.resize(input_list.size());
+  for (size_t i = 0; i < input_list.size(); ++i) {
+    output_list[i].load_info_obj(ASN1_S1AP_ID_ERAB_ITEM);
+    output_list[i].value.erab_item() = input_list[i];
+  }
+}
+
 /*********************************************************
  * TS 36.413 - Section 8.4.1 - "Handover Preparation"
  *********************************************************/
@@ -66,14 +126,16 @@ s1ap::ue::ho_prep_proc_t::ho_prep_proc_t(s1ap::ue* ue_) : ue_ptr(ue_), s1ap_ptr(
 srsran::proc_outcome_t s1ap::ue::ho_prep_proc_t::init(uint32_t                     target_eci_,
                                                       srsran::plmn_id_t            target_plmn_,
                                                       srsran::span<uint32_t>       fwd_erabs,
-                                                      srsran::unique_byte_buffer_t rrc_container_)
+                                                      srsran::unique_byte_buffer_t rrc_container_,
+                                                      bool                         has_direct_fwd_path)
 {
   ho_cmd_msg  = nullptr;
   target_eci  = target_eci_;
   target_plmn = target_plmn_;
 
   procInfo("Sending HandoverRequired to MME id=%d", ue_ptr->ctxt.mme_ue_s1ap_id.value());
-  if (not ue_ptr->send_ho_required(target_eci, target_plmn, fwd_erabs, std::move(rrc_container_))) {
+  if (not ue_ptr->send_ho_required(
+          target_eci, target_plmn, fwd_erabs, std::move(rrc_container_), has_direct_fwd_path)) {
     procError("Failed to send HORequired to cell 0x%x", target_eci);
     return srsran::proc_outcome_t::error;
   }
@@ -117,12 +179,6 @@ srsran::proc_outcome_t s1ap::ue::ho_prep_proc_t::react(const asn1::s1ap::ho_cmd_
     procWarning("Not handling HandoverCommand extensions and non-intraLTE params");
   }
 
-  // Check for E-RABs that could not be admitted in the target
-  if (msg.protocol_ies.erab_to_release_list_ho_cmd_present) {
-    procWarning("Not handling E-RABtoReleaseList");
-    // TODO
-  }
-
   // In case of intra-system Handover, Target to Source Transparent Container IE shall be encoded as
   // Target eNB to Source eNB Transparent Container IE
   asn1::cbit_ref bref(msg.protocol_ies.target_to_source_transparent_container.value.data(),
@@ -152,9 +208,13 @@ srsran::proc_outcome_t s1ap::ue::ho_prep_proc_t::react(const asn1::s1ap::ho_cmd_
 void s1ap::ue::ho_prep_proc_t::then(const srsran::proc_state_t& result)
 {
   if (result.is_error()) {
-    s1ap_ptr->rrc->ho_preparation_complete(ue_ptr->ctxt.rnti, false, *ho_cmd_msg, {});
+    rrc_interface_s1ap::ho_prep_result ho_prep_result = ue_ptr->ts1_reloc_prep.is_expired()
+                                                            ? rrc_interface_s1ap::ho_prep_result::timeout
+                                                            : rrc_interface_s1ap::ho_prep_result::failure;
+    s1ap_ptr->rrc->ho_preparation_complete(ue_ptr->ctxt.rnti, ho_prep_result, *ho_cmd_msg, {});
   } else {
-    s1ap_ptr->rrc->ho_preparation_complete(ue_ptr->ctxt.rnti, true, *ho_cmd_msg, std::move(rrc_container));
+    s1ap_ptr->rrc->ho_preparation_complete(
+        ue_ptr->ctxt.rnti, rrc_interface_s1ap::ho_prep_result::success, *ho_cmd_msg, std::move(rrc_container));
     procInfo("Completed with success");
   }
 }
@@ -402,27 +462,13 @@ void s1ap::user_mod(uint16_t old_rnti, uint16_t new_rnti)
   users.find_ue_rnti(old_rnti)->ctxt.rnti = new_rnti;
 }
 
-void s1ap::ue_ctxt_setup_complete(uint16_t rnti, const asn1::s1ap::init_context_setup_resp_s& res)
+void s1ap::ue_ctxt_setup_complete(uint16_t rnti)
 {
   ue* u = users.find_ue_rnti(rnti);
   if (u == nullptr) {
     return;
   }
-  if (res.protocol_ies.erab_setup_list_ctxt_su_res.value.size() > 0) {
-    u->send_initial_ctxt_setup_response(res);
-  } else {
-    u->send_initial_ctxt_setup_failure();
-  }
-}
-
-void s1ap::ue_erab_setup_complete(uint16_t rnti, const asn1::s1ap::erab_setup_resp_s& res)
-{
-  ue* u = users.find_ue_rnti(rnti);
-  if (u == nullptr) {
-    logger.error("rnti 0x%x not found", rnti);
-    return;
-  }
-  u->send_erab_setup_response(res);
+  u->ue_ctxt_setup_complete();
 }
 
 bool s1ap::is_mme_connected()
@@ -678,9 +724,21 @@ bool s1ap::handle_dlnastransport(const dl_nas_transport_s& msg)
 
 bool s1ap::handle_initialctxtsetuprequest(const init_context_setup_request_s& msg)
 {
-  if (msg.ext) {
-    logger.warning("Not handling S1AP message extension");
-  }
+  const auto& prot_ies = msg.protocol_ies;
+  WarnUnsupportFeature(msg.ext, "message extension");
+  WarnUnsupportFeature(prot_ies.add_cs_fallback_ind_present, "AdditionalCSFallbackIndicator");
+  WarnUnsupportFeature(prot_ies.csg_membership_status_present, "CSGMembershipStatus");
+  WarnUnsupportFeature(prot_ies.gummei_id_present, "GUMMEI_ID");
+  WarnUnsupportFeature(prot_ies.ho_restrict_list_present, "HandoverRestrictionList");
+  WarnUnsupportFeature(prot_ies.management_based_mdt_allowed_present, "ManagementBasedMDTAllowed");
+  WarnUnsupportFeature(prot_ies.management_based_mdtplmn_list_present, "ManagementBasedMDTPLMNList");
+  WarnUnsupportFeature(prot_ies.mme_ue_s1ap_id_minus2_present, "MME_UE_S1AP_ID_2");
+  WarnUnsupportFeature(prot_ies.registered_lai_present, "RegisteredLAI");
+  WarnUnsupportFeature(prot_ies.srvcc_operation_possible_present, "SRVCCOperationPossible");
+  WarnUnsupportFeature(prot_ies.subscriber_profile_idfor_rfp_present, "SubscriberProfileIDforRFP");
+  WarnUnsupportFeature(prot_ies.trace_activation_present, "TraceActivation");
+  WarnUnsupportFeature(prot_ies.ue_radio_cap_present, "UERadioCapability");
+
   ue* u =
       handle_s1apmsg_ue_id(msg.protocol_ies.enb_ue_s1ap_id.value.value, msg.protocol_ies.mme_ue_s1ap_id.value.value);
   if (u == nullptr) {
@@ -692,6 +750,44 @@ bool s1ap::handle_initialctxtsetuprequest(const init_context_setup_request_s& ms
     return false;
   }
 
+  // Update E-RABs
+  erab_id_list   updated_erabs;
+  erab_item_list failed_cfg_erabs;
+  add_repeated_erab_ids(prot_ies.erab_to_be_setup_list_ctxt_su_req.value, failed_cfg_erabs);
+
+  for (const auto& item : msg.protocol_ies.erab_to_be_setup_list_ctxt_su_req.value) {
+    const auto& erab = item.value.erab_to_be_setup_item_ctxt_su_req();
+    if (contains_erab_id(failed_cfg_erabs, erab.erab_id)) {
+      // E-RAB is duplicate
+      continue;
+    }
+    WarnUnsupportFeature(erab.ext, "E-RABToBeSetupListBearerSUReq extensions");
+    WarnUnsupportFeature(erab.ie_exts_present, "E-RABToBeSetupListBearerSUReq extensions");
+
+    if (erab.transport_layer_address.length() > 32) {
+      logger.error("IPv6 addresses not currently supported");
+      failed_cfg_erabs.push_back(erab_item_s());
+      failed_cfg_erabs.back().erab_id                         = erab.erab_id;
+      failed_cfg_erabs.back().cause.set_radio_network().value = cause_radio_network_opts::invalid_qos_combination;
+      continue;
+    }
+
+    cause_c cause;
+    if (rrc->setup_erab(u->ctxt.rnti,
+                        erab.erab_id,
+                        erab.erab_level_qos_params,
+                        erab.nas_pdu,
+                        erab.transport_layer_address,
+                        erab.gtp_teid.to_number(),
+                        cause) == SRSRAN_SUCCESS) {
+      updated_erabs.push_back(erab.erab_id);
+    } else {
+      failed_cfg_erabs.push_back(erab_item_s());
+      failed_cfg_erabs.back().erab_id = erab.erab_id;
+      failed_cfg_erabs.back().cause   = cause;
+    }
+  }
+
   /* Ideally the check below would be "if (users[rnti].is_csfb)" */
   if (msg.protocol_ies.cs_fallback_ind_present) {
     if (msg.protocol_ies.cs_fallback_ind.value.value == cs_fallback_ind_opts::cs_fallback_required ||
@@ -699,20 +795,22 @@ bool s1ap::handle_initialctxtsetuprequest(const init_context_setup_request_s& ms
       // Send RRC Release (cs-fallback-triggered) to MME
       cause_c cause;
       cause.set_radio_network().value = cause_radio_network_opts::cs_fallback_triggered;
-
       /* TODO: This should normally probably only be sent after the SecurityMode procedure has completed! */
       u->send_uectxtreleaserequest(cause);
     }
   }
 
+  // E-RAB Setup Response is sent after the security cfg is complete
+  // Note: No need to notify RRC to send RRC Reconfiguration
+  sanitize_response_erab_lists(failed_cfg_erabs, updated_erabs);
+  u->set_state(s1ap_proc_id_t::init_context_setup_request, updated_erabs, failed_cfg_erabs);
   return true;
 }
 
 bool s1ap::handle_paging(const asn1::s1ap::paging_s& msg)
 {
-  if (msg.ext) {
-    logger.warning("Not handling S1AP message extension");
-  }
+  WarnUnsupportFeature(msg.ext, "S1AP message extension");
+
   uint32_t ueid = msg.protocol_ies.ue_id_idx_value.value.to_number();
   rrc->add_paging_id(ueid, msg.protocol_ies.ue_paging_id.value);
   return true;
@@ -720,43 +818,110 @@ bool s1ap::handle_paging(const asn1::s1ap::paging_s& msg)
 
 bool s1ap::handle_erabsetuprequest(const erab_setup_request_s& msg)
 {
-  if (msg.ext) {
-    logger.warning("Not handling S1AP message extension");
-  }
+  WarnUnsupportFeature(msg.ext, "S1AP message extension");
+
   ue* u =
       handle_s1apmsg_ue_id(msg.protocol_ies.enb_ue_s1ap_id.value.value, msg.protocol_ies.mme_ue_s1ap_id.value.value);
   if (u == nullptr) {
     return false;
   }
 
-  // Setup UE ctxt in RRC
-  return rrc->setup_ue_erabs(u->ctxt.rnti, msg);
+  if (msg.protocol_ies.ueaggregate_maximum_bitrate_present) {
+    rrc->set_aggregate_max_bitrate(u->ctxt.rnti, msg.protocol_ies.ueaggregate_maximum_bitrate.value);
+  }
+
+  erab_id_list   updated_erabs;
+  erab_item_list failed_cfg_erabs;
+  add_repeated_erab_ids(msg.protocol_ies.erab_to_be_setup_list_bearer_su_req.value, failed_cfg_erabs);
+
+  for (const auto& item : msg.protocol_ies.erab_to_be_setup_list_bearer_su_req.value) {
+    const auto& erab = item.value.erab_to_be_setup_item_bearer_su_req();
+    if (contains_erab_id(failed_cfg_erabs, erab.erab_id)) {
+      // E-RAB is duplicate
+      continue;
+    }
+    WarnUnsupportFeature(erab.ext, "E-RABToBeSetupListBearerSUReq extensions");
+    WarnUnsupportFeature(erab.ie_exts_present, "E-RABToBeSetupListBearerSUReq extensions");
+
+    if (erab.transport_layer_address.length() > 32) {
+      logger.error("IPv6 addresses not currently supported");
+      failed_cfg_erabs.push_back(erab_item_s());
+      failed_cfg_erabs.back().erab_id                         = erab.erab_id;
+      failed_cfg_erabs.back().cause.set_radio_network().value = cause_radio_network_opts::invalid_qos_combination;
+      continue;
+    }
+
+    cause_c cause;
+    if (rrc->setup_erab(u->ctxt.rnti,
+                        erab.erab_id,
+                        erab.erab_level_qos_params,
+                        erab.nas_pdu,
+                        erab.transport_layer_address,
+                        erab.gtp_teid.to_number(),
+                        cause) == SRSRAN_SUCCESS) {
+      updated_erabs.push_back(erab.erab_id);
+    } else {
+      failed_cfg_erabs.push_back(erab_item_s());
+      failed_cfg_erabs.back().erab_id = erab.erab_id;
+      failed_cfg_erabs.back().cause   = cause;
+    }
+  }
+
+  // Notify UE of updates
+  if (not updated_erabs.empty()) {
+    rrc->notify_ue_erab_updates(u->ctxt.rnti, {});
+  }
+
+  sanitize_response_erab_lists(failed_cfg_erabs, updated_erabs);
+  return u->send_erab_setup_response(updated_erabs, failed_cfg_erabs);
 }
 
 bool s1ap::handle_erabmodifyrequest(const erab_modify_request_s& msg)
 {
-  std::vector<uint16_t> erab_successful_modified = {};
-  std::vector<uint16_t> erab_failed_to_modify    = {};
+  WarnUnsupportFeature(msg.ext, "S1AP message extension");
 
-  if (msg.ext) {
-    logger.warning("Not handling S1AP message extension");
-  }
   ue* u =
       handle_s1apmsg_ue_id(msg.protocol_ies.enb_ue_s1ap_id.value.value, msg.protocol_ies.mme_ue_s1ap_id.value.value);
   if (u == nullptr) {
     return false;
   }
 
-  // Modify E-RABs from RRC
-  rrc->modify_erabs(u->ctxt.rnti, msg, &erab_successful_modified, &erab_failed_to_modify);
+  if (msg.protocol_ies.ueaggregate_maximum_bitrate_present) {
+    rrc->set_aggregate_max_bitrate(u->ctxt.rnti, msg.protocol_ies.ueaggregate_maximum_bitrate.value);
+  }
 
-  // Send E-RAB modify response back to the MME
-  if (not u->send_erab_modify_response(erab_successful_modified, erab_failed_to_modify)) {
-    logger.info("Failed to send ERABReleaseResponse");
-    return false;
+  erab_id_list   updated_erabs;
+  erab_item_list failed_cfg_erabs;
+  add_repeated_erab_ids(msg.protocol_ies.erab_to_be_modified_list_bearer_mod_req.value, failed_cfg_erabs);
+
+  for (const auto& item : msg.protocol_ies.erab_to_be_modified_list_bearer_mod_req.value) {
+    const auto& erab = item.value.erab_to_be_modified_item_bearer_mod_req();
+    if (contains_erab_id(failed_cfg_erabs, erab.erab_id)) {
+      // E-RAB is duplicate
+      continue;
+    }
+    WarnUnsupportFeature(erab.ext, "E-RABToBeSetupListBearerSUReq extensions");
+    WarnUnsupportFeature(erab.ie_exts_present, "E-RABToBeSetupListBearerSUReq extensions");
+
+    cause_c cause;
+    if (rrc->modify_erab(u->ctxt.rnti, erab.erab_id, erab.erab_level_qos_params, erab.nas_pdu, cause) ==
+        SRSRAN_SUCCESS) {
+      updated_erabs.push_back(erab.erab_id);
+    } else {
+      failed_cfg_erabs.push_back(erab_item_s());
+      failed_cfg_erabs.back().erab_id = erab.erab_id;
+      failed_cfg_erabs.back().cause   = cause;
+    }
   }
 
-  return true;
+  // Notify UE of updates
+  if (not updated_erabs.empty()) {
+    rrc->notify_ue_erab_updates(u->ctxt.rnti, {});
+  }
+
+  // send E-RAB modify response back to the mme
+  sanitize_response_erab_lists(failed_cfg_erabs, updated_erabs);
+  return u->send_erab_modify_response(updated_erabs, failed_cfg_erabs);
 }
 
 /**
@@ -768,23 +933,48 @@ bool s1ap::handle_erabmodifyrequest(const erab_modify_request_s& msg)
  */
 bool s1ap::handle_erabreleasecommand(const erab_release_cmd_s& msg)
 {
-  std::vector<uint16_t> erab_successful_release = {};
-  std::vector<uint16_t> erab_failed_to_release  = {};
+  WarnUnsupportFeature(msg.ext, "S1AP message extension");
 
-  if (msg.ext) {
-    logger.warning("Not handling S1AP message extension");
-  }
   ue* u =
       handle_s1apmsg_ue_id(msg.protocol_ies.enb_ue_s1ap_id.value.value, msg.protocol_ies.mme_ue_s1ap_id.value.value);
   if (u == nullptr) {
     return false;
   }
 
-  // Release E-RABs from RRC
-  rrc->release_erabs(u->ctxt.rnti, msg, &erab_successful_release, &erab_failed_to_release);
+  erab_id_list   updated_erabs;
+  erab_item_list failed_cfg_erabs;
+
+  auto is_repeated_erab_id = [&updated_erabs, &failed_cfg_erabs](uint8_t erab_id) {
+    return (std::count(updated_erabs.begin(), updated_erabs.end(), erab_id) > 0) or
+           (std::any_of(failed_cfg_erabs.begin(), failed_cfg_erabs.end(), [erab_id](const erab_item_s& e) {
+             return e.erab_id == erab_id;
+           }));
+  };
+  for (const auto& item : msg.protocol_ies.erab_to_be_released_list.value) {
+    const auto& erab = item.value.erab_item();
+
+    if (is_repeated_erab_id(erab.erab_id)) {
+      // TS 36.413, 8.2.3.3 - ignore the duplication of E-RAB ID IEs
+      continue;
+    }
+
+    if (rrc->release_erab(u->ctxt.rnti, erab.erab_id) == SRSRAN_SUCCESS) {
+      updated_erabs.push_back(erab.erab_id);
+    } else {
+      failed_cfg_erabs.push_back(erab_item_s());
+      failed_cfg_erabs.back().erab_id                         = erab.erab_id;
+      failed_cfg_erabs.back().cause.set_radio_network().value = cause_radio_network_opts::unknown_erab_id;
+    }
+  }
+
+  // Notify RRC of E-RAB update. (RRC reconf message is going to be sent.
+  if (not updated_erabs.empty()) {
+    rrc->notify_ue_erab_updates(u->ctxt.rnti, msg.protocol_ies.nas_pdu.value);
+  }
 
   // Send E-RAB release response back to the MME
-  if (not u->send_erab_release_response(erab_successful_release, erab_failed_to_release)) {
+  sanitize_response_erab_lists(failed_cfg_erabs, updated_erabs);
+  if (not u->send_erab_release_response(updated_erabs, failed_cfg_erabs)) {
     logger.info("Failed to send ERABReleaseResponse");
     return false;
   }
@@ -794,6 +984,12 @@ bool s1ap::handle_erabreleasecommand(const erab_release_cmd_s& msg)
 
 bool s1ap::handle_uecontextmodifyrequest(const ue_context_mod_request_s& msg)
 {
+  WarnUnsupportFeature(msg.ext, "S1AP message extension");
+  WarnUnsupportFeature(msg.protocol_ies.add_cs_fallback_ind_present, "AdditionalCSFallbackIndicator");
+  WarnUnsupportFeature(msg.protocol_ies.csg_membership_status_present, "CSGMembershipStatus");
+  WarnUnsupportFeature(msg.protocol_ies.registered_lai_present, "RegisteredLAI");
+  WarnUnsupportFeature(msg.protocol_ies.subscriber_profile_idfor_rfp_present, "SubscriberProfileIDforRFP");
+
   ue* u =
       handle_s1apmsg_ue_id(msg.protocol_ies.enb_ue_s1ap_id.value.value, msg.protocol_ies.mme_ue_s1ap_id.value.value);
   if (u == nullptr) {
@@ -827,13 +1023,11 @@ bool s1ap::handle_uecontextmodifyrequest(const ue_context_mod_request_s& msg)
 
 bool s1ap::handle_uectxtreleasecommand(const ue_context_release_cmd_s& msg)
 {
-  if (msg.ext) {
-    logger.warning("Not handling S1AP message extension");
-  }
+  WarnUnsupportFeature(msg.ext, "S1AP message extension");
 
   ue* u = nullptr;
   if (msg.protocol_ies.ue_s1ap_ids.value.type().value == ue_s1ap_ids_c::types_opts::ue_s1ap_id_pair) {
-    auto& idpair = msg.protocol_ies.ue_s1ap_ids.value.ue_s1ap_id_pair();
+    const auto& idpair = msg.protocol_ies.ue_s1ap_ids.value.ue_s1ap_id_pair();
 
     if (idpair.ext) {
       logger.warning("Not handling S1AP message extension");
@@ -899,14 +1093,10 @@ bool s1ap::handle_handover_command(const asn1::s1ap::ho_cmd_s& msg)
 
 bool s1ap::handle_handover_request(const asn1::s1ap::ho_request_s& msg)
 {
-  uint16_t rnti = SRSRAN_INVALID_RNTI;
-
-  auto on_scope_exit = srsran::make_scope_exit([this, &rnti, msg]() {
-    // If rnti is not allocated successfully, remove from s1ap and send handover failure
-    if (rnti == SRSRAN_INVALID_RNTI) {
-      send_ho_failure(msg.protocol_ies.mme_ue_s1ap_id.value.value);
-    }
-  });
+  uint16_t            rnti           = SRSRAN_INVALID_RNTI;
+  uint32_t            mme_ue_s1ap_id = msg.protocol_ies.mme_ue_s1ap_id.value.value;
+  asn1::s1ap::cause_c cause;
+  cause.set_misc().value = cause_misc_opts::unspecified;
 
   if (msg.ext or msg.protocol_ies.ho_restrict_list_present) {
     logger.warning("Not handling S1AP Handover Request extensions or Handover Restriction List");
@@ -914,6 +1104,8 @@ bool s1ap::handle_handover_request(const asn1::s1ap::ho_request_s& msg)
 
   if (msg.protocol_ies.handov_type.value.value != handov_type_opts::intralte) {
     logger.error("Not handling S1AP non-intra LTE handovers");
+    cause.set_radio_network().value = cause_radio_network_opts::interrat_redirection;
+    send_ho_failure(mme_ue_s1ap_id, cause);
     return false;
   }
 
@@ -921,31 +1113,37 @@ bool s1ap::handle_handover_request(const asn1::s1ap::ho_request_s& msg)
   if (users.find_ue_mmeid(msg.protocol_ies.mme_ue_s1ap_id.value.value) != nullptr) {
     logger.error("The provided MME_UE_S1AP_ID=%" PRIu64 " is already connected to the cell",
                  msg.protocol_ies.mme_ue_s1ap_id.value.value);
+    cause.set_radio_network().value = cause_radio_network_opts::unknown_mme_ue_s1ap_id;
+    send_ho_failure(mme_ue_s1ap_id, cause);
     return false;
   }
 
   // Create user ctxt object and associated MME context
   std::unique_ptr<ue> ue_ptr{new ue{this}};
   ue_ptr->ctxt.mme_ue_s1ap_id = msg.protocol_ies.mme_ue_s1ap_id.value.value;
-  if (users.add_user(std::move(ue_ptr)) == nullptr) {
-    return false;
-  }
+  srsran_assert(users.add_user(std::move(ue_ptr)) != nullptr, "Unexpected failure to create S1AP UE");
 
   // Unpack Transparent Container
   sourceenb_to_targetenb_transparent_container_s container;
   asn1::cbit_ref bref{msg.protocol_ies.source_to_target_transparent_container.value.data(),
                       msg.protocol_ies.source_to_target_transparent_container.value.size()};
   if (container.unpack(bref) != asn1::SRSASN_SUCCESS) {
-    logger.error("Failed to unpack SourceToTargetTransparentContainer");
+    logger.warning("Failed to unpack SourceToTargetTransparentContainer");
+    cause.set_protocol().value = cause_protocol_opts::transfer_syntax_error;
+    send_ho_failure(mme_ue_s1ap_id, cause);
     return false;
   }
 
   // Handle Handover Resource Allocation
-  rnti = rrc->start_ho_ue_resource_alloc(msg, container);
-  return rnti != SRSRAN_INVALID_RNTI;
+  rnti = rrc->start_ho_ue_resource_alloc(msg, container, cause);
+  if (rnti == SRSRAN_INVALID_RNTI) {
+    send_ho_failure(mme_ue_s1ap_id, cause);
+    return false;
+  }
+  return true;
 }
 
-bool s1ap::send_ho_failure(uint32_t mme_ue_s1ap_id)
+void s1ap::send_ho_failure(uint32_t mme_ue_s1ap_id, const asn1::s1ap::cause_c& cause)
 {
   // Remove created s1ap user
   ue* u = users.find_ue_mmeid(mme_ue_s1ap_id);
@@ -958,17 +1156,17 @@ bool s1ap::send_ho_failure(uint32_t mme_ue_s1ap_id)
   ho_fail_ies_container& container = tx_pdu.unsuccessful_outcome().value.ho_fail().protocol_ies;
 
   container.mme_ue_s1ap_id.value = mme_ue_s1ap_id;
-  // TODO: Setup cause
-  container.cause.value.set_radio_network().value = cause_radio_network_opts::ho_target_not_allowed;
+  container.cause.value          = cause;
 
-  return sctp_send_s1ap_pdu(tx_pdu, SRSRAN_INVALID_RNTI, "HandoverFailure");
+  sctp_send_s1ap_pdu(tx_pdu, SRSRAN_INVALID_RNTI, "HandoverFailure");
 }
 
 bool s1ap::send_ho_req_ack(const asn1::s1ap::ho_request_s&                msg,
                            uint16_t                                       rnti,
                            uint32_t                                       enb_cc_idx,
                            srsran::unique_byte_buffer_t                   ho_cmd,
-                           srsran::span<asn1::s1ap::erab_admitted_item_s> admitted_bearers)
+                           srsran::span<asn1::s1ap::erab_admitted_item_s> admitted_bearers,
+                           srsran::const_span<asn1::s1ap::erab_item_s>    not_admitted_bearers)
 {
   s1ap_pdu_c tx_pdu;
   tx_pdu.set_successful_outcome().load_info_obj(ASN1_S1AP_ID_HO_RES_ALLOC);
@@ -1000,6 +1198,19 @@ bool s1ap::send_ho_req_ack(const asn1::s1ap::ho_request_s&                msg,
     }
   }
 
+  // Add failed to Setup E-RABs
+  if (not not_admitted_bearers.empty()) {
+    container.erab_failed_to_setup_list_ho_req_ack_present = true;
+    container.erab_failed_to_setup_list_ho_req_ack.value.resize(not_admitted_bearers.size());
+    for (size_t i = 0; i < not_admitted_bearers.size(); ++i) {
+      container.erab_failed_to_setup_list_ho_req_ack.value[i].load_info_obj(
+          ASN1_S1AP_ID_ERAB_FAILEDTO_SETUP_ITEM_HO_REQ_ACK);
+      auto& erab = container.erab_failed_to_setup_list_ho_req_ack.value[i].value.erab_failedto_setup_item_ho_req_ack();
+      erab.erab_id = not_admitted_bearers[i].erab_id;
+      erab.cause   = not_admitted_bearers[i].cause;
+    }
+  }
+
   // Pack transparent container
   asn1::s1ap::targetenb_to_sourceenb_transparent_container_s transparent_container;
   transparent_container.rrc_container.resize(ho_cmd->N_bytes);
@@ -1053,10 +1264,11 @@ void s1ap::send_ho_notify(uint16_t rnti, uint64_t target_eci)
   sctp_send_s1ap_pdu(tx_pdu, rnti, "HandoverNotify");
 }
 
-void s1ap::send_ho_cancel(uint16_t rnti)
+void s1ap::send_ho_cancel(uint16_t rnti, const asn1::s1ap::cause_c& cause)
 {
   ue* user_ptr = users.find_ue_rnti(rnti);
   if (user_ptr == nullptr) {
+    logger.warning("Canceling handover for non-existent rnti=0x%x", rnti);
     return;
   }
 
@@ -1065,9 +1277,9 @@ void s1ap::send_ho_cancel(uint16_t rnti)
   tx_pdu.set_init_msg().load_info_obj(ASN1_S1AP_ID_HO_CANCEL);
   ho_cancel_ies_container& container = tx_pdu.init_msg().value.ho_cancel().protocol_ies;
 
-  container.mme_ue_s1ap_id.value                  = user_ptr->ctxt.mme_ue_s1ap_id.value();
-  container.enb_ue_s1ap_id.value                  = user_ptr->ctxt.enb_ue_s1ap_id;
-  container.cause.value.set_radio_network().value = cause_radio_network_opts::ho_cancelled;
+  container.mme_ue_s1ap_id.value = user_ptr->ctxt.mme_ue_s1ap_id.value();
+  container.enb_ue_s1ap_id.value = user_ptr->ctxt.enb_ue_s1ap_id;
+  container.cause.value          = cause;
 
   sctp_send_s1ap_pdu(tx_pdu, rnti, "HandoverCancel");
 }
@@ -1218,103 +1430,101 @@ bool s1ap::ue::send_uectxtreleaserequest(const cause_c& cause)
 
 bool s1ap::ue::send_uectxtreleasecomplete()
 {
-  if (not s1ap_ptr->mme_connected) {
-    return false;
-  }
-
   s1ap_pdu_c tx_pdu;
   tx_pdu.set_successful_outcome().load_info_obj(ASN1_S1AP_ID_UE_CONTEXT_RELEASE);
   auto& container                = tx_pdu.successful_outcome().value.ue_context_release_complete().protocol_ies;
   container.enb_ue_s1ap_id.value = ctxt.enb_ue_s1ap_id;
   container.mme_ue_s1ap_id.value = ctxt.mme_ue_s1ap_id.value();
 
+  // Stop TS1 Reloc Overall
+  ts1_reloc_overall.stop();
+
   // Log event.
   event_logger::get().log_s1_ctx_delete(ctxt.enb_cc_idx, ctxt.mme_ue_s1ap_id.value(), ctxt.enb_ue_s1ap_id, ctxt.rnti);
 
   return s1ap_ptr->sctp_send_s1ap_pdu(tx_pdu, ctxt.rnti, "UEContextReleaseComplete");
 }
 
-bool s1ap::ue::send_initial_ctxt_setup_response(const asn1::s1ap::init_context_setup_resp_s& res_)
+void s1ap::ue::ue_ctxt_setup_complete()
 {
-  if (not s1ap_ptr->mme_connected) {
-    return false;
+  if (current_state != s1ap_elem_procs_o::init_msg_c::types_opts::init_context_setup_request) {
+    logger.warning("Procedure %s,rnti=0x%x - Received unexpected complete notification",
+                   s1ap_elem_procs_o::init_msg_c::types_opts{current_state}.to_string().c_str(),
+                   ctxt.rnti);
+    return;
   }
+  current_state = s1ap_elem_procs_o::init_msg_c::types_opts::nulltype;
 
   s1ap_pdu_c tx_pdu;
-  tx_pdu.set_successful_outcome().load_info_obj(ASN1_S1AP_ID_INIT_CONTEXT_SETUP);
+  if (updated_erabs.empty()) {
+    // It is ICS Failure
+    tx_pdu.set_unsuccessful_outcome().load_info_obj(ASN1_S1AP_ID_INIT_CONTEXT_SETUP);
+    auto& container = tx_pdu.unsuccessful_outcome().value.init_context_setup_fail().protocol_ies;
+
+    container.enb_ue_s1ap_id.value = ctxt.enb_ue_s1ap_id;
+    container.mme_ue_s1ap_id.value = ctxt.mme_ue_s1ap_id.value();
+    container.cause.value          = failed_cfg_erabs.front().cause;
+    s1ap_ptr->sctp_send_s1ap_pdu(tx_pdu, ctxt.rnti, "UEContextModificationFailure");
+    return;
+  }
 
-  // Copy in the provided response message
-  tx_pdu.successful_outcome().value.init_context_setup_resp() = res_;
+  // It is ICS Response
+  tx_pdu.set_successful_outcome().load_info_obj(ASN1_S1AP_ID_INIT_CONTEXT_SETUP);
+  auto& container = tx_pdu.successful_outcome().value.init_context_setup_resp().protocol_ies;
 
   // Fill in the MME and eNB IDs
-  auto& container                = tx_pdu.successful_outcome().value.init_context_setup_resp().protocol_ies;
   container.mme_ue_s1ap_id.value = ctxt.mme_ue_s1ap_id.value();
   container.enb_ue_s1ap_id.value = ctxt.enb_ue_s1ap_id;
 
-  // Fill in the GTP bind address for all bearers
-  for (uint32_t i = 0; i < container.erab_setup_list_ctxt_su_res.value.size(); ++i) {
-    auto& item = container.erab_setup_list_ctxt_su_res.value[i].value.erab_setup_item_ctxt_su_res();
-    item.transport_layer_address.resize(32);
-    uint8_t addr[4];
-    inet_pton(AF_INET, s1ap_ptr->args.gtp_bind_addr.c_str(), addr);
-    for (uint32_t j = 0; j < 4; ++j) {
-      item.transport_layer_address.data()[j] = addr[3 - j];
-    }
+  // Add list of E-RABs that were not setup
+  if (not failed_cfg_erabs.empty()) {
+    container.erab_failed_to_setup_list_ctxt_su_res_present = true;
+    fill_erab_failed_setup_list(container.erab_failed_to_setup_list_ctxt_su_res.value, failed_cfg_erabs);
+  }
+
+  // Add setup E-RABs
+  container.erab_setup_list_ctxt_su_res.value.resize(updated_erabs.size());
+  for (size_t i = 0; i < updated_erabs.size(); ++i) {
+    container.erab_setup_list_ctxt_su_res.value[i].load_info_obj(ASN1_S1AP_ID_ERAB_SETUP_ITEM_CTXT_SU_RES);
+    auto& item   = container.erab_setup_list_ctxt_su_res.value[i].value.erab_setup_item_ctxt_su_res();
+    item.erab_id = updated_erabs[i];
+    get_erab_addr(item.erab_id, item.transport_layer_address, item.gtp_teid);
   }
 
   // Log event.
   event_logger::get().log_s1_ctx_create(ctxt.enb_cc_idx, ctxt.mme_ue_s1ap_id.value(), ctxt.enb_ue_s1ap_id, ctxt.rnti);
 
-  return s1ap_ptr->sctp_send_s1ap_pdu(tx_pdu, ctxt.rnti, "InitialContextSetupResponse");
+  s1ap_ptr->sctp_send_s1ap_pdu(tx_pdu, ctxt.rnti, "E-RABSetupResponse");
 }
 
-bool s1ap::ue::send_erab_setup_response(const erab_setup_resp_s& res_)
+bool s1ap::ue::send_erab_setup_response(const erab_id_list& erabs_setup, const erab_item_list& erabs_failed)
 {
-  if (not s1ap_ptr->mme_connected) {
-    return false;
-  }
-
   asn1::s1ap::s1ap_pdu_c tx_pdu;
   tx_pdu.set_successful_outcome().load_info_obj(ASN1_S1AP_ID_ERAB_SETUP);
   erab_setup_resp_s& res = tx_pdu.successful_outcome().value.erab_setup_resp();
 
-  res = res_;
-
-  // Fill in the GTP bind address for all bearers
-  if (res.protocol_ies.erab_setup_list_bearer_su_res_present) {
-    for (uint32_t i = 0; i < res.protocol_ies.erab_setup_list_bearer_su_res.value.size(); ++i) {
-      auto& item = res.protocol_ies.erab_setup_list_bearer_su_res.value[i].value.erab_setup_item_bearer_su_res();
-      item.transport_layer_address.resize(32);
-      uint8_t addr[4];
-      inet_pton(AF_INET, s1ap_ptr->args.gtp_bind_addr.c_str(), addr);
-      for (uint32_t j = 0; j < 4; ++j) {
-        item.transport_layer_address.data()[j] = addr[3 - j];
-      }
-    }
-  }
-
   // Fill in the MME and eNB IDs
   res.protocol_ies.mme_ue_s1ap_id.value = ctxt.mme_ue_s1ap_id.value();
   res.protocol_ies.enb_ue_s1ap_id.value = ctxt.enb_ue_s1ap_id;
 
-  return s1ap_ptr->sctp_send_s1ap_pdu(tx_pdu, ctxt.rnti, "E_RABSetupResponse");
-}
-
-bool s1ap::ue::send_initial_ctxt_setup_failure()
-{
-  if (not s1ap_ptr->mme_connected) {
-    return false;
+  // Add list of E-RABs that were not setup
+  if (not erabs_failed.empty()) {
+    res.protocol_ies.erab_failed_to_setup_list_bearer_su_res_present = true;
+    fill_erab_failed_setup_list(res.protocol_ies.erab_failed_to_setup_list_bearer_su_res.value, erabs_failed);
   }
 
-  s1ap_pdu_c tx_pdu;
-  tx_pdu.set_unsuccessful_outcome().load_info_obj(ASN1_S1AP_ID_INIT_CONTEXT_SETUP);
-  auto& container = tx_pdu.unsuccessful_outcome().value.init_context_setup_fail().protocol_ies;
-
-  container.enb_ue_s1ap_id.value                  = ctxt.enb_ue_s1ap_id;
-  container.mme_ue_s1ap_id.value                  = ctxt.mme_ue_s1ap_id.value();
-  container.cause.value.set_radio_network().value = cause_radio_network_opts::unspecified;
+  if (not erabs_setup.empty()) {
+    res.protocol_ies.erab_setup_list_bearer_su_res_present = true;
+    res.protocol_ies.erab_setup_list_bearer_su_res.value.resize(erabs_setup.size());
+    for (size_t i = 0; i < erabs_setup.size(); ++i) {
+      res.protocol_ies.erab_setup_list_bearer_su_res.value[i].load_info_obj(ASN1_S1AP_ID_ERAB_SETUP_ITEM_BEARER_SU_RES);
+      auto& item   = res.protocol_ies.erab_setup_list_bearer_su_res.value[i].value.erab_setup_item_bearer_su_res();
+      item.erab_id = erabs_setup[i];
+      get_erab_addr(item.erab_id, item.transport_layer_address, item.gtp_teid);
+    }
+  }
 
-  return s1ap_ptr->sctp_send_s1ap_pdu(tx_pdu, ctxt.rnti, "InitialContextSetupFailure");
+  return s1ap_ptr->sctp_send_s1ap_pdu(tx_pdu, ctxt.rnti, "E_RABSetupResponse");
 }
 
 bool s1ap::ue::send_uectxtmodifyresp()
@@ -1357,13 +1567,8 @@ bool s1ap::ue::send_uectxtmodifyfailure(const cause_c& cause)
  * @param erabs_failed_to_release
  * @return true if message was sent
  */
-bool s1ap::ue::send_erab_release_response(const std::vector<uint16_t>& erabs_successfully_released,
-                                          const std::vector<uint16_t>& erabs_failed_to_release)
+bool s1ap::ue::send_erab_release_response(const erab_id_list& erabs_released, const erab_item_list& erabs_failed)
 {
-  if (not s1ap_ptr->mme_connected) {
-    return false;
-  }
-
   asn1::s1ap::s1ap_pdu_c tx_pdu;
   tx_pdu.set_successful_outcome().load_info_obj(ASN1_S1AP_ID_ERAB_RELEASE);
 
@@ -1372,40 +1577,28 @@ bool s1ap::ue::send_erab_release_response(const std::vector<uint16_t>& erabs_suc
   container.mme_ue_s1ap_id.value = ctxt.mme_ue_s1ap_id.value();
 
   // Fill in which E-RABs were successfully released
-  if (not erabs_successfully_released.empty()) {
+  if (not erabs_released.empty()) {
     container.erab_release_list_bearer_rel_comp_present = true;
-    container.erab_release_list_bearer_rel_comp.value.resize(erabs_successfully_released.size());
-    for (uint32_t i = 0; i < container.erab_release_list_bearer_rel_comp.value.size(); i++) {
+    container.erab_release_list_bearer_rel_comp.value.resize(erabs_released.size());
+    for (size_t i = 0; i < erabs_released.size(); ++i) {
       container.erab_release_list_bearer_rel_comp.value[i].load_info_obj(
           ASN1_S1AP_ID_ERAB_RELEASE_ITEM_BEARER_REL_COMP);
       container.erab_release_list_bearer_rel_comp.value[i].value.erab_release_item_bearer_rel_comp().erab_id =
-          erabs_successfully_released[i];
+          erabs_released[i];
     }
   }
 
   // Fill in which E-RABs were *not* successfully released
-  if (not erabs_failed_to_release.empty()) {
+  if (not erabs_failed.empty()) {
     container.erab_failed_to_release_list_present = true;
-    container.erab_failed_to_release_list.value.resize(erabs_failed_to_release.size());
-    for (uint32_t i = 0; i < container.erab_failed_to_release_list.value.size(); i++) {
-      container.erab_failed_to_release_list.value[i].load_info_obj(ASN1_S1AP_ID_ERAB_ITEM);
-      container.erab_failed_to_release_list.value[i].value.erab_item().erab_id = erabs_failed_to_release[i];
-      container.erab_failed_to_release_list.value[i].value.erab_item().cause.set(asn1::s1ap::cause_c::types::misc);
-      container.erab_failed_to_release_list.value[i].value.erab_item().cause.misc() =
-          asn1::s1ap::cause_misc_opts::unspecified;
-    }
+    fill_erab_failed_setup_list(container.erab_failed_to_release_list.value, erabs_failed);
   }
 
   return s1ap_ptr->sctp_send_s1ap_pdu(tx_pdu, ctxt.rnti, "E-RABReleaseResponse");
 }
 
-bool s1ap::ue::send_erab_modify_response(const std::vector<uint16_t>& erabs_successfully_modified,
-                                         const std::vector<uint16_t>& erabs_failed_to_modify)
+bool s1ap::ue::send_erab_modify_response(const erab_id_list& erabs_modified, const erab_item_list& erabs_failed)
 {
-  if (not s1ap_ptr->mme_connected) {
-    return false;
-  }
-
   asn1::s1ap::s1ap_pdu_c tx_pdu;
   tx_pdu.set_successful_outcome().load_info_obj(ASN1_S1AP_ID_ERAB_MODIFY);
 
@@ -1414,28 +1607,20 @@ bool s1ap::ue::send_erab_modify_response(const std::vector<uint16_t>& erabs_succ
   container.mme_ue_s1ap_id.value = ctxt.mme_ue_s1ap_id.value();
 
   // Fill in which E-RABs were successfully released
-  if (not erabs_successfully_modified.empty()) {
+  if (not erabs_modified.empty()) {
     container.erab_modify_list_bearer_mod_res_present = true;
-    container.erab_modify_list_bearer_mod_res.value.resize(erabs_successfully_modified.size());
+    container.erab_modify_list_bearer_mod_res.value.resize(erabs_modified.size());
     for (uint32_t i = 0; i < container.erab_modify_list_bearer_mod_res.value.size(); i++) {
       container.erab_modify_list_bearer_mod_res.value[i].load_info_obj(ASN1_S1AP_ID_ERAB_MODIFY_ITEM_BEARER_MOD_RES);
       container.erab_modify_list_bearer_mod_res.value[i].value.erab_modify_item_bearer_mod_res().erab_id =
-          erabs_successfully_modified[i];
+          erabs_modified[i];
     }
   }
 
   // Fill in which E-RABs were *not* successfully released
-  if (not erabs_failed_to_modify.empty()) {
+  if (not erabs_failed.empty()) {
     container.erab_failed_to_modify_list_present = true;
-    container.erab_failed_to_modify_list.value.resize(erabs_failed_to_modify.size());
-    for (uint32_t i = 0; i < container.erab_failed_to_modify_list.value.size(); i++) {
-      container.erab_failed_to_modify_list.value[i].load_info_obj(ASN1_S1AP_ID_ERAB_ITEM);
-      container.erab_failed_to_modify_list.value[i].value.erab_item().erab_id = erabs_failed_to_modify[i];
-      container.erab_failed_to_modify_list.value[i].value.erab_item().cause.set(
-          asn1::s1ap::cause_c::types_opts::radio_network);
-      container.erab_failed_to_modify_list.value[i].value.erab_item().cause.radio_network().value =
-          cause_radio_network_opts::unknown_erab_id;
-    }
+    fill_erab_failed_setup_list(container.erab_failed_to_modify_list.value, erabs_failed);
   }
 
   return s1ap_ptr->sctp_send_s1ap_pdu(tx_pdu, ctxt.rnti, "E-RABModifyResponse");
@@ -1443,9 +1628,6 @@ bool s1ap::ue::send_erab_modify_response(const std::vector<uint16_t>& erabs_succ
 
 bool s1ap::ue::send_erab_release_indication(const std::vector<uint16_t>& erabs_successfully_released)
 {
-  if (not s1ap_ptr->mme_connected) {
-    return false;
-  }
   if (not erabs_successfully_released.empty()) {
     logger.error("Failed to initiate E-RAB RELEASE INDICATION procedure for user rnti=0x%x", ctxt.rnti);
     return false;
@@ -1470,10 +1652,6 @@ bool s1ap::ue::send_erab_release_indication(const std::vector<uint16_t>& erabs_s
 
 bool s1ap::ue::send_ue_cap_info_indication(srsran::unique_byte_buffer_t ue_radio_cap)
 {
-  if (not s1ap_ptr->mme_connected) {
-    return false;
-  }
-
   asn1::s1ap::s1ap_pdu_c tx_pdu;
   tx_pdu.set_init_msg().load_info_obj(ASN1_S1AP_ID_UE_CAP_INFO_IND);
   ue_cap_info_ind_ies_container& container = tx_pdu.init_msg().value.ue_cap_info_ind().protocol_ies;
@@ -1487,6 +1665,30 @@ bool s1ap::ue::send_ue_cap_info_indication(srsran::unique_byte_buffer_t ue_radio
   return s1ap_ptr->sctp_send_s1ap_pdu(tx_pdu, ctxt.rnti, "UECapabilityInfoIndication");
 }
 
+void s1ap::ue::set_state(s1ap_proc_id_t        next_state,
+                         const erab_id_list&   erabs_updated,
+                         const erab_item_list& erabs_failed_to_modify)
+{
+  current_state = next_state;
+  updated_erabs.assign(erabs_updated.begin(), erabs_updated.end());
+  failed_cfg_erabs.assign(erabs_failed_to_modify.begin(), erabs_failed_to_modify.end());
+}
+
+void s1ap::ue::get_erab_addr(uint16_t erab_id, transp_addr_t& transp_addr, asn1::fixed_octstring<4, true>& gtpu_teid_id)
+{
+  uint32_t teidin = 0;
+  int      ret    = s1ap_ptr->rrc->get_erab_addr_in(ctxt.rnti, erab_id, transp_addr, teidin);
+  srsran_expect(ret == SRSRAN_SUCCESS, "Invalid E-RAB setup");
+  // Note: RRC does not yet update correctly gtpu transp_addr
+  transp_addr.resize(32);
+  uint8_t addr[4];
+  inet_pton(AF_INET, s1ap_ptr->args.gtp_bind_addr.c_str(), addr);
+  for (uint32_t j = 0; j < 4; ++j) {
+    transp_addr.data()[j] = addr[3 - j];
+  }
+  gtpu_teid_id.from_number(teidin);
+}
+
 /*********************
  * Handover Messages
  ********************/
@@ -1495,7 +1697,8 @@ bool s1ap::send_ho_required(uint16_t                     rnti,
                             uint32_t                     target_eci,
                             srsran::plmn_id_t            target_plmn,
                             srsran::span<uint32_t>       fwd_erabs,
-                            srsran::unique_byte_buffer_t rrc_container)
+                            srsran::unique_byte_buffer_t rrc_container,
+                            bool                         has_direct_fwd_path)
 {
   if (!mme_connected) {
     return false;
@@ -1506,7 +1709,7 @@ bool s1ap::send_ho_required(uint16_t                     rnti,
   }
 
   // launch procedure
-  if (not u->ho_prep_proc.launch(target_eci, target_plmn, fwd_erabs, std::move(rrc_container))) {
+  if (not u->ho_prep_proc.launch(target_eci, target_plmn, fwd_erabs, std::move(rrc_container), has_direct_fwd_path)) {
     logger.error("Failed to initiate an HandoverPreparation procedure for user rnti=0x%x", u->ctxt.rnti);
     return false;
   }
@@ -1596,6 +1799,11 @@ void s1ap::user_list::erase(ue* ue_ptr)
 
 bool s1ap::sctp_send_s1ap_pdu(const asn1::s1ap::s1ap_pdu_c& tx_pdu, uint32_t rnti, const char* procedure_name)
 {
+  if (not mme_connected and rnti != SRSRAN_INVALID_RNTI) {
+    logger.error("Aborting %s for rnti=0x%x. Cause: MME is not connected.", procedure_name, rnti);
+    return false;
+  }
+
   srsran::unique_byte_buffer_t buf = srsran::make_byte_buffer();
   if (buf == nullptr) {
     logger.error("Fatal Error: Couldn't allocate buffer for %s.", procedure_name);
@@ -1739,13 +1947,18 @@ s1ap::ue::ue(s1ap* s1ap_ptr_) : s1ap_ptr(s1ap_ptr_), ho_prep_proc(this), logger(
   ts1_reloc_prep.set(ts1_reloc_prep_timeout_ms,
                      [this](uint32_t tid) { ho_prep_proc.trigger(ho_prep_proc_t::ts1_reloc_prep_expired{}); });
   ts1_reloc_overall = s1ap_ptr->task_sched.get_unique_timer();
-  ts1_reloc_overall.set(ts1_reloc_overall_timeout_ms, [](uint32_t tid) { /* TODO */ });
+  ts1_reloc_overall.set(ts1_reloc_overall_timeout_ms, [this](uint32_t tid) {
+    //> If the UE Context Release procedure is not initiated towards the eNB before the expiry of the timer
+    //  TS1RELOCOverall, the eNB shall request the MME to release the UE context.
+    s1ap_ptr->user_release(ctxt.rnti, asn1::s1ap::cause_radio_network_opts::ts1relocoverall_expiry);
+  });
 }
 
 bool s1ap::ue::send_ho_required(uint32_t                     target_eci,
                                 srsran::plmn_id_t            target_plmn,
                                 srsran::span<uint32_t>       fwd_erabs,
-                                srsran::unique_byte_buffer_t rrc_container)
+                                srsran::unique_byte_buffer_t rrc_container,
+                                bool                         has_direct_fwd_path)
 {
   /*** Setup S1AP PDU as HandoverRequired ***/
   s1ap_pdu_c tx_pdu;
@@ -1753,11 +1966,16 @@ bool s1ap::ue::send_ho_required(uint32_t                     target_eci,
   ho_required_ies_container& container = tx_pdu.init_msg().value.ho_required().protocol_ies;
 
   /*** fill HO Required message ***/
-  container.enb_ue_s1ap_id.value                        = ctxt.enb_ue_s1ap_id;
-  container.mme_ue_s1ap_id.value                        = ctxt.mme_ue_s1ap_id.value();
-  container.direct_forwarding_path_availability_present = false;                // NOTE: X2 for fwd path not supported
+  container.enb_ue_s1ap_id.value                  = ctxt.enb_ue_s1ap_id;
+  container.mme_ue_s1ap_id.value                  = ctxt.mme_ue_s1ap_id.value();
   container.handov_type.value.value               = handov_type_opts::intralte; // NOTE: only intra-LTE HO supported
-  container.cause.value.set_radio_network().value = cause_radio_network_opts::s1_intra_sys_ho_triggered;
+  container.cause.value.set_radio_network().value = cause_radio_network_opts::ho_desirable_for_radio_reason;
+
+  container.direct_forwarding_path_availability_present = has_direct_fwd_path;
+  if (container.direct_forwarding_path_availability_present) {
+    container.direct_forwarding_path_availability.value.value =
+        asn1::s1ap::direct_forwarding_path_availability_opts::direct_path_available;
+  }
 
   /*** set the target eNB ***/
   container.csg_id_present           = false; // NOTE: CSG/hybrid target cell not supported
@@ -1773,14 +1991,14 @@ bool s1ap::ue::send_ho_required(uint32_t                     target_eci,
   // NOTE: Only HO to different Macro eNB is supported.
   auto& macroenb = targetenb.global_enb_id.enb_id.set_macro_enb_id();
   target_plmn.to_s1ap_plmn_bytes(targetenb.global_enb_id.plm_nid.data());
-  macroenb.from_number(target_eci >> 8u);
+  macroenb.from_number(target_eci >> 8U);
 
   /*** fill the transparent container ***/
   container.source_to_target_transparent_container_secondary_present = false;
   sourceenb_to_targetenb_transparent_container_s transparent_cntr;
-  transparent_cntr.erab_info_list_present               = true;  // TODO: CHECK
   transparent_cntr.subscriber_profile_idfor_rfp_present = false; // TODO: CHECK
 
+  transparent_cntr.erab_info_list_present = true;
   transparent_cntr.erab_info_list.resize(fwd_erabs.size());
   for (uint32_t i = 0; i < fwd_erabs.size(); ++i) {
     transparent_cntr.erab_info_list[i].load_info_obj(ASN1_S1AP_ID_ERAB_INFO_LIST_ITEM);
diff --git a/srsenb/test/common/dummy_classes.h b/srsenb/test/common/dummy_classes.h
index 4d54ad7dc..f8e1cf5b5 100644
--- a/srsenb/test/common/dummy_classes.h
+++ b/srsenb/test/common/dummy_classes.h
@@ -113,14 +113,14 @@ public:
   void write_pdu(uint16_t rnti, srsran::unique_byte_buffer_t pdu) override {}
   bool user_exists(uint16_t rnti) override { return true; }
   bool user_release(uint16_t rnti, asn1::s1ap::cause_radio_network_e cause_radio) override { return true; }
-  void ue_ctxt_setup_complete(uint16_t rnti, const asn1::s1ap::init_context_setup_resp_s& res) override {}
-  void ue_erab_setup_complete(uint16_t rnti, const asn1::s1ap::erab_setup_resp_s& res) override {}
+  void ue_ctxt_setup_complete(uint16_t rnti) override {}
   bool is_mme_connected() override { return true; }
   bool send_ho_required(uint16_t                     rnti,
                         uint32_t                     target_eci,
                         srsran::plmn_id_t            target_plmn,
                         srsran::span<uint32_t>       fwd_erabs,
-                        srsran::unique_byte_buffer_t rrc_container) override
+                        srsran::unique_byte_buffer_t rrc_container,
+                        bool                         has_direct_fwd_path) override
   {
     return true;
   }
@@ -132,13 +132,14 @@ public:
                        uint16_t                                       rnti,
                        uint32_t                                       enb_cc_idx,
                        srsran::unique_byte_buffer_t                   ho_cmd,
-                       srsran::span<asn1::s1ap::erab_admitted_item_s> admitted_bearers) override
+                       srsran::span<asn1::s1ap::erab_admitted_item_s> admitted_bearers,
+                       srsran::const_span<asn1::s1ap::erab_item_s>    not_admitted_bearers) override
   {
     return true;
   }
   void send_ho_notify(uint16_t rnti, uint64_t target_eci) override {}
 
-  void send_ho_cancel(uint16_t rnti) override {}
+  void send_ho_cancel(uint16_t rnti, const asn1::s1ap::cause_c& cause) override {}
 
   bool release_erabs(uint16_t rnti, const std::vector<uint16_t>& erabs_successfully_released) override { return true; }
   bool send_ue_cap_info_indication(uint16_t rnti, const srsran::unique_byte_buffer_t ue_radio_cap) override
@@ -176,31 +177,47 @@ public:
   void release_ue(uint16_t rnti) override {}
   bool setup_ue_ctxt(uint16_t rnti, const asn1::s1ap::init_context_setup_request_s& msg) override { return true; }
   bool modify_ue_ctxt(uint16_t rnti, const asn1::s1ap::ue_context_mod_request_s& msg) override { return true; }
-  bool setup_ue_erabs(uint16_t rnti, const asn1::s1ap::erab_setup_request_s& msg) override { return true; }
-  void modify_erabs(uint16_t                                 rnti,
-                    const asn1::s1ap::erab_modify_request_s& msg,
-                    std::vector<uint16_t>*                   erabs_modified,
-                    std::vector<uint16_t>*                   erabs_failed_to_modify) override
-  {}
+  int  get_erab_addr_in(uint16_t rnti, uint16_t erab_id, transp_addr_t& addr_in, uint32_t& teid_in) const override
+  {
+    return SRSRAN_SUCCESS;
+  }
+  void set_aggregate_max_bitrate(uint16_t rnti, const asn1::s1ap::ue_aggregate_maximum_bitrate_s& bitrate) override {}
+  int  setup_erab(uint16_t                                           rnti,
+                  uint16_t                                           erab_id,
+                  const asn1::s1ap::erab_level_qos_params_s&         qos_params,
+                  srsran::const_byte_span                            nas_pdu,
+                  const asn1::bounded_bitstring<1, 160, true, true>& addr,
+                  uint32_t                                           gtpu_teid_out,
+                  asn1::s1ap::cause_c&                               cause) override
+  {
+    return SRSRAN_SUCCESS;
+  }
+  int modify_erab(uint16_t                                   rnti,
+                  uint16_t                                   erab_id,
+                  const asn1::s1ap::erab_level_qos_params_s& qos_params,
+                  srsran::const_byte_span                    nas_pdu,
+                  asn1::s1ap::cause_c&                       cause) override
+  {
+    return SRSRAN_SUCCESS;
+  }
+  bool has_erab(uint16_t rnti, uint32_t erab_id) const override { return true; }
   bool release_erabs(uint32_t rnti) override { return true; }
-  void release_erabs(uint32_t                              rnti,
-                     const asn1::s1ap::erab_release_cmd_s& msg,
-                     std::vector<uint16_t>*                erabs_released,
-                     std::vector<uint16_t>*                erabs_failed_to_release) override
-  {}
+  int  release_erab(uint16_t rnti, uint16_t erab_id) override { return SRSRAN_SUCCESS; }
   void add_paging_id(uint32_t ueid, const asn1::s1ap::ue_paging_id_c& ue_paging_id) override {}
   void ho_preparation_complete(uint16_t                     rnti,
-                               bool                         is_success,
+                               ho_prep_result               result,
                                const asn1::s1ap::ho_cmd_s&  msg,
                                srsran::unique_byte_buffer_t container) override
   {}
-  uint16_t
-  start_ho_ue_resource_alloc(const asn1::s1ap::ho_request_s&                                   msg,
-                             const asn1::s1ap::sourceenb_to_targetenb_transparent_container_s& container) override
+  uint16_t start_ho_ue_resource_alloc(const asn1::s1ap::ho_request_s&                                   msg,
+                                      const asn1::s1ap::sourceenb_to_targetenb_transparent_container_s& container,
+                                      asn1::s1ap::cause_c& failure_cause) override
   {
     return SRSRAN_INVALID_RNTI;
   }
   void set_erab_status(uint16_t rnti, const asn1::s1ap::bearers_subject_to_status_transfer_list_l& erabs) override {}
+
+  int notify_ue_erab_updates(uint16_t rnti, srsran::const_byte_span nas_pdu) override { return SRSRAN_SUCCESS; }
 };
 
 } // namespace srsenb
diff --git a/srsenb/test/mac/sched_benchmark.cc b/srsenb/test/mac/sched_benchmark.cc
index 75d5aa9c0..c0b76a46f 100644
--- a/srsenb/test/mac/sched_benchmark.cc
+++ b/srsenb/test/mac/sched_benchmark.cc
@@ -22,6 +22,7 @@
 #include "sched_test_common.h"
 #include "srsenb/hdr/stack/mac/sched.h"
 #include "srsran/adt/accumulators.h"
+#include "srsran/common/lte_common.h"
 #include <chrono>
 
 namespace srsenb {
@@ -35,7 +36,7 @@ struct run_params {
 };
 
 struct run_params_range {
-  std::vector<uint32_t>    nof_prbs     = {6, 15, 25, 50, 75, 100};
+  std::vector<uint32_t>    nof_prbs{srsran::lte_cell_nof_prbs.begin(), srsran::lte_cell_nof_prbs.end()};
   std::vector<uint32_t>    nof_ues      = {1, 2, 5};
   uint32_t                 nof_ttis     = 10000;
   std::vector<uint32_t>    cqi          = {5, 10, 15};
diff --git a/srsenb/test/mac/sched_ca_test.cc b/srsenb/test/mac/sched_ca_test.cc
index 57a6d05bf..cce02feea 100644
--- a/srsenb/test/mac/sched_ca_test.cc
+++ b/srsenb/test/mac/sched_ca_test.cc
@@ -22,6 +22,7 @@
 #include "sched_test_common.h"
 #include "sched_test_utils.h"
 #include "srsenb/hdr/stack/mac/sched.h"
+#include "srsran/common/lte_common.h"
 #include "srsran/mac/pdu.h"
 
 using namespace srsenb;
@@ -88,10 +89,8 @@ struct test_scell_activation_params {
 
 int test_scell_activation(uint32_t sim_number, test_scell_activation_params params)
 {
-  std::array<uint32_t, 6> prb_list{6, 15, 25, 50, 75, 100};
-
   /* Simulation Configuration Arguments */
-  uint32_t nof_prb   = prb_list[std::uniform_int_distribution<uint32_t>{0, 5}(get_rand_gen())];
+  uint32_t nof_prb   = srsran::lte_cell_nof_prbs[std::uniform_int_distribution<uint32_t>{0, 5}(get_rand_gen())];
   uint32_t nof_ccs   = 2;
   uint32_t start_tti = 0; // rand_int(0, 10240);
 
diff --git a/srsenb/test/mac/sched_dci_test.cc b/srsenb/test/mac/sched_dci_test.cc
index 704aaa76b..cef9d6497 100644
--- a/srsenb/test/mac/sched_dci_test.cc
+++ b/srsenb/test/mac/sched_dci_test.cc
@@ -22,6 +22,7 @@
 #include "sched_test_utils.h"
 #include "srsenb/hdr/stack/mac/sched_common.h"
 #include "srsenb/hdr/stack/mac/sched_phy_ch/sched_dci.h"
+#include "srsran/common/lte_common.h"
 #include "srsran/common/test_common.h"
 
 namespace srsenb {
@@ -187,10 +188,9 @@ int test_mcs_lookup_specific()
 /// Verify consistency of MCS,TBS computation for different permutations of banwidths, grant sizes, cqi, max_mcs
 int test_mcs_tbs_consistency_all()
 {
-  uint32_t                      prb_list[] = {6, 15, 25, 50, 75, 100};
   sched_interface::sched_args_t sched_args = {};
 
-  for (auto& nof_prb_cell : prb_list) {
+  for (auto& nof_prb_cell : srsran::lte_cell_nof_prbs) {
     sched_interface::cell_cfg_t cell_cfg    = generate_default_cell_cfg(nof_prb_cell);
     sched_cell_params_t         cell_params = {};
     cell_params.set_cfg(0, cell_cfg, sched_args);
diff --git a/srsenb/test/mac/sched_grid_test.cc b/srsenb/test/mac/sched_grid_test.cc
index 6e857f754..fbd4a170c 100644
--- a/srsenb/test/mac/sched_grid_test.cc
+++ b/srsenb/test/mac/sched_grid_test.cc
@@ -21,13 +21,13 @@
 
 #include "sched_test_common.h"
 #include "srsenb/hdr/stack/mac/sched_grid.h"
+#include "srsran/common/lte_common.h"
 #include "srsran/common/test_common.h"
 
 using namespace srsenb;
 const uint32_t seed = std::chrono::system_clock::now().time_since_epoch().count();
 
-const uint32_t                PCell_IDX = 0;
-const std::array<uint32_t, 6> prb_list  = {6, 15, 25, 50, 75, 100};
+const uint32_t PCell_IDX = 0;
 
 uint32_t get_aggr_level(sched_ue& sched_ue, uint32_t enb_cc_idx, const std::vector<sched_cell_params_t>& cell_params)
 {
@@ -42,7 +42,7 @@ int test_pdcch_one_ue()
   using rand_uint           = std::uniform_int_distribution<uint32_t>;
   const uint32_t ENB_CC_IDX = 0;
   // Params
-  uint32_t          nof_prb = prb_list[rand_uint{0, 5}(get_rand_gen())];
+  uint32_t          nof_prb = srsran::lte_cell_nof_prbs[rand_uint{0, 5}(get_rand_gen())];
   uint16_t          rnti    = rand_uint{70, 120}(get_rand_gen());
   srsran::tti_point start_tti{rand_uint{0, 10240}(get_rand_gen())};
   uint32_t          nof_ttis = 100;
diff --git a/srsenb/test/mac/sched_lc_ch_test.cc b/srsenb/test/mac/sched_lc_ch_test.cc
index 8c783b04c..9b44c0dcb 100644
--- a/srsenb/test/mac/sched_lc_ch_test.cc
+++ b/srsenb/test/mac/sched_lc_ch_test.cc
@@ -87,47 +87,47 @@ int test_lc_ch_pbr_infinity()
 {
   srsenb::lch_ue_manager lch_handler;
 
-  srsenb::sched_interface::ue_cfg_t ue_cfg        = generate_default_ue_cfg();
-  ue_cfg                                          = generate_setup_ue_cfg(ue_cfg);
-  ue_cfg.ue_bearers[srsenb::RB_ID_SRB1]           = {};
-  ue_cfg.ue_bearers[srsenb::RB_ID_SRB1].direction = sched_interface::ue_bearer_cfg_t::BOTH;
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB1]           = {};
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB1].direction = sched_interface::ue_bearer_cfg_t::BOTH;
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB1].priority  = 5;
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB2]           = {};
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB2].direction = sched_interface::ue_bearer_cfg_t::BOTH;
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB2].priority  = 3;
+  srsenb::sched_interface::ue_cfg_t ue_cfg                  = generate_default_ue_cfg();
+  ue_cfg                                                    = generate_setup_ue_cfg(ue_cfg);
+  ue_cfg.ue_bearers[srb_to_lcid((lte_srb::srb1))]           = {};
+  ue_cfg.ue_bearers[srb_to_lcid((lte_srb::srb1))].direction = sched_interface::ue_bearer_cfg_t::BOTH;
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb1))]           = {};
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb1))].direction = sched_interface::ue_bearer_cfg_t::BOTH;
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb1))].priority  = 5;
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb2))]           = {};
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb2))].direction = sched_interface::ue_bearer_cfg_t::BOTH;
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb2))].priority  = 3;
 
   lch_handler.set_cfg(ue_cfg);
   lch_handler.new_tti();
 
-  lch_handler.dl_buffer_state(srsenb::RB_ID_SRB1, 50000, 10000);
-  lch_handler.dl_buffer_state(srsenb::RB_ID_DRB1, 5000, 10000);
-  lch_handler.dl_buffer_state(srsenb::RB_ID_DRB2, 5000, 10000);
+  lch_handler.dl_buffer_state(srb_to_lcid(lte_srb::srb1), 50000, 10000);
+  lch_handler.dl_buffer_state(drb_to_lcid(lte_drb::drb1), 5000, 10000);
+  lch_handler.dl_buffer_state(drb_to_lcid(lte_drb::drb2), 5000, 10000);
 
   // TEST1 - retx of SRB1 is prioritized. Do not transmit other bearers until there are no SRB1 retxs
-  int nof_pending_bytes = lch_handler.get_dl_retx(srsenb::RB_ID_SRB1);
-  TESTASSERT(test_retx_until_empty(lch_handler, srsenb::RB_ID_SRB1, 500) == nof_pending_bytes);
+  int nof_pending_bytes = lch_handler.get_dl_retx(srb_to_lcid(lte_srb::srb1));
+  TESTASSERT(test_retx_until_empty(lch_handler, srb_to_lcid(lte_srb::srb1), 500) == nof_pending_bytes);
 
   // TEST2 - the DRB2 has lower prio level than SRB1, but has retxs
-  nof_pending_bytes = lch_handler.get_dl_retx(srsenb::RB_ID_DRB2);
-  TESTASSERT(test_retx_until_empty(lch_handler, srsenb::RB_ID_DRB2, 500) == nof_pending_bytes);
+  nof_pending_bytes = lch_handler.get_dl_retx(drb_to_lcid(lte_drb::drb2));
+  TESTASSERT(test_retx_until_empty(lch_handler, drb_to_lcid(lte_drb::drb2), 500) == nof_pending_bytes);
 
   // TEST3 - the DRB1 has lower prio level, but has retxs
-  nof_pending_bytes = lch_handler.get_dl_retx(srsenb::RB_ID_DRB1);
-  TESTASSERT(test_retx_until_empty(lch_handler, srsenb::RB_ID_DRB1, 500) == nof_pending_bytes);
+  nof_pending_bytes = lch_handler.get_dl_retx(drb_to_lcid(lte_drb::drb1));
+  TESTASSERT(test_retx_until_empty(lch_handler, drb_to_lcid(lte_drb::drb1), 500) == nof_pending_bytes);
 
   // TEST4 - The SRB1 newtx buffer is emptied before other bearers newtxs
-  nof_pending_bytes = lch_handler.get_dl_tx(srsenb::RB_ID_SRB1);
-  TESTASSERT(test_newtx_until_empty(lch_handler, srsenb::RB_ID_SRB1, 500) == nof_pending_bytes);
+  nof_pending_bytes = lch_handler.get_dl_tx(srb_to_lcid(lte_srb::srb1));
+  TESTASSERT(test_newtx_until_empty(lch_handler, srb_to_lcid(lte_srb::srb1), 500) == nof_pending_bytes);
 
   // TEST5 - The DRB2 newtx buffer is emptied before DRB1 newtxs
-  nof_pending_bytes = lch_handler.get_dl_tx(srsenb::RB_ID_DRB2);
-  TESTASSERT(test_newtx_until_empty(lch_handler, srsenb::RB_ID_DRB2, 500) == nof_pending_bytes);
+  nof_pending_bytes = lch_handler.get_dl_tx(drb_to_lcid(lte_drb::drb2));
+  TESTASSERT(test_newtx_until_empty(lch_handler, drb_to_lcid(lte_drb::drb2), 500) == nof_pending_bytes);
 
   // TEST6 - The DRB1 buffer is emptied
-  nof_pending_bytes = lch_handler.get_dl_tx(srsenb::RB_ID_DRB1);
-  TESTASSERT(test_newtx_until_empty(lch_handler, srsenb::RB_ID_DRB1, 500) == nof_pending_bytes);
+  nof_pending_bytes = lch_handler.get_dl_tx(drb_to_lcid(lte_drb::drb1));
+  TESTASSERT(test_newtx_until_empty(lch_handler, drb_to_lcid(lte_drb::drb1), 500) == nof_pending_bytes);
 
   return SRSRAN_SUCCESS;
 }
@@ -137,20 +137,20 @@ int test_lc_ch_pbr_finite()
   srsenb::lch_ue_manager          lch_handler;
   sched_interface::dl_sched_pdu_t pdu;
 
-  srsenb::sched_interface::ue_cfg_t ue_cfg        = generate_default_ue_cfg();
-  ue_cfg                                          = generate_setup_ue_cfg(ue_cfg);
-  ue_cfg.ue_bearers[srsenb::RB_ID_SRB1]           = {};
-  ue_cfg.ue_bearers[srsenb::RB_ID_SRB1].direction = sched_interface::ue_bearer_cfg_t::BOTH;
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB1]           = {};
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB1].direction = sched_interface::ue_bearer_cfg_t::BOTH;
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB1].pbr       = 256; // kBps
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB1].bsd       = 50;  // msec
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB1].priority  = 5;
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB2]           = {};
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB2].direction = sched_interface::ue_bearer_cfg_t::BOTH;
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB2].pbr       = 8;  // kBps
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB2].bsd       = 50; // msec
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB2].priority  = 3;
+  srsenb::sched_interface::ue_cfg_t ue_cfg                  = generate_default_ue_cfg();
+  ue_cfg                                                    = generate_setup_ue_cfg(ue_cfg);
+  ue_cfg.ue_bearers[srb_to_lcid((lte_srb::srb1))]           = {};
+  ue_cfg.ue_bearers[srb_to_lcid((lte_srb::srb1))].direction = sched_interface::ue_bearer_cfg_t::BOTH;
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb1))]           = {};
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb1))].direction = sched_interface::ue_bearer_cfg_t::BOTH;
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb1))].pbr       = 256; // kBps
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb1))].bsd       = 50;  // msec
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb1))].priority  = 5;
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb2))]           = {};
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb2))].direction = sched_interface::ue_bearer_cfg_t::BOTH;
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb2))].pbr       = 8;  // kBps
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb2))].bsd       = 50; // msec
+  ue_cfg.ue_bearers[drb_to_lcid((lte_drb::drb2))].priority  = 3;
 
   lch_handler.set_cfg(ue_cfg);
   for (uint32_t i = 0; i < 50; ++i) {
@@ -158,41 +158,41 @@ int test_lc_ch_pbr_finite()
   }
   // Bj={0, infinity, 0, 12800, 400}
 
-  lch_handler.dl_buffer_state(srsenb::RB_ID_SRB1, 50000, 1000);
-  lch_handler.dl_buffer_state(srsenb::RB_ID_DRB1, 50000, 1000);
-  lch_handler.dl_buffer_state(srsenb::RB_ID_DRB2, 50000, 0);
+  lch_handler.dl_buffer_state(srb_to_lcid(lte_srb::srb1), 50000, 1000);
+  lch_handler.dl_buffer_state(drb_to_lcid(lte_drb::drb1), 50000, 1000);
+  lch_handler.dl_buffer_state(drb_to_lcid(lte_drb::drb2), 50000, 0);
 
   // TEST1 - SRB1 retxs are emptied first
-  int nof_pending_bytes = lch_handler.get_dl_retx(srsenb::RB_ID_SRB1);
-  TESTASSERT(test_retx_until_empty(lch_handler, srsenb::RB_ID_SRB1, 500) == nof_pending_bytes);
+  int nof_pending_bytes = lch_handler.get_dl_retx(srb_to_lcid(lte_srb::srb1));
+  TESTASSERT(test_retx_until_empty(lch_handler, srb_to_lcid(lte_srb::srb1), 500) == nof_pending_bytes);
 
   // TEST2 - DRB1 retxs are emptied
-  nof_pending_bytes = lch_handler.get_dl_retx(srsenb::RB_ID_DRB1);
-  TESTASSERT(test_retx_until_empty(lch_handler, srsenb::RB_ID_DRB1, 500) == nof_pending_bytes);
+  nof_pending_bytes = lch_handler.get_dl_retx(drb_to_lcid(lte_drb::drb1));
+  TESTASSERT(test_retx_until_empty(lch_handler, drb_to_lcid(lte_drb::drb1), 500) == nof_pending_bytes);
 
   // TEST3 - SRB1 newtxs are emptied (PBR==infinity)
-  nof_pending_bytes = lch_handler.get_dl_tx(srsenb::RB_ID_SRB1);
-  TESTASSERT(test_newtx_until_empty(lch_handler, srsenb::RB_ID_SRB1, 500) == nof_pending_bytes);
+  nof_pending_bytes = lch_handler.get_dl_tx(srb_to_lcid(lte_srb::srb1));
+  TESTASSERT(test_newtx_until_empty(lch_handler, srb_to_lcid(lte_srb::srb1), 500) == nof_pending_bytes);
 
   // TEST4 - DRB2 has higher priority so it gets allocated until Bj <= 0
-  TESTASSERT(test_pdu_alloc_successful(lch_handler, pdu, srsenb::RB_ID_DRB2, 200) == SRSRAN_SUCCESS);
+  TESTASSERT(test_pdu_alloc_successful(lch_handler, pdu, drb_to_lcid(lte_drb::drb2), 200) == SRSRAN_SUCCESS);
   // Bj={0, infinity, 0, 12800, 200}
-  TESTASSERT(test_pdu_alloc_successful(lch_handler, pdu, srsenb::RB_ID_DRB2, 600) == SRSRAN_SUCCESS);
+  TESTASSERT(test_pdu_alloc_successful(lch_handler, pdu, drb_to_lcid(lte_drb::drb2), 600) == SRSRAN_SUCCESS);
   // Bj={0, infinity, 0, 256000, -400}
 
   // TEST5 - DRB1 has lower prio, but DRB2 Bj <= 0.
   for (uint32_t i = 0; i < 50; ++i) {
     lch_handler.new_tti();
-    TESTASSERT(test_pdu_alloc_successful(lch_handler, pdu, srsenb::RB_ID_DRB1, 50) == SRSRAN_SUCCESS);
+    TESTASSERT(test_pdu_alloc_successful(lch_handler, pdu, drb_to_lcid(lte_drb::drb1), 50) == SRSRAN_SUCCESS);
   }
 
   // TEST6 - new tti restores DRB2 Bj>=0, and DRB2 gets allocated
   lch_handler.new_tti();
   // Bj={0, infinity, 0, 256000, 8}
-  TESTASSERT(test_pdu_alloc_successful(lch_handler, pdu, srsenb::RB_ID_DRB2, 50) == SRSRAN_SUCCESS);
+  TESTASSERT(test_pdu_alloc_successful(lch_handler, pdu, drb_to_lcid(lte_drb::drb2), 50) == SRSRAN_SUCCESS);
   // Bj={0, infinity, 0, 256000, -42}
   lch_handler.new_tti();
-  TESTASSERT(test_pdu_alloc_successful(lch_handler, pdu, srsenb::RB_ID_DRB1, 50) == SRSRAN_SUCCESS);
+  TESTASSERT(test_pdu_alloc_successful(lch_handler, pdu, drb_to_lcid(lte_drb::drb1), 50) == SRSRAN_SUCCESS);
 
   return SRSRAN_SUCCESS;
 }
diff --git a/srsenb/test/mac/sched_sim_ue.cc b/srsenb/test/mac/sched_sim_ue.cc
index e7acd307d..c0d5960e9 100644
--- a/srsenb/test/mac/sched_sim_ue.cc
+++ b/srsenb/test/mac/sched_sim_ue.cc
@@ -405,7 +405,7 @@ int sched_sim_base::apply_tti_events(sim_ue_ctxt_t& ue_ctxt, const ue_tti_events
       sched_interface::ue_cfg_t ue_cfg = generate_setup_ue_cfg(final_ue_cfg[ue_ctxt.rnti]);
       TESTASSERT(ue_recfg(ue_ctxt.rnti, ue_cfg) == SRSRAN_SUCCESS);
 
-      uint32_t lcid = RB_ID_SRB0; // Use SRB0 to schedule Msg4
+      uint32_t lcid = srb_to_lcid(lte_srb::srb0); // Use SRB0 to schedule Msg4
       TESTASSERT(sched_ptr->dl_rlc_buffer_state(ue_ctxt.rnti, lcid, 50, 0) == SRSRAN_SUCCESS);
       TESTASSERT(sched_ptr->dl_mac_buffer_state(ue_ctxt.rnti, (uint32_t)srsran::dl_sch_lcid::CON_RES_ID, 1) ==
                  SRSRAN_SUCCESS);
diff --git a/srsenb/test/mac/sched_test_common.cc b/srsenb/test/mac/sched_test_common.cc
index 9a026229f..c0082212b 100644
--- a/srsenb/test/mac/sched_test_common.cc
+++ b/srsenb/test/mac/sched_test_common.cc
@@ -239,7 +239,7 @@ int common_sched_tester::process_tti_events(const tti_ev& tti_ev)
       const auto& ue_sim_ctxt = user->get_ctxt();
       if (ue_ev.buffer_ev->dl_data > 0 and ue_sim_ctxt.conres_rx) {
         // If Msg4 has already been tx and there DL data to transmit
-        uint32_t lcid                = RB_ID_DRB1;
+        uint32_t lcid                = drb_to_lcid(lte_drb::drb1);
         uint32_t pending_dl_new_data = ue_db[ue_ev.rnti]->get_pending_dl_rlc_data();
         // DRB is set. Update DL buffer
         uint32_t tot_dl_data = pending_dl_new_data + ue_ev.buffer_ev->dl_data; // TODO: derive pending based on rx
diff --git a/srsenb/test/mac/sched_test_common.h b/srsenb/test/mac/sched_test_common.h
index 44a5f7890..75492b9fe 100644
--- a/srsenb/test/mac/sched_test_common.h
+++ b/srsenb/test/mac/sched_test_common.h
@@ -43,7 +43,9 @@ struct rrc_dummy : public rrc_interface_mac {
 public:
   int      add_user(uint16_t rnti, const sched_interface::ue_cfg_t& init_ue_cfg) { return SRSRAN_SUCCESS; }
   void     upd_user(uint16_t new_rnti, uint16_t old_rnti) {}
-  void     set_activity_user(uint16_t rnti, bool ack_info) {}
+  void     set_activity_user(uint16_t rnti) {}
+  void     set_radiolink_ul_state(uint16_t rnti, bool crc_res) {}
+  void     set_radiolink_dl_state(uint16_t rnti, bool crc_res) {}
   bool     is_paging_opportunity(uint32_t tti, uint32_t* payload_len) { return false; }
   uint8_t* read_pdu_bcch_dlsch(const uint8_t enb_cc_idx, const uint32_t sib_index) { return nullptr; }
 };
diff --git a/srsenb/test/mac/sched_test_rand.cc b/srsenb/test/mac/sched_test_rand.cc
index 062ab30b4..24ea9acbe 100644
--- a/srsenb/test/mac/sched_test_rand.cc
+++ b/srsenb/test/mac/sched_test_rand.cc
@@ -34,9 +34,10 @@
 #include "sched_common_test_suite.h"
 #include "sched_test_common.h"
 #include "sched_test_utils.h"
+#include "srsran/common/lte_common.h"
 #include "srsran/common/test_common.h"
 
-using srsran::tti_point;
+namespace srsenb {
 
 uint32_t seed = std::chrono::system_clock::now().time_since_epoch().count();
 
@@ -242,7 +243,7 @@ int sched_tester::update_ue_stats()
   return SRSRAN_SUCCESS;
 }
 
-int test_scheduler_rand(sched_sim_events sim)
+int test_scheduler_rand(srsenb::sched_sim_events sim)
 {
   // Create classes
   sched_tester  tester;
@@ -273,7 +274,7 @@ sched_sim_events rand_sim_params(uint32_t nof_ttis)
   std::uniform_int_distribution<>         connection_dur_dist(min_conn_dur, max_conn_dur);
   std::uniform_int_distribution<uint32_t> dist_prb_idx(0, 5);
   uint32_t                                prb_idx = dist_prb_idx(srsenb::get_rand_gen());
-  uint32_t                                nof_prb = std::array<uint32_t, 6>({6, 15, 25, 50, 75, 100})[prb_idx];
+  uint32_t                                nof_prb = srsran::lte_cell_nof_prbs[prb_idx];
   printf("Number of PRBs is %u\n", nof_prb);
 
   sched_sim_event_generator generator;
@@ -331,11 +332,13 @@ sched_sim_events rand_sim_params(uint32_t nof_ttis)
   return sim_gen;
 }
 
+} // namespace srsenb
+
 int main()
 {
   // Setup seed
-  srsenb::set_randseed(seed);
-  printf("This is the chosen seed: %u\n", seed);
+  srsenb::set_randseed(srsenb::seed);
+  printf("This is the chosen seed: %u\n", srsenb::seed);
 
   // Setup the log spy to intercept error and warning log entries.
   if (!srslog::install_custom_sink(
@@ -359,11 +362,11 @@ int main()
 
   uint32_t N_runs = 1, nof_ttis = 10240 + 10;
 
-  sched_diagnostic_printer printer(*spy);
+  srsenb::sched_diagnostic_printer printer(*spy);
   for (uint32_t n = 0; n < N_runs; ++n) {
     printf("Sim run number: %u\n", n + 1);
-    sched_sim_events sim = rand_sim_params(nof_ttis);
-    TESTASSERT(test_scheduler_rand(std::move(sim)) == SRSRAN_SUCCESS);
+    srsenb::sched_sim_events sim = srsenb::rand_sim_params(nof_ttis);
+    TESTASSERT(srsenb::test_scheduler_rand(std::move(sim)) == SRSRAN_SUCCESS);
   }
 
   return 0;
diff --git a/srsenb/test/mac/sched_test_utils.h b/srsenb/test/mac/sched_test_utils.h
index 5a8a2f935..9832545c8 100644
--- a/srsenb/test/mac/sched_test_utils.h
+++ b/srsenb/test/mac/sched_test_utils.h
@@ -30,6 +30,8 @@
 #include <chrono>
 #include <unordered_map>
 
+namespace srsenb {
+
 /*****************************
  * Setup Sched Configuration
  ****************************/
@@ -72,15 +74,15 @@ inline srsenb::sched_interface::ue_cfg_t generate_default_ue_cfg()
 
   ue_cfg.maxharq_tx = 5;
   ue_cfg.supported_cc_list.resize(1);
-  ue_cfg.supported_cc_list[0].aperiodic_cqi_period = 40;
-  ue_cfg.supported_cc_list[0].enb_cc_idx           = 0;
-  ue_cfg.supported_cc_list[0].active               = true;
-  ue_cfg.supported_cc_list[0].dl_cfg.tm            = SRSRAN_TM1;
-  ue_cfg.ue_bearers[srsenb::RB_ID_SRB0].direction  = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
-  ue_cfg.ue_bearers[srsenb::RB_ID_SRB1].direction  = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
-  ue_cfg.ue_bearers[srsenb::RB_ID_SRB2].direction  = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB1].direction  = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB1].group      = 1;
+  ue_cfg.supported_cc_list[0].aperiodic_cqi_period        = 40;
+  ue_cfg.supported_cc_list[0].enb_cc_idx                  = 0;
+  ue_cfg.supported_cc_list[0].active                      = true;
+  ue_cfg.supported_cc_list[0].dl_cfg.tm                   = SRSRAN_TM1;
+  ue_cfg.ue_bearers[srb_to_lcid(lte_srb::srb0)].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
+  ue_cfg.ue_bearers[srb_to_lcid(lte_srb::srb1)].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
+  ue_cfg.ue_bearers[srb_to_lcid(lte_srb::srb2)].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
+  ue_cfg.ue_bearers[drb_to_lcid(lte_drb::drb1)].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
+  ue_cfg.ue_bearers[drb_to_lcid(lte_drb::drb1)].group     = 1;
 
   ue_cfg.pucch_cfg.sr_configured = true;
   ue_cfg.pucch_cfg.I_sr          = 15; // periodicity of 20 msec
@@ -93,18 +95,18 @@ inline srsenb::sched_interface::ue_cfg_t generate_default_ue_cfg2()
 {
   srsenb::sched_interface::ue_cfg_t ue_cfg = generate_default_ue_cfg();
 
-  ue_cfg.ue_bearers[srsenb::RB_ID_SRB1].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
-  ue_cfg.ue_bearers[srsenb::RB_ID_SRB2].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB1].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
-  ue_cfg.ue_bearers[srsenb::RB_ID_DRB1].group     = 1;
+  ue_cfg.ue_bearers[srb_to_lcid(lte_srb::srb1)].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
+  ue_cfg.ue_bearers[srb_to_lcid(lte_srb::srb2)].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
+  ue_cfg.ue_bearers[drb_to_lcid(lte_drb::drb1)].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
+  ue_cfg.ue_bearers[drb_to_lcid(lte_drb::drb1)].group     = 1;
 
   return ue_cfg;
 }
 
 inline srsenb::sched_interface::ue_cfg_t generate_rach_ue_cfg(const srsenb::sched_interface::ue_cfg_t& final_cfg)
 {
-  srsenb::sched_interface::ue_cfg_t cfg        = {};
-  cfg.ue_bearers[srsenb::RB_ID_SRB0].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
+  srsenb::sched_interface::ue_cfg_t cfg                = {};
+  cfg.ue_bearers[srb_to_lcid(lte_srb::srb0)].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
   cfg.supported_cc_list.resize(1);
   cfg.supported_cc_list[0].enb_cc_idx = final_cfg.supported_cc_list[0].enb_cc_idx;
   cfg.supported_cc_list[0].active     = true;
@@ -115,10 +117,10 @@ inline srsenb::sched_interface::ue_cfg_t generate_setup_ue_cfg(const srsenb::sch
 {
   srsenb::sched_interface::ue_cfg_t cfg = generate_rach_ue_cfg(final_cfg);
 
-  cfg.maxharq_tx                               = final_cfg.maxharq_tx;
-  cfg.ue_bearers[srsenb::RB_ID_SRB1].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
-  cfg.supported_cc_list[0].dl_cfg.tm           = SRSRAN_TM1;
-  cfg.continuous_pusch                         = final_cfg.continuous_pusch;
+  cfg.maxharq_tx                                       = final_cfg.maxharq_tx;
+  cfg.ue_bearers[srb_to_lcid(lte_srb::srb1)].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
+  cfg.supported_cc_list[0].dl_cfg.tm                   = SRSRAN_TM1;
+  cfg.continuous_pusch                                 = final_cfg.continuous_pusch;
 
   cfg.supported_cc_list[0].dl_cfg.cqi_report    = final_cfg.supported_cc_list[0].dl_cfg.cqi_report;
   cfg.pucch_cfg                                 = final_cfg.pucch_cfg;
@@ -129,8 +131,8 @@ inline srsenb::sched_interface::ue_cfg_t generate_setup_ue_cfg(const srsenb::sch
 
 inline srsenb::sched_interface::ue_cfg_t generate_reconf_ue_cfg(const srsenb::sched_interface::ue_cfg_t& final_cfg)
 {
-  srsenb::sched_interface::ue_cfg_t cfg = generate_setup_ue_cfg(final_cfg);
-  cfg.ue_bearers[srsenb::RB_ID_SRB2]    = final_cfg.ue_bearers[srsenb::RB_ID_SRB1];
+  srsenb::sched_interface::ue_cfg_t cfg      = generate_setup_ue_cfg(final_cfg);
+  cfg.ue_bearers[srb_to_lcid(lte_srb::srb2)] = final_cfg.ue_bearers[srb_to_lcid(lte_srb::srb1)];
 
   return cfg;
 }
@@ -264,9 +266,11 @@ struct sched_sim_event_generator {
     ue_sim_cfg.ue_cfg = generate_default_ue_cfg();
     user->ue_sim_cfg.reset(new ue_ctxt_test_cfg{ue_sim_cfg});
     // it should by now have a DRB1. Add other DRBs manually
-    user->ue_sim_cfg->ue_cfg.ue_bearers[srsenb::RB_ID_SRB2].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
-    user->ue_sim_cfg->ue_cfg.ue_bearers[srsenb::RB_ID_DRB1].direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
-    user->ue_sim_cfg->ue_cfg.ue_bearers[srsenb::RB_ID_DRB1].group     = 1;
+    user->ue_sim_cfg->ue_cfg.ue_bearers[srb_to_lcid(lte_srb::srb2)].direction =
+        srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
+    user->ue_sim_cfg->ue_cfg.ue_bearers[drb_to_lcid(lte_drb::drb1)].direction =
+        srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
+    user->ue_sim_cfg->ue_cfg.ue_bearers[drb_to_lcid(lte_drb::drb1)].group = 1;
     return user;
   }
 
@@ -311,4 +315,6 @@ private:
   }
 };
 
+} // namespace srsenb
+
 #endif // SRSRAN_SCHED_TEST_UTILS_H
diff --git a/srsenb/test/upper/erab_setup_test.cc b/srsenb/test/upper/erab_setup_test.cc
index 7c1fcae5d..e90a2baf2 100644
--- a/srsenb/test/upper/erab_setup_test.cc
+++ b/srsenb/test/upper/erab_setup_test.cc
@@ -97,14 +97,32 @@ int test_erab_setup(srsran::log_sink_spy& spy, bool qci_exists)
   asn1::cbit_ref bref(byte_buf.msg, byte_buf.N_bytes);
 
   TESTASSERT(s1ap_pdu.unpack(bref) == asn1::SRSASN_SUCCESS);
-  rrc.setup_ue_erabs(rnti, s1ap_pdu.init_msg().value.erab_setup_request());
+  const auto& setupmsg = s1ap_pdu.init_msg().value.erab_setup_request().protocol_ies;
+  if (setupmsg.ueaggregate_maximum_bitrate_present) {
+    rrc.set_aggregate_max_bitrate(rnti, setupmsg.ueaggregate_maximum_bitrate.value);
+  }
+  for (const auto& item : setupmsg.erab_to_be_setup_list_bearer_su_req.value) {
+    const auto&         erab = item.value.erab_to_be_setup_item_bearer_su_req();
+    asn1::s1ap::cause_c cause;
+    int                 ret = rrc.setup_erab(rnti,
+                             erab.erab_id,
+                             erab.erab_level_qos_params,
+                             erab.nas_pdu,
+                             erab.transport_layer_address,
+                             erab.gtp_teid.to_number(),
+                             cause);
+    if (qci_exists) {
+      TESTASSERT(ret == SRSRAN_SUCCESS);
+      TESTASSERT(rrc.has_erab(rnti, erab.erab_id));
+    } else {
+      TESTASSERT(ret != SRSRAN_SUCCESS);
+      TESTASSERT(not rrc.has_erab(rnti, erab.erab_id));
+    }
+  }
 
   if (qci_exists) {
-    // NOTE: It does not add DRB1/ERAB-ID=5 bc that bearer already existed
-    TESTASSERT(s1ap.added_erab_ids.size() == 1);
     TESTASSERT(spy.get_error_counter() == 0);
   } else {
-    TESTASSERT(s1ap.added_erab_ids.empty());
     TESTASSERT(spy.get_error_counter() > 0);
   }
 
diff --git a/srsenb/test/upper/rrc_mobility_test.cc b/srsenb/test/upper/rrc_mobility_test.cc
index e38767d13..1913cc802 100644
--- a/srsenb/test/upper/rrc_mobility_test.cc
+++ b/srsenb/test/upper/rrc_mobility_test.cc
@@ -36,7 +36,9 @@ struct mobility_test_params {
     concurrent_ho,
     ho_prep_failure,
     duplicate_crnti_ce,
-    recover
+    recover,
+    wrong_target_cell,
+    unknown_qci,
   } fail_at;
   const char* to_string()
   {
@@ -53,6 +55,10 @@ struct mobility_test_params {
         return "fail and success";
       case test_event::duplicate_crnti_ce:
         return "duplicate CRNTI CE";
+      case test_event::wrong_target_cell:
+        return "wrong target cell";
+      case test_event::unknown_qci:
+        return "invalid QoS";
       default:
         return "none";
     }
@@ -234,7 +240,7 @@ int test_s1ap_mobility(srsran::log_sink_spy& spy, mobility_test_params test_para
 
   /* Test Case: HandoverPreparation has failed */
   if (test_params.fail_at == mobility_test_params::test_event::ho_prep_failure) {
-    tester.rrc.ho_preparation_complete(tester.rnti, false, {}, nullptr);
+    tester.rrc.ho_preparation_complete(tester.rnti, rrc::ho_prep_result::failure, {}, nullptr);
     //    TESTASSERT(spy.get_error_counter() == 1);
     TESTASSERT(not s1ap.last_enb_status.status_present);
     return SRSRAN_SUCCESS;
@@ -247,7 +253,7 @@ int test_s1ap_mobility(srsran::log_sink_spy& spy, mobility_test_params test_para
                                     0x86, 0x0d, 0x30, 0x00, 0x0b, 0x5a, 0x02, 0x17, 0x86, 0x00, 0x05, 0xa0, 0x20};
   test_helpers::copy_msg_to_buffer(pdu, ho_cmd_rrc_container);
   TESTASSERT(s1ap.last_enb_status.rnti != tester.rnti);
-  tester.rrc.ho_preparation_complete(tester.rnti, true, asn1::s1ap::ho_cmd_s{}, std::move(pdu));
+  tester.rrc.ho_preparation_complete(tester.rnti, rrc::ho_prep_result::success, asn1::s1ap::ho_cmd_s{}, std::move(pdu));
   TESTASSERT(s1ap.last_enb_status.status_present);
   TESTASSERT(spy.get_error_counter() == 0);
   asn1::rrc::dl_dcch_msg_s ho_cmd;
@@ -279,8 +285,22 @@ int test_s1ap_tenb_mobility(mobility_test_params test_params)
   auto& erab   = ho_req.protocol_ies.erab_to_be_setup_list_ho_req.value[0].value.erab_to_be_setup_item_ho_req();
   erab.erab_id = 5;
   erab.erab_level_qos_params.qci = 9;
+  if (test_params.fail_at == mobility_test_params::test_event::unknown_qci) {
+    erab.erab_level_qos_params.qci = 10;
+  }
+  ho_req.protocol_ies.ue_security_cap.value.integrity_protection_algorithms.set(14, true);
   asn1::s1ap::sourceenb_to_targetenb_transparent_container_s container;
   container.target_cell_id.cell_id.from_number(0x19C02);
+  if (test_params.fail_at == mobility_test_params::test_event::wrong_target_cell) {
+    container.target_cell_id.cell_id.from_number(0x19C03);
+  }
+  container.erab_info_list_present = true;
+  container.erab_info_list.resize(1);
+  container.erab_info_list[0].load_info_obj(ASN1_S1AP_ID_ERAB_INFO_LIST_ITEM);
+  container.erab_info_list[0].value.erab_info_list_item().erab_id               = 5;
+  container.erab_info_list[0].value.erab_info_list_item().dl_forwarding_present = true;
+  container.erab_info_list[0].value.erab_info_list_item().dl_forwarding.value =
+      asn1::s1ap::dl_forwarding_opts::dl_forwarding_proposed;
   uint8_t ho_prep_container[] = {
       0x0a, 0x10, 0x0b, 0x81, 0x80, 0x00, 0x01, 0x80, 0x00, 0xf3, 0x02, 0x08, 0x00, 0x00, 0x15, 0x80, 0x00, 0x14,
       0x06, 0xa4, 0x02, 0xf0, 0x04, 0x04, 0xf0, 0x00, 0x14, 0x80, 0x4a, 0x00, 0x00, 0x00, 0x02, 0x12, 0x31, 0xb6,
@@ -290,37 +310,45 @@ int test_s1ap_tenb_mobility(mobility_test_params test_params)
       0x5c, 0xe1, 0x86, 0x35, 0x39, 0x80, 0x0e, 0x06, 0xa4, 0x40, 0x0f, 0x22, 0x78};
   // 0a100b818000018000f3020800001580001406a402f00404f00014804a000000021231b6f83ea06f05e465141d39d0544c00025400200460000000100100c000000000020500041400670dfbc46606500f00080020800c14ca2d5ce1863539800e06a4400f2278
   container.rrc_container.resize(sizeof(ho_prep_container));
-  container.erab_info_list_present = true;
-  container.erab_info_list.resize(1);
-  container.erab_info_list[0].load_info_obj(ASN1_S1AP_ID_ERAB_INFO_LIST_ITEM);
-  container.erab_info_list[0].value.erab_info_list_item().erab_id               = 5;
-  container.erab_info_list[0].value.erab_info_list_item().dl_forwarding_present = true;
-  container.erab_info_list[0].value.erab_info_list_item().dl_forwarding.value =
-      asn1::s1ap::dl_forwarding_opts::dl_forwarding_proposed;
   memcpy(container.rrc_container.data(), ho_prep_container, sizeof(ho_prep_container));
-  tester.rrc.start_ho_ue_resource_alloc(ho_req, container);
+  asn1::s1ap::cause_c cause;
+  int                 rnti = tester.rrc.start_ho_ue_resource_alloc(ho_req, container, cause);
+  if (test_params.fail_at == mobility_test_params::test_event::wrong_target_cell) {
+    TESTASSERT(rnti == SRSRAN_INVALID_RNTI);
+    TESTASSERT(cause.type().value == asn1::s1ap::cause_c::types_opts::radio_network);
+    TESTASSERT(cause.radio_network().value == asn1::s1ap::cause_radio_network_opts::ho_target_not_allowed);
+    TESTASSERT(tester.rrc.get_nof_users() == 0);
+    return SRSRAN_SUCCESS;
+  }
+  if (test_params.fail_at == mobility_test_params::test_event::unknown_qci) {
+    TESTASSERT(rnti == SRSRAN_INVALID_RNTI);
+    TESTASSERT(cause.type().value == asn1::s1ap::cause_c::types_opts::radio_network);
+    TESTASSERT(cause.radio_network().value == asn1::s1ap::cause_radio_network_opts::not_supported_qci_value);
+    TESTASSERT(tester.rrc.get_nof_users() == 0);
+    return SRSRAN_SUCCESS;
+  }
   tester.tic();
   TESTASSERT(tester.rrc.get_nof_users() == 1);
   TESTASSERT(tester.mac.ue_db.count(0x46));
   auto& mac_ue = tester.mac.ue_db[0x46];
   TESTASSERT(mac_ue.supported_cc_list[0].active);
   TESTASSERT(mac_ue.supported_cc_list[0].enb_cc_idx == 0);
-  TESTASSERT(mac_ue.ue_bearers[rb_id_t::RB_ID_SRB0].direction == sched_interface::ue_bearer_cfg_t::BOTH);
+  TESTASSERT(mac_ue.ue_bearers[srb_to_lcid(lte_srb::srb0)].direction == sched_interface::ue_bearer_cfg_t::BOTH);
   // Check Security Configuration
   TESTASSERT(tester.pdcp.bearers.count(0x46));
-  TESTASSERT(tester.pdcp.bearers[0x46].count(rb_id_t::RB_ID_SRB1) and
-             tester.pdcp.bearers[0x46].count(rb_id_t::RB_ID_SRB2));
-  TESTASSERT(tester.pdcp.bearers[0x46][rb_id_t::RB_ID_SRB1].enable_encryption);
-  TESTASSERT(tester.pdcp.bearers[0x46][rb_id_t::RB_ID_SRB1].enable_integrity);
+  TESTASSERT(tester.pdcp.bearers[0x46].count(srb_to_lcid(lte_srb::srb1)) and
+             tester.pdcp.bearers[0x46].count(srb_to_lcid(lte_srb::srb2)));
+  TESTASSERT(tester.pdcp.bearers[0x46][srb_to_lcid(lte_srb::srb1)].enable_encryption);
+  TESTASSERT(tester.pdcp.bearers[0x46][srb_to_lcid(lte_srb::srb1)].enable_integrity);
   sec_cfg.set_security_capabilities(ho_req.protocol_ies.ue_security_cap.value);
   sec_cfg.set_security_key(ho_req.protocol_ies.security_context.value.next_hop_param);
   sec_cfg.regenerate_keys_handover(tester.cfg.cell_list[0].pci, tester.cfg.cell_list[0].dl_earfcn);
   srsran::as_security_config_t as_sec_cfg = sec_cfg.get_as_sec_cfg();
-  TESTASSERT(tester.pdcp.bearers[0x46][rb_id_t::RB_ID_SRB1].sec_cfg.k_rrc_int == as_sec_cfg.k_rrc_int);
-  TESTASSERT(tester.pdcp.bearers[0x46][rb_id_t::RB_ID_SRB1].sec_cfg.k_rrc_enc == as_sec_cfg.k_rrc_enc);
-  TESTASSERT(tester.pdcp.bearers[0x46][rb_id_t::RB_ID_SRB1].sec_cfg.k_up_int == as_sec_cfg.k_up_int);
-  TESTASSERT(tester.pdcp.bearers[0x46][rb_id_t::RB_ID_SRB1].sec_cfg.cipher_algo == as_sec_cfg.cipher_algo);
-  TESTASSERT(tester.pdcp.bearers[0x46][rb_id_t::RB_ID_SRB1].sec_cfg.integ_algo == as_sec_cfg.integ_algo);
+  TESTASSERT(tester.pdcp.bearers[0x46][srb_to_lcid(lte_srb::srb1)].sec_cfg.k_rrc_int == as_sec_cfg.k_rrc_int);
+  TESTASSERT(tester.pdcp.bearers[0x46][srb_to_lcid(lte_srb::srb1)].sec_cfg.k_rrc_enc == as_sec_cfg.k_rrc_enc);
+  TESTASSERT(tester.pdcp.bearers[0x46][srb_to_lcid(lte_srb::srb1)].sec_cfg.k_up_int == as_sec_cfg.k_up_int);
+  TESTASSERT(tester.pdcp.bearers[0x46][srb_to_lcid(lte_srb::srb1)].sec_cfg.cipher_algo == as_sec_cfg.cipher_algo);
+  TESTASSERT(tester.pdcp.bearers[0x46][srb_to_lcid(lte_srb::srb1)].sec_cfg.integ_algo == as_sec_cfg.integ_algo);
 
   // Check if S1AP Handover Request ACK send is called
   TESTASSERT(tester.s1ap.last_ho_req_ack.rnti == 0x46);
@@ -364,11 +392,11 @@ int test_s1ap_tenb_mobility(mobility_test_params test_params)
 
   uint8_t recfg_complete[] = {0x10, 0x00};
   test_helpers::copy_msg_to_buffer(pdu, recfg_complete);
-  tester.rrc.write_pdu(0x46, rb_id_t::RB_ID_SRB1, std::move(pdu));
+  tester.rrc.write_pdu(0x46, srb_to_lcid(lte_srb::srb1), std::move(pdu));
   tester.tic();
-  TESTASSERT(mac_ue.ue_bearers[rb_id_t::RB_ID_SRB1].direction == sched_interface::ue_bearer_cfg_t::BOTH);
-  TESTASSERT(mac_ue.ue_bearers[rb_id_t::RB_ID_SRB2].direction == sched_interface::ue_bearer_cfg_t::BOTH);
-  TESTASSERT(mac_ue.ue_bearers[rb_id_t::RB_ID_DRB1].direction == sched_interface::ue_bearer_cfg_t::BOTH);
+  TESTASSERT(mac_ue.ue_bearers[srb_to_lcid(lte_srb::srb1)].direction == sched_interface::ue_bearer_cfg_t::BOTH);
+  TESTASSERT(mac_ue.ue_bearers[srb_to_lcid(lte_srb::srb2)].direction == sched_interface::ue_bearer_cfg_t::BOTH);
+  TESTASSERT(mac_ue.ue_bearers[drb_to_lcid(lte_drb::drb1)].direction == sched_interface::ue_bearer_cfg_t::BOTH);
   TESTASSERT(mac_ue.pucch_cfg.I_sr == recfg_r8.rr_cfg_ded.phys_cfg_ded.sched_request_cfg.setup().sr_cfg_idx);
   TESTASSERT(mac_ue.pucch_cfg.n_pucch_sr ==
              recfg_r8.rr_cfg_ded.phys_cfg_ded.sched_request_cfg.setup().sr_pucch_res_idx);
@@ -477,7 +505,7 @@ int test_intraenb_mobility(srsran::log_sink_spy& spy, mobility_test_params test_
   /* Test Case: Terminate first Handover. No extra messages should be sent DL. SR/CQI resources match recfg message */
   uint8_t recfg_complete[] = {0x10, 0x00};
   test_helpers::copy_msg_to_buffer(pdu, recfg_complete);
-  tester.rrc.write_pdu(tester.rnti, rb_id_t::RB_ID_SRB2, std::move(pdu));
+  tester.rrc.write_pdu(tester.rnti, srb_to_lcid(lte_srb::srb2), std::move(pdu));
   TESTASSERT(tester.pdcp.last_sdu.sdu == nullptr);
   sched_interface::ue_cfg_t& ue_cfg = tester.mac.ue_db[tester.rnti];
   TESTASSERT(ue_cfg.pucch_cfg.sr_configured);
@@ -534,12 +562,14 @@ int main(int argc, char** argv)
   }
   argparse::parse_args(argc, argv);
 
-  // S1AP Handover
+  // Source ENB - S1 Handover
   TESTASSERT(test_s1ap_mobility(*spy, mobility_test_params{event::wrong_measreport}) == 0);
   TESTASSERT(test_s1ap_mobility(*spy, mobility_test_params{event::concurrent_ho}) == 0);
   TESTASSERT(test_s1ap_mobility(*spy, mobility_test_params{event::ho_prep_failure}) == 0);
   TESTASSERT(test_s1ap_mobility(*spy, mobility_test_params{event::success}) == 0);
 
+  TESTASSERT(test_s1ap_tenb_mobility(mobility_test_params{event::wrong_target_cell}) == 0);
+  TESTASSERT(test_s1ap_tenb_mobility(mobility_test_params{event::unknown_qci}) == 0);
   TESTASSERT(test_s1ap_tenb_mobility(mobility_test_params{event::success}) == 0);
 
   // intraeNB Handover
diff --git a/srsenb/test/upper/s1ap_test.cc b/srsenb/test/upper/s1ap_test.cc
index 16e7897c8..9d3ea0093 100644
--- a/srsenb/test/upper/s1ap_test.cc
+++ b/srsenb/test/upper/s1ap_test.cc
@@ -100,18 +100,27 @@ struct dummy_socket_manager : public srsran::socket_manager_itf {
 };
 
 struct rrc_tester : public rrc_dummy {
-  void modify_erabs(uint16_t                                 rnti,
-                    const asn1::s1ap::erab_modify_request_s& msg,
-                    std::vector<uint16_t>*                   erabs_modified,
-                    std::vector<uint16_t>*                   erabs_failed_to_modify) override
+  int modify_erab(uint16_t                                   rnti,
+                  uint16_t                                   erab_id,
+                  const asn1::s1ap::erab_level_qos_params_s& qos_params,
+                  srsran::const_byte_span                    nas_pdu,
+                  asn1::s1ap::cause_c&                       cause) override
   {
-    *erabs_modified         = next_erabs_modified;
-    *erabs_failed_to_modify = next_erabs_failed_to_modify;
+    if (std::count(next_erabs_failed_to_modify.begin(), next_erabs_failed_to_modify.end(), erab_id) > 0) {
+      cause.set_radio_network().value = asn1::s1ap::cause_radio_network_opts::unknown_erab_id;
+      return SRSRAN_ERROR;
+    }
+    last_erabs_modified.push_back(erab_id);
+    return SRSRAN_SUCCESS;
+  }
+  bool has_erab(uint16_t rnti, uint32_t erab_id) const override
+  {
+    return std::count(next_erabs_failed_to_modify.begin(), next_erabs_failed_to_modify.end(), erab_id) == 0;
   }
   void release_ue(uint16_t rnti) override { last_released_rnti = rnti; }
 
   uint16_t              last_released_rnti = SRSRAN_INVALID_RNTI;
-  std::vector<uint16_t> next_erabs_modified, next_erabs_failed_to_modify;
+  std::vector<uint16_t> next_erabs_failed_to_modify, last_erabs_modified;
 };
 
 void run_s1_setup(s1ap& s1ap_obj, mme_dummy& mme)
@@ -184,16 +193,19 @@ void add_rnti(s1ap& s1ap_obj, mme_dummy& mme)
                        0x40, 0x0a, 0x0a, 0x1f, 0x7f, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x01};
   cbref             = asn1::cbit_ref(icsresp, sizeof(icsresp));
   TESTASSERT(s1ap_pdu.unpack(cbref) == SRSRAN_SUCCESS);
-  s1ap_obj.ue_ctxt_setup_complete(0x46, s1ap_pdu.successful_outcome().value.init_context_setup_resp());
+  s1ap_obj.ue_ctxt_setup_complete(0x46);
   sdu = mme.read_msg();
   TESTASSERT(sdu->N_bytes > 0);
   cbref = asn1::cbit_ref{sdu->msg, sdu->N_bytes};
   TESTASSERT(s1ap_pdu.unpack(cbref) == SRSRAN_SUCCESS);
   TESTASSERT(s1ap_pdu.type().value == asn1::s1ap::s1ap_pdu_c::types_opts::successful_outcome);
   TESTASSERT(s1ap_pdu.successful_outcome().proc_code == ASN1_S1AP_ID_INIT_CONTEXT_SETUP);
+  const auto& resp = s1ap_pdu.successful_outcome().value.init_context_setup_resp().protocol_ies;
+  TESTASSERT(resp.erab_setup_list_ctxt_su_res.value.size() > 0);
+  TESTASSERT(not resp.erab_failed_to_setup_list_ctxt_su_res_present);
 }
 
-enum class test_event { success, wrong_erabid_mod, wrong_mme_s1ap_id };
+enum class test_event { success, wrong_erabid_mod, wrong_mme_s1ap_id, repeated_erabid_mod };
 
 void test_s1ap_erab_setup(test_event event)
 {
@@ -226,24 +238,37 @@ void test_s1ap_erab_setup(test_event event)
   add_rnti(s1ap_obj, mme);
 
   // E-RAB Modify Request
-  sockaddr_in     mme_addr      = {};
-  sctp_sndrcvinfo rcvinfo       = {};
-  int             flags         = 0;
-  uint8_t         mod_req_msg[] = {0x00, 0x06, 0x00, 0x1E, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x02, 0x00,
-                           0x01, 0x00, 0x08, 0x00, 0x02, 0x00, 0x01, 0x00, 0x1E, 0x00, 0x0B, 0x00,
-                           0x00, 0x24, 0x00, 0x06, 0x0A, 0x00, 0x09, 0x3C, 0x01, 0x00};
-  // 00 06 00 1E 00 00 03 00 00 00 02 00 01 00 08 00 02 00 01 00 1E 00 0B 00 00 24 00 06 0A 00 09 3C 01 00
+  sockaddr_in            mme_addr = {};
+  sctp_sndrcvinfo        rcvinfo  = {};
+  int                    flags    = 0;
+  asn1::s1ap::s1ap_pdu_c mod_req_pdu;
+  mod_req_pdu.set_init_msg().load_info_obj(ASN1_S1AP_ID_ERAB_MODIFY);
+  auto& protocols                = mod_req_pdu.init_msg().value.erab_modify_request().protocol_ies;
+  protocols.enb_ue_s1ap_id.value = 1;
+  protocols.mme_ue_s1ap_id.value = event == test_event::wrong_mme_s1ap_id ? 2 : 1;
+  auto& erab_list                = protocols.erab_to_be_modified_list_bearer_mod_req.value;
+  erab_list.resize(2);
+  erab_list[0].load_info_obj(ASN1_S1AP_ID_ERAB_TO_BE_MODIFIED_ITEM_BEARER_MOD_REQ);
+  auto* erab_ptr                      = &erab_list[0].value.erab_to_be_modified_item_bearer_mod_req();
+  erab_ptr->erab_id                   = 5;
+  erab_ptr->erab_level_qos_params.qci = 9;
+  erab_ptr->erab_level_qos_params.alloc_retention_prio.prio_level = 15;
+  erab_ptr->erab_level_qos_params.alloc_retention_prio.pre_emption_cap.value =
+      asn1::s1ap::pre_emption_cap_opts::shall_not_trigger_pre_emption;
+  erab_ptr->erab_level_qos_params.alloc_retention_prio.pre_emption_vulnerability.value =
+      asn1::s1ap::pre_emption_vulnerability_opts::not_pre_emptable;
+  erab_ptr->nas_pdu.resize(1);
+  erab_list[1]      = erab_list[0];
+  erab_ptr          = &erab_list[1].value.erab_to_be_modified_item_bearer_mod_req();
+  erab_ptr->erab_id = event == test_event::repeated_erabid_mod ? 5 : 6;
   if (event == test_event::wrong_erabid_mod) {
-    mod_req_msg[sizeof(mod_req_msg) - 6] = 0x0C; // E-RAB id = 6
     rrc.next_erabs_failed_to_modify.push_back(6);
-  } else if (event == test_event::wrong_mme_s1ap_id) {
-    mod_req_msg[12] = 0x02; // MME-UE-S1AP-ID = 2
-  } else {
-    rrc.next_erabs_modified.push_back(5);
   }
   sdu = srsran::make_byte_buffer();
-  memcpy(sdu->msg, mod_req_msg, sizeof(mod_req_msg));
-  sdu->N_bytes = sizeof(mod_req_msg);
+  asn1::bit_ref bref(sdu->msg, sdu->get_tailroom());
+  TESTASSERT(mod_req_pdu.pack(bref) == SRSRAN_SUCCESS);
+  sdu->N_bytes = bref.distance_bytes();
+
   TESTASSERT(rrc.last_released_rnti == SRSRAN_INVALID_RNTI);
   TESTASSERT(s1ap_obj.handle_mme_rx_msg(std::move(sdu), mme_addr, rcvinfo, flags));
   sdu = mme.read_msg();
@@ -266,7 +291,10 @@ void test_s1ap_erab_setup(test_event event)
   TESTASSERT(s1ap_pdu.successful_outcome().proc_code == ASN1_S1AP_ID_ERAB_MODIFY);
   auto& protocol_ies = s1ap_pdu.successful_outcome().value.erab_modify_resp().protocol_ies;
   if (event == test_event::wrong_erabid_mod) {
-    TESTASSERT(not protocol_ies.erab_modify_list_bearer_mod_res_present);
+    TESTASSERT(protocol_ies.erab_modify_list_bearer_mod_res_present);
+    TESTASSERT(protocol_ies.erab_modify_list_bearer_mod_res.value.size() == 1);
+    TESTASSERT(protocol_ies.erab_modify_list_bearer_mod_res.value[0].value.erab_modify_item_bearer_mod_res().erab_id ==
+               5);
     TESTASSERT(protocol_ies.erab_failed_to_modify_list_present);
     TESTASSERT(protocol_ies.erab_failed_to_modify_list.value.size() == 1);
     auto& erab_item = protocol_ies.erab_failed_to_modify_list.value[0].value.erab_item();
@@ -275,10 +303,21 @@ void test_s1ap_erab_setup(test_event event)
     TESTASSERT(erab_item.cause.radio_network().value == asn1::s1ap::cause_radio_network_opts::unknown_erab_id);
     return;
   }
+  if (event == test_event::repeated_erabid_mod) {
+    TESTASSERT(not protocol_ies.erab_modify_list_bearer_mod_res_present);
+    TESTASSERT(protocol_ies.erab_failed_to_modify_list_present);
+    TESTASSERT(protocol_ies.erab_failed_to_modify_list.value.size() == 1);
+    auto& erab_item = protocol_ies.erab_failed_to_modify_list.value[0].value.erab_item();
+    TESTASSERT(erab_item.erab_id == 5);
+    TESTASSERT(erab_item.cause.type().value == asn1::s1ap::cause_c::types_opts::radio_network);
+    TESTASSERT(erab_item.cause.radio_network().value ==
+               asn1::s1ap::cause_radio_network_opts::multiple_erab_id_instances);
+    return;
+  }
 
   TESTASSERT(protocol_ies.erab_modify_list_bearer_mod_res_present);
   TESTASSERT(not protocol_ies.erab_failed_to_modify_list_present);
-  TESTASSERT(protocol_ies.erab_modify_list_bearer_mod_res.value.size() == 1);
+  TESTASSERT(protocol_ies.erab_modify_list_bearer_mod_res.value.size() == 2);
   auto& erab_item = protocol_ies.erab_modify_list_bearer_mod_res.value[0].value.erab_modify_item_bearer_mod_res();
   TESTASSERT(erab_item.erab_id == 5);
 }
@@ -296,4 +335,5 @@ int main(int argc, char** argv)
   test_s1ap_erab_setup(test_event::success);
   test_s1ap_erab_setup(test_event::wrong_erabid_mod);
   test_s1ap_erab_setup(test_event::wrong_mme_s1ap_id);
+  test_s1ap_erab_setup(test_event::repeated_erabid_mod);
 }
\ No newline at end of file
diff --git a/srsenb/test/upper/test_helpers.cc b/srsenb/test/upper/test_helpers.cc
index d28cc3300..4e389db17 100644
--- a/srsenb/test/upper/test_helpers.cc
+++ b/srsenb/test/upper/test_helpers.cc
@@ -118,6 +118,18 @@ int bring_rrc_to_reconf_state(srsenb::rrc& rrc, srsran::timer_handler& timers, u
   asn1::cbit_ref bref(byte_buf.msg, byte_buf.N_bytes);
   TESTASSERT(s1ap_pdu.unpack(bref) == asn1::SRSASN_SUCCESS);
   rrc.setup_ue_ctxt(rnti, s1ap_pdu.init_msg().value.init_context_setup_request());
+  for (auto& item :
+       s1ap_pdu.init_msg().value.init_context_setup_request().protocol_ies.erab_to_be_setup_list_ctxt_su_req.value) {
+    const auto&         erab = item.value.erab_to_be_setup_item_ctxt_su_req();
+    asn1::s1ap::cause_c cause;
+    TESTASSERT(rrc.setup_erab(rnti,
+                              erab.erab_id,
+                              erab.erab_level_qos_params,
+                              erab.nas_pdu,
+                              erab.transport_layer_address,
+                              erab.gtp_teid.to_number(),
+                              cause) == SRSRAN_SUCCESS);
+  }
   timers.step_all();
   rrc.tti_clock();
 
diff --git a/srsenb/test/upper/test_helpers.h b/srsenb/test/upper/test_helpers.h
index d47d05be0..036e5a519 100644
--- a/srsenb/test/upper/test_helpers.h
+++ b/srsenb/test/upper/test_helpers.h
@@ -81,18 +81,19 @@ public:
     uint16_t                        rnti;
     std::vector<bearer_status_info> bearer_list;
   } last_enb_status = {};
-  std::vector<uint8_t> added_erab_ids;
   struct ho_req_ack {
     uint16_t                                      rnti;
     srsran::unique_byte_buffer_t                  ho_cmd_pdu;
     std::vector<asn1::s1ap::erab_admitted_item_s> admitted_bearers;
+    std::vector<asn1::s1ap::erab_item_s>          not_admitted_bearers;
   } last_ho_req_ack;
 
   bool send_ho_required(uint16_t                     rnti,
                         uint32_t                     target_eci,
                         srsran::plmn_id_t            target_plmn,
                         srsran::span<uint32_t>       fwd_erabs,
-                        srsran::unique_byte_buffer_t rrc_container) final
+                        srsran::unique_byte_buffer_t rrc_container,
+                        bool                         has_direct_fwd_path) final
   {
     last_ho_required = ho_req_data{rnti, target_eci, target_plmn, std::move(rrc_container)};
     return true;
@@ -106,21 +107,15 @@ public:
                        uint16_t                                       rnti,
                        uint32_t                                       enb_cc_idx,
                        srsran::unique_byte_buffer_t                   ho_cmd,
-                       srsran::span<asn1::s1ap::erab_admitted_item_s> admitted_bearers) override
+                       srsran::span<asn1::s1ap::erab_admitted_item_s> admitted_bearers,
+                       srsran::const_span<asn1::s1ap::erab_item_s>    not_admitted_bearers) override
   {
     last_ho_req_ack.rnti       = rnti;
     last_ho_req_ack.ho_cmd_pdu = std::move(ho_cmd);
     last_ho_req_ack.admitted_bearers.assign(admitted_bearers.begin(), admitted_bearers.end());
+    last_ho_req_ack.not_admitted_bearers.assign(not_admitted_bearers.begin(), not_admitted_bearers.end());
     return true;
   }
-  void ue_erab_setup_complete(uint16_t rnti, const asn1::s1ap::erab_setup_resp_s& res) override
-  {
-    if (res.protocol_ies.erab_setup_list_bearer_su_res_present) {
-      for (const auto& item : res.protocol_ies.erab_setup_list_bearer_su_res.value) {
-        added_erab_ids.push_back(item.value.erab_setup_item_bearer_su_res().erab_id);
-      }
-    }
-  }
   void user_mod(uint16_t old_rnti, uint16_t new_rnti) override {}
 };
 
diff --git a/srsue/hdr/phy/nr/state.h b/srsue/hdr/phy/nr/state.h
index 16a52068b..de0fc286b 100644
--- a/srsue/hdr/phy/nr/state.h
+++ b/srsue/hdr/phy/nr/state.h
@@ -22,6 +22,7 @@
 #ifndef SRSRAN_STATE_H
 #define SRSRAN_STATE_H
 
+#include "../phy_metrics.h"
 #include "srsran/adt/circular_array.h"
 #include "srsran/common/common.h"
 #include "srsran/interfaces/ue_nr_interfaces.h"
@@ -56,9 +57,27 @@ private:
   srsran::circular_array<srsran_pdsch_ack_nr_t, TTIMOD_SZ> pending_ack = {};
   mutable std::mutex                                       pending_ack_mutex;
 
+  info_metrics_t     info_metrics = {};
+  sync_metrics_t     sync_metrics = {};
+  ch_metrics_t       ch_metrics   = {};
+  dl_metrics_t       dl_metrics   = {};
+  ul_metrics_t       ul_metrics   = {};
+  mutable std::mutex metrics_mutex;
+
   /// CSI-RS measurements
   std::array<srsran_csi_measurements_t, SRSRAN_CSI_MAX_NOF_RESOURCES> csi_measurements = {};
 
+  /**
+   * @brief Resets all metrics (unprotected)
+   */
+  void reset_metrics_()
+  {
+    sync_metrics.reset();
+    ch_metrics.reset();
+    dl_metrics.reset();
+    ul_metrics.reset();
+  }
+
 public:
   mac_interface_phy_nr* stack   = nullptr;
   srsran_carrier_nr_t   carrier = {};
@@ -80,6 +99,8 @@ public:
     carrier.nof_prb         = 100;
     carrier.max_mimo_layers = 1;
 
+    info_metrics.pci = carrier.id;
+
     // Hard-coded values, this should be set when the measurements take place
     csi_measurements[0].K_csi_rs  = 1;
     csi_measurements[0].nof_ports = 1;
@@ -266,7 +287,11 @@ public:
     return true;
   }
 
-  void reset() { clear_pending_grants(); }
+  void reset()
+  {
+    clear_pending_grants();
+    reset_metrics();
+  }
 
   bool has_valid_sr_resource(uint32_t sr_id)
   {
@@ -332,6 +357,85 @@ public:
 
     uci_data.cfg.pucch.rnti = stack->get_ul_sched_rnti_nr(tti).id;
   }
+
+  /**
+   * @brief Sets time and frequency synchronization metrics
+   * @param m Metrics object
+   */
+  void set_info_metrics(const info_metrics_t& m)
+  {
+    std::lock_guard<std::mutex> lock(metrics_mutex);
+    info_metrics = m;
+  }
+
+  /**
+   * @brief Sets time and frequency synchronization metrics
+   * @param m Metrics object
+   */
+  void set_sync_metrics(const sync_metrics_t& m)
+  {
+    std::lock_guard<std::mutex> lock(metrics_mutex);
+    sync_metrics.set(m);
+  }
+
+  /**
+   * @brief Sets DL channel metrics from received CSI-RS resources
+   * @param m Metrics object
+   */
+  void set_channel_metrics(const ch_metrics_t& m)
+  {
+    std::lock_guard<std::mutex> lock(metrics_mutex);
+    ch_metrics.set(m);
+  }
+
+  /**
+   * @brief Sets DL metrics of a given PDSCH transmission
+   * @param m Metrics object
+   */
+  void set_dl_metrics(const dl_metrics_t& m)
+  {
+    std::lock_guard<std::mutex> lock(metrics_mutex);
+    dl_metrics.set(m);
+  }
+
+  /**
+   * @brief Sets UL metrics of a given PUSCH transmission
+   * @param m Metrics object
+   */
+  void set_ul_metrics(const ul_metrics_t& m)
+  {
+    std::lock_guard<std::mutex> lock(metrics_mutex);
+    ul_metrics.set(m);
+  }
+
+  /**
+   * @brief Resets all metrics (protected)
+   */
+  void reset_metrics()
+  {
+    std::lock_guard<std::mutex> lock(metrics_mutex);
+    reset_metrics_();
+  }
+
+  /**
+   * @brief Appends the NR PHY metrics to the general metric hub
+   * @param m PHY Metrics object
+   */
+  void get_metrics(phy_metrics_t& m)
+  {
+    std::lock_guard<std::mutex> lock(metrics_mutex);
+
+    uint32_t cc = m.nof_active_cc;
+    m.info[cc]  = info_metrics;
+    m.sync[cc]  = sync_metrics;
+    m.ch[cc]    = ch_metrics;
+    m.dl[cc]    = dl_metrics;
+    m.ul[cc]    = ul_metrics;
+    m.nof_active_cc++;
+
+    // Reset all metrics
+    reset_metrics_();
+  }
 };
 } // namespace nr
 } // namespace srsue
diff --git a/srsue/hdr/phy/nr/worker_pool.h b/srsue/hdr/phy/nr/worker_pool.h
index 88eb44b60..fd07876e6 100644
--- a/srsue/hdr/phy/nr/worker_pool.h
+++ b/srsue/hdr/phy/nr/worker_pool.h
@@ -51,6 +51,7 @@ public:
   bool set_config(const srsran::phy_cfg_nr_t& cfg);
   bool       has_valid_sr_resource(uint32_t sr_id);
   void       clear_pending_grants();
+  void       get_metrics(phy_metrics_t& m);
 };
 
 } // namespace nr
diff --git a/srsue/hdr/phy/phy.h b/srsue/hdr/phy/phy.h
index 4484d0245..2566e7f19 100644
--- a/srsue/hdr/phy/phy.h
+++ b/srsue/hdr/phy/phy.h
@@ -100,7 +100,7 @@ public:
   void wait_initialize() final;
   bool is_initiated();
 
-  void get_metrics(phy_metrics_t* m) final;
+  void get_metrics(const srsran::srsran_rat_t& rat, phy_metrics_t* m) final;
   void srsran_phy_logger(phy_logger_level_t log_level, char* str);
 
   void enable_pregen_signals(bool enable) final;
diff --git a/srsue/hdr/phy/phy_common.h b/srsue/hdr/phy/phy_common.h
index 194063286..502bc72d6 100644
--- a/srsue/hdr/phy/phy_common.h
+++ b/srsue/hdr/phy/phy_common.h
@@ -145,16 +145,16 @@ public:
   srsran::radio_interface_phy* get_radio();
 
   void set_dl_metrics(uint32_t cc_idx, const dl_metrics_t& m);
-  void get_dl_metrics(dl_metrics_t m[SRSRAN_MAX_CARRIERS]);
+  void get_dl_metrics(dl_metrics_t::array_t& m);
 
   void set_ch_metrics(uint32_t cc_idx, const ch_metrics_t& m);
-  void get_ch_metrics(ch_metrics_t m[SRSRAN_MAX_CARRIERS]);
+  void get_ch_metrics(ch_metrics_t::array_t& m);
 
   void set_ul_metrics(uint32_t cc_idx, const ul_metrics_t& m);
-  void get_ul_metrics(ul_metrics_t m[SRSRAN_MAX_CARRIERS]);
+  void get_ul_metrics(ul_metrics_t::array_t& m);
 
   void set_sync_metrics(const uint32_t& cc_idx, const sync_metrics_t& m);
-  void get_sync_metrics(sync_metrics_t m[SRSRAN_MAX_CARRIERS]);
+  void get_sync_metrics(sync_metrics_t::array_t& m);
 
   void reset();
   void reset_radio();
@@ -311,14 +311,10 @@ private:
 
   std::mutex metrics_mutex;
 
-  ch_metrics_t   ch_metrics[SRSRAN_MAX_CARRIERS]         = {};
-  uint32_t       ch_metrics_count[SRSRAN_MAX_CARRIERS]   = {};
-  dl_metrics_t   dl_metrics[SRSRAN_MAX_CARRIERS]         = {};
-  uint32_t       dl_metrics_count[SRSRAN_MAX_CARRIERS]   = {};
-  ul_metrics_t   ul_metrics[SRSRAN_MAX_CARRIERS]         = {};
-  uint32_t       ul_metrics_count[SRSRAN_MAX_CARRIERS]   = {};
-  sync_metrics_t sync_metrics[SRSRAN_MAX_CARRIERS]       = {};
-  uint32_t       sync_metrics_count[SRSRAN_MAX_CARRIERS] = {};
+  ch_metrics_t::array_t   ch_metrics   = {};
+  dl_metrics_t::array_t   dl_metrics   = {};
+  ul_metrics_t::array_t   ul_metrics   = {};
+  sync_metrics_t::array_t sync_metrics = {};
 
   // MBSFN
   bool     sib13_configured = false;
diff --git a/srsue/hdr/phy/phy_metrics.h b/srsue/hdr/phy/phy_metrics.h
index 60c89b92c..4ad516168 100644
--- a/srsue/hdr/phy/phy_metrics.h
+++ b/srsue/hdr/phy/phy_metrics.h
@@ -23,23 +23,58 @@
 #define SRSUE_PHY_METRICS_H
 
 #include "srsran/srsran.h"
+#include <array>
 
 namespace srsue {
 
 struct info_metrics_t {
+  typedef std::array<info_metrics_t, SRSRAN_MAX_CARRIERS> array_t;
+
   uint32_t pci;
   uint32_t dl_earfcn;
 };
 
+#define PHY_METRICS_SET(PARAM)                                                                                         \
+  do {                                                                                                                 \
+    PARAM = PARAM + (other.PARAM - PARAM) / count;                                                                     \
+  } while (false)
+
 struct sync_metrics_t {
+  typedef std::array<sync_metrics_t, SRSRAN_MAX_CARRIERS> array_t;
+
   float ta_us;
   float distance_km;
   float speed_kmph;
   float cfo;
   float sfo;
+
+  void set(const sync_metrics_t& other)
+  {
+    count++;
+    ta_us       = other.ta_us;
+    distance_km = other.distance_km;
+    speed_kmph  = other.speed_kmph;
+    PHY_METRICS_SET(cfo);
+    PHY_METRICS_SET(sfo);
+  }
+
+  void reset()
+  {
+    count       = 0;
+    ta_us       = 0.0f;
+    distance_km = 0.0f;
+    speed_kmph  = 0.0f;
+    cfo         = 0.0f;
+    sfo         = 0.0f;
+  }
+
+private:
+  uint32_t count = 0;
 };
 
 struct ch_metrics_t {
+  typedef std::array<ch_metrics_t, SRSRAN_MAX_CARRIERS> array_t;
+
   float n;
   float sinr;
   float rsrp;
@@ -48,25 +83,97 @@ struct ch_metrics_t {
   float ri;
   float pathloss;
   float sync_err;
+
+  void set(const ch_metrics_t& other)
+  {
+    count++;
+    PHY_METRICS_SET(n);
+    PHY_METRICS_SET(sinr);
+    PHY_METRICS_SET(rsrp);
+    PHY_METRICS_SET(rsrq);
+    PHY_METRICS_SET(rssi);
+    PHY_METRICS_SET(ri);
+    PHY_METRICS_SET(pathloss);
+    PHY_METRICS_SET(sync_err);
+  }
+
+  void reset()
+  {
+    count    = 0;
+    n        = 0.0;
+    sinr     = 0.0;
+    rsrp     = 0.0;
+    rsrq     = 0.0;
+    rssi     = 0.0;
+    ri       = 0.0;
+    pathloss = 0.0;
+    sync_err = 0.0;
+  }
+
+private:
+  uint32_t count = 0;
 };
 
 struct dl_metrics_t {
-  float turbo_iters;
+  typedef std::array<dl_metrics_t, SRSRAN_MAX_CARRIERS> array_t;
+
+  float fec_iters;
   float mcs;
+  float evm;
+
+  void set(const dl_metrics_t& other)
+  {
+    count++;
+    PHY_METRICS_SET(fec_iters);
+    PHY_METRICS_SET(mcs);
+    PHY_METRICS_SET(evm);
+  }
+
+  void reset()
+  {
+    count     = 0;
+    fec_iters = 0.0f;
+    mcs       = 0.0f;
+    evm       = 0.0f;
+  }
+
+private:
+  uint32_t count = 0;
 };
 
 struct ul_metrics_t {
+  typedef std::array<ul_metrics_t, SRSRAN_MAX_CARRIERS> array_t;
+
   float mcs;
   float power;
+
+  void set(const ul_metrics_t& other)
+  {
+    count++;
+    PHY_METRICS_SET(mcs);
+    PHY_METRICS_SET(power);
+  }
+
+  void reset()
+  {
+    count = 0;
+    mcs   = 0.0f;
+    power = 0.0f;
+  }
+
+private:
+  uint32_t count = 0;
 };
 
+#undef PHY_METRICS_SET
+
 struct phy_metrics_t {
-  info_metrics_t info[SRSRAN_MAX_CARRIERS];
-  sync_metrics_t sync[SRSRAN_MAX_CARRIERS];
-  ch_metrics_t   ch[SRSRAN_MAX_CARRIERS];
-  dl_metrics_t   dl[SRSRAN_MAX_CARRIERS];
-  ul_metrics_t   ul[SRSRAN_MAX_CARRIERS];
-  uint32_t       nof_active_cc;
+  info_metrics_t::array_t info          = {};
+  sync_metrics_t::array_t sync          = {};
+  ch_metrics_t::array_t   ch            = {};
+  dl_metrics_t::array_t   dl            = {};
+  ul_metrics_t::array_t   ul            = {};
+  uint32_t                nof_active_cc = 0;
 };
 
 } // namespace srsue
diff --git a/srsue/hdr/phy/ue_lte_phy_base.h b/srsue/hdr/phy/ue_lte_phy_base.h
index dad72aa8a..20ccac68c 100644
--- a/srsue/hdr/phy/ue_lte_phy_base.h
+++ b/srsue/hdr/phy/ue_lte_phy_base.h
@@ -49,7 +49,7 @@ public:
   virtual void wait_initialize() = 0;
   virtual void start_plot()      = 0;
 
-  virtual void get_metrics(phy_metrics_t* m) = 0;
+  virtual void get_metrics(const srsran::srsran_rat_t& rat, phy_metrics_t* m) = 0;
 };
 
 } // namespace srsue
diff --git a/srsue/hdr/phy/ue_nr_phy_base.h b/srsue/hdr/phy/ue_nr_phy_base.h
index 3011156ad..1911c916e 100644
--- a/srsue/hdr/phy/ue_nr_phy_base.h
+++ b/srsue/hdr/phy/ue_nr_phy_base.h
@@ -47,7 +47,7 @@ public:
 
   virtual void set_earfcn(std::vector<uint32_t> earfcns) = 0;
 
-  virtual void get_metrics(phy_metrics_t* m) = 0;
+  virtual void get_metrics(const srsran::srsran_rat_t& rat, phy_metrics_t* m) = 0;
 };
 
 } // namespace srsue
diff --git a/srsue/hdr/phy/ue_phy_base.h b/srsue/hdr/phy/ue_phy_base.h
index 0e2fca68b..7343d6722 100644
--- a/srsue/hdr/phy/ue_phy_base.h
+++ b/srsue/hdr/phy/ue_phy_base.h
@@ -47,7 +47,7 @@ public:
   virtual void wait_initialize() = 0;
   virtual void start_plot()      = 0;
 
-  virtual void get_metrics(phy_metrics_t* m) = 0;
+  virtual void get_metrics(const srsran::srsran_rat_t& rat, phy_metrics_t* m) = 0;
 };
 
 } // namespace srsue
diff --git a/srsue/hdr/phy/vnf_phy_nr.h b/srsue/hdr/phy/vnf_phy_nr.h
index a88f29b8f..c5e805ca6 100644
--- a/srsue/hdr/phy/vnf_phy_nr.h
+++ b/srsue/hdr/phy/vnf_phy_nr.h
@@ -47,7 +47,7 @@ public:
   void stop() override;
 
   void wait_initialize() override;
-  void get_metrics(phy_metrics_t* m) override;
+  void get_metrics(const srsran::srsran_rat_t& rat, phy_metrics_t* m) override;
 
   std::string get_type() override { return "vnf_nr"; };
 
diff --git a/srsue/hdr/stack/mac_nr/mac_nr.h b/srsue/hdr/stack/mac_nr/mac_nr.h
index 7e5b1cb6d..034c6b31b 100644
--- a/srsue/hdr/stack/mac_nr/mac_nr.h
+++ b/srsue/hdr/stack/mac_nr/mac_nr.h
@@ -34,6 +34,7 @@
 #include "srsue/hdr/stack/mac_common/mac_common.h"
 #include "srsue/hdr/stack/mac_nr/mux_nr.h"
 #include "srsue/hdr/stack/ue_stack_base.h"
+#include "ul_harq_nr.h"
 
 namespace srsue {
 
@@ -44,7 +45,9 @@ struct mac_nr_args_t {};
 class mac_nr final : public mac_interface_phy_nr,
                      public mac_interface_rrc_nr,
                      public mac_interface_proc_ra_nr,
-                     public mac_interface_mux_nr
+                     public mac_interface_sr_nr,
+                     public mac_interface_mux_nr,
+                     public mac_interface_harq_nr
 {
 public:
   mac_nr(srsran::ext_task_sched_handle task_sched_);
@@ -85,11 +88,19 @@ public:
   void set_config(const srsran::rach_nr_cfg_t& rach_cfg);
   void set_contention_id(const uint64_t ue_identity);
   bool set_crnti(const uint16_t crnti);
+  int  add_tag_config(const srsran::tag_cfg_nr_t& tag_cfg);
+  int  set_config(const srsran::phr_cfg_nr_t& phr_cfg);
+  int  remove_tag_config(const uint32_t tag_id);
   void start_ra_procedure();
 
-  /// procedure ra nr interface + mux
+  /// Interface for internal procedures (RA, MUX, HARQ)
   uint64_t get_contention_id();
   uint16_t get_crnti();
+  uint16_t get_temp_crnti();
+  uint16_t get_csrnti() { return SRSRAN_INVALID_RNTI; }; // SPS not supported
+
+  /// procedure sr nr interface
+  void start_ra() { proc_ra.start_by_mac(); }
 
   /// Interface for MUX
   srsran::mac_sch_subpdu_nr::lcg_bsr_t generate_sbsr();
@@ -138,7 +149,7 @@ private:
   srslog::basic_logger& logger;
   mac_nr_args_t         args = {};
 
-  bool started = false;
+  std::atomic<bool> started = {false};
 
   uint16_t c_rnti        = SRSRAN_INVALID_RNTI;
   uint64_t contention_id = 0;
@@ -151,11 +162,6 @@ private:
   /// Rx buffer
   srsran::mac_sch_pdu_nr rx_pdu;
 
-  /// Tx buffer
-  srsran::unique_byte_buffer_t ul_harq_buffer = nullptr; // store PDU generated from MUX
-  srsran::unique_byte_buffer_t rlc_buffer     = nullptr;
-  srsran_softbuffer_tx_t       softbuffer_tx  = {}; /// UL HARQ (temporal)
-
   srsran::task_multiqueue::queue_handle stack_task_dispatch_queue;
 
   // MAC Uplink-related procedures
@@ -163,6 +169,12 @@ private:
   proc_sr_nr  proc_sr;
   proc_bsr_nr proc_bsr;
   mux_nr      mux;
+
+  // UL HARQ
+  ul_harq_entity_nr_vector ul_harq = {};
+  ul_harq_cfg_t            ul_harq_cfg;
+
+  const uint8_t PCELL_CC_IDX = 0;
 };
 
 } // namespace srsue
diff --git a/srsue/hdr/stack/mac_nr/mac_nr_interfaces.h b/srsue/hdr/stack/mac_nr/mac_nr_interfaces.h
index 9afcc49df..ca6143d71 100644
--- a/srsue/hdr/stack/mac_nr/mac_nr_interfaces.h
+++ b/srsue/hdr/stack/mac_nr/mac_nr_interfaces.h
@@ -48,7 +48,17 @@ public:
 };
 
 /**
- * @brief Interface from MAC NR parent class to mux ubclass
+ * @brief Interface from MAC NR parent class to SR subclass
+ */
+class mac_interface_sr_nr
+{
+public:
+  // SR can query MAC (as proxy for RA) to start RA procedure
+  virtual void start_ra() = 0;
+};
+
+/**
+ * @brief Interface from MAC NR parent class to mux subclass
  */
 class mac_interface_mux_nr
 {
@@ -60,6 +70,22 @@ public:
   virtual srsran::mac_sch_subpdu_nr::lcg_bsr_t generate_sbsr() = 0;
 };
 
+/**
+ * @brief Interface from MAC NR parent class to HARQ subclass
+ */
+class mac_interface_harq_nr
+{
+public:
+  // HARQ can query MAC for current C-RNTI
+  virtual uint16_t get_crnti() = 0;
+
+  // MAC also provides Temp C-RNTI (through RA proc)
+  virtual uint16_t get_temp_crnti() = 0;
+
+  // MAC provides the Currently Scheduled RNTI (for SPS)
+  virtual uint16_t get_csrnti() = 0;
+};
+
 } // namespace srsue
 
 #endif // SRSUE_MAC_NR_INTERFACES_H
\ No newline at end of file
diff --git a/srsue/hdr/stack/mac_nr/proc_ra_nr.h b/srsue/hdr/stack/mac_nr/proc_ra_nr.h
index 683fe0d0e..c1fa415d5 100644
--- a/srsue/hdr/stack/mac_nr/proc_ra_nr.h
+++ b/srsue/hdr/stack/mac_nr/proc_ra_nr.h
@@ -47,9 +47,9 @@ public:
   bool is_rar_opportunity(uint32_t tti);
   bool has_rar_rnti();
   uint16_t get_rar_rnti();
-  bool has_temp_rnti();
-  uint16_t get_temp_rnti();
-  
+  bool     has_temp_crnti();
+  uint16_t get_temp_crnti();
+
   // PHY interfaces
   void prach_sent(uint32_t tti, uint32_t s_id, uint32_t t_id, uint32_t f_id, uint32_t ul_carrier_id);
   void handle_rar_pdu(mac_interface_phy_nr::mac_nr_grant_dl_t& grant);
@@ -68,7 +68,7 @@ private:
 
   int      ra_window_length = -1, ra_window_start = -1;
   uint16_t rar_rnti  = SRSRAN_INVALID_RNTI;
-  uint16_t temp_rnti = SRSRAN_INVALID_RNTI;
+  uint16_t temp_crnti = SRSRAN_INVALID_RNTI;
 
   srsran::rach_nr_cfg_t rach_cfg   = {};
   bool                  configured = false;
diff --git a/srsue/hdr/stack/mac_nr/proc_sr_nr.h b/srsue/hdr/stack/mac_nr/proc_sr_nr.h
index 16b71e94e..da7b7e07b 100644
--- a/srsue/hdr/stack/mac_nr/proc_sr_nr.h
+++ b/srsue/hdr/stack/mac_nr/proc_sr_nr.h
@@ -22,7 +22,9 @@
 #ifndef SRSUE_PROC_SR_NR_H
 #define SRSUE_PROC_SR_NR_H
 
+#include "srsue/hdr/stack/mac_nr/mac_nr_interfaces.h"
 #include "srsran/interfaces/ue_mac_interfaces.h"
+#include "srsran/interfaces/ue_nr_interfaces.h"
 #include "srsran/srslog/srslog.h"
 #include <stdint.h>
 
@@ -32,14 +34,13 @@
 namespace srsue {
 
 class proc_ra_nr;
-class phy_interface_mac_nr;
 class rrc_interface_mac;
 
 class proc_sr_nr
 {
 public:
   explicit proc_sr_nr(srslog::basic_logger& logger);
-  int32_t init(proc_ra_nr* ra_, phy_interface_mac_nr* phy_, rrc_interface_mac* rrc_);
+  int32_t init(mac_interface_sr_nr* mac_, phy_interface_mac_nr* phy_, rrc_interface_mac* rrc_);
   void    step(uint32_t tti);
   int32_t set_config(const srsran::sr_cfg_nr_t& cfg);
   void    reset();
@@ -48,11 +49,11 @@ public:
 
 private:
   int  sr_counter    = 0;
-  bool is_pending_sr = 0;
+  bool is_pending_sr = false;
 
   srsran::sr_cfg_nr_t cfg = {};
 
-  proc_ra_nr*           ra  = nullptr;
+  mac_interface_sr_nr*  mac = nullptr;
   rrc_interface_mac*    rrc = nullptr;
   phy_interface_mac_nr* phy = nullptr;
   srslog::basic_logger& logger;
diff --git a/srsue/hdr/stack/mac_nr/ul_harq_nr.h b/srsue/hdr/stack/mac_nr/ul_harq_nr.h
new file mode 100644
index 000000000..bdb90f632
--- /dev/null
+++ b/srsue/hdr/stack/mac_nr/ul_harq_nr.h
@@ -0,0 +1,108 @@
+/**
+ *
+ * \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 SRSUE_UL_HARQ_NR_H
+#define SRSUE_UL_HARQ_NR_H
+
+#include "mux_nr.h"
+#include "proc_ra_nr.h"
+#include "srsran/common/interfaces_common.h"
+#include "srsran/common/timers.h"
+#include "srsran/interfaces/ue_nr_interfaces.h"
+
+using namespace srsran;
+
+namespace srsue {
+
+class ul_harq_entity_nr
+{
+public:
+  ul_harq_entity_nr(const uint8_t cc_idx_, mac_interface_harq_nr* mac_, proc_ra_nr* ra_proc__, mux_nr* mux_);
+
+  int init();
+
+  void reset();
+  void reset_ndi();
+  void set_config(srsran::ul_harq_cfg_t& harq_cfg);
+
+  /***************** PHY->MAC interface for UL processes **************************/
+  void new_grant_ul(const mac_interface_phy_nr::mac_nr_grant_ul_t& grant, mac_interface_phy_nr::tb_action_ul_t* action);
+
+  int   get_current_tbs(uint32_t pid);
+  float get_average_retx();
+
+private:
+  class ul_harq_process_nr
+  {
+  public:
+    ul_harq_process_nr();
+    ~ul_harq_process_nr();
+
+    bool init(uint32_t pid_, ul_harq_entity_nr* entity_);
+    void reset();
+    void reset_ndi();
+    uint8_t get_ndi();
+    bool    has_grant();
+
+    uint32_t get_nof_retx();
+    int      get_current_tbs();
+
+    /**
+     * Implements Section 5.4.2.1
+     *
+     * @param grant       The unmodified grant as received from PHY
+     * @param ndi_toggled The NDI toggled state determined by the entity
+     * @param action      The resulting UL action structure to be filled.
+     */
+    void new_grant_ul(const mac_interface_phy_nr::mac_nr_grant_ul_t& grant,
+                      const bool&                                    ndi_toggled,
+                      mac_interface_phy_nr::tb_action_ul_t*          action);
+
+  private:
+    mac_interface_phy_nr::mac_nr_grant_ul_t current_grant = {};
+    bool                                    grant_configured = false;
+
+    uint32_t pid          = 0;
+    uint32_t nof_retx     = 0;
+    bool     is_initiated = false;
+
+    srslog::basic_logger&  logger;
+    ul_harq_entity_nr*     harq_entity = nullptr;
+    srsran_softbuffer_tx_t softbuffer;
+
+    std::unique_ptr<byte_buffer_t> harq_buffer = nullptr;
+
+    void generate_tx(mac_interface_phy_nr::tb_action_ul_t* action);
+    void generate_new_tx(const mac_interface_phy_nr::mac_nr_grant_ul_t& grant,
+                         mac_interface_phy_nr::tb_action_ul_t*          action);
+    void generate_retx(const mac_interface_phy_nr::mac_nr_grant_ul_t& grant,
+                       mac_interface_phy_nr::tb_action_ul_t*          action);
+  };
+
+  std::array<ul_harq_process_nr, SRSRAN_MAX_HARQ_PROC_UL_NR> harq_procs;
+
+  mac_interface_harq_nr* mac = nullptr;
+  mux_nr*                mux = nullptr;
+  srslog::basic_logger&  logger;
+
+  srsran::ul_harq_cfg_t harq_cfg = {};
+
+  float    average_retx = 0.0;
+  uint64_t nof_pkts     = 0;
+};
+
+typedef std::unique_ptr<ul_harq_entity_nr>                     ul_harq_entity_nr_ptr;
+typedef std::array<ul_harq_entity_nr_ptr, SRSRAN_MAX_CARRIERS> ul_harq_entity_nr_vector;
+
+} // namespace srsue
+
+#endif // SRSUE_UL_HARQ_NR_H
diff --git a/srsue/hdr/stack/rrc/rrc.h b/srsue/hdr/stack/rrc/rrc.h
index fc5bb55a3..e1a1cf030 100644
--- a/srsue/hdr/stack/rrc/rrc.h
+++ b/srsue/hdr/stack/rrc/rrc.h
@@ -30,6 +30,7 @@
 #include "srsran/common/block_queue.h"
 #include "srsran/common/buffer_pool.h"
 #include "srsran/common/common.h"
+#include "srsran/common/lte_common.h"
 #include "srsran/common/security.h"
 #include "srsran/common/stack_procedure.h"
 #include "srsran/interfaces/ue_interfaces.h"
@@ -223,32 +224,9 @@ private:
   uint32_t                            n311_cnt = 0, N311 = 0;
   srsran::timer_handler::unique_timer t300, t301, t302, t310, t311, t304;
 
-  // Radio bearers
-  typedef enum {
-    RB_ID_SRB0 = 0,
-    RB_ID_SRB1,
-    RB_ID_SRB2,
-    RB_ID_DRB1,
-    RB_ID_DRB2,
-    RB_ID_DRB3,
-    RB_ID_DRB4,
-    RB_ID_DRB5,
-    RB_ID_DRB6,
-    RB_ID_DRB7,
-    RB_ID_DRB8,
-    RB_ID_MAX
-  } rb_id_t;
-
   static const std::string rb_id_str[];
 
-  std::string get_rb_name(uint32_t lcid)
-  {
-    if (lcid < RB_ID_MAX) {
-      return rb_id_str[lcid];
-    } else {
-      return "INVALID_RB";
-    }
-  }
+  const char* get_rb_name(uint32_t lcid) { return srsran::is_lte_rb(lcid) ? rb_id_str[lcid].c_str() : "invalid RB"; }
 
   // Measurements private subclass
   class rrc_meas;
diff --git a/srsue/hdr/stack/rrc/rrc_nr.h b/srsue/hdr/stack/rrc/rrc_nr.h
index 41852aa7b..3810bfdda 100644
--- a/srsue/hdr/stack/rrc/rrc_nr.h
+++ b/srsue/hdr/stack/rrc/rrc_nr.h
@@ -105,7 +105,6 @@ public:
   void in_sync() final;
   void out_of_sync() final;
 
-
   // RLC interface
   void max_retx_attempted() final;
 
@@ -182,7 +181,7 @@ private:
   // RRC constants and timers
   srsran::timer_handler* timers = nullptr;
 
-  std::string get_rb_name(uint32_t lcid) final { return srsran::to_string((srsran::rb_id_nr_t)lcid); }
+  const char* get_rb_name(uint32_t lcid) final { return srsran::to_string((srsran::rb_id_nr_t)lcid); }
 
   typedef enum { Srb = 0, Drb } rb_type_t;
   typedef struct {
@@ -227,7 +226,7 @@ private:
   {
   public:
     explicit connection_reconf_no_ho_proc(rrc_nr* parent_);
-    srsran::proc_outcome_t init(const reconf_initiator_t initiator_,
+    srsran::proc_outcome_t init(const reconf_initiator_t  initiator_,
                                 const bool                endc_release_and_add_r15,
                                 const bool                nr_secondary_cell_group_cfg_r15_present,
                                 const asn1::dyn_octstring nr_secondary_cell_group_cfg_r15,
diff --git a/srsue/hdr/stack/ue_stack_lte.h b/srsue/hdr/stack/ue_stack_lte.h
index e5a438813..ef866633f 100644
--- a/srsue/hdr/stack/ue_stack_lte.h
+++ b/srsue/hdr/stack/ue_stack_lte.h
@@ -190,6 +190,10 @@ private:
   srslog::basic_logger& usim_logger;
   srslog::basic_logger& nas_logger;
 
+  // UE nr stack logging
+  srslog::basic_logger& mac_nr_logger;
+  srslog::basic_logger& rrc_nr_logger;
+
   // tracing
   srsran::mac_pcap mac_pcap;
   srsran::mac_pcap mac_nr_pcap;
diff --git a/srsue/src/metrics_csv.cc b/srsue/src/metrics_csv.cc
index 8c5d7f45b..e4f0960c5 100644
--- a/srsue/src/metrics_csv.cc
+++ b/srsue/src/metrics_csv.cc
@@ -79,7 +79,7 @@ void metrics_csv::set_metrics_helper(const srsran::rf_metrics_t  rf,
                                      const phy_metrics_t         phy,
                                      const mac_metrics_t         mac[SRSRAN_MAX_CARRIERS],
                                      const rrc_metrics_t         rrc,
-                                     const uint32_t              cc, 
+                                     const uint32_t              cc,
                                      const uint32_t              r)
 {
   if (not file.is_open()) {
@@ -117,7 +117,7 @@ void metrics_csv::set_metrics_helper(const srsran::rf_metrics_t  rf,
 
   file << float_to_string(phy.dl[r].mcs, 2);
   file << float_to_string(phy.ch[r].sinr, 2);
-  file << float_to_string(phy.dl[r].turbo_iters, 2);
+  file << float_to_string(phy.dl[r].fec_iters, 2);
 
   if (mac[r].rx_brate > 0) {
     file << float_to_string(mac[r].rx_brate / (mac[r].nof_tti * 1e-3), 2);
@@ -200,20 +200,20 @@ void metrics_csv::set_metrics(const ue_metrics_t& metrics, const uint32_t period
       file << "\n";
     }
 
-    // Metrics for LTE carrier 
+    // Metrics for LTE carrier
     for (uint32_t r = 0; r < metrics.phy.nof_active_cc; r++) {
       set_metrics_helper(metrics.rf, metrics.sys, metrics.phy, metrics.stack.mac, metrics.stack.rrc, r, r);
     }
 
-    // Metrics for NR carrier 
+    // Metrics for NR carrier
     for (uint32_t r = 0; r < metrics.phy_nr.nof_active_cc; r++) {
       set_metrics_helper(metrics.rf,
                          metrics.sys,
                          metrics.phy_nr,
                          metrics.stack.mac_nr,
                          metrics.stack.rrc,
-                         r,
-                         metrics.phy.nof_active_cc + r);
+                         metrics.phy.nof_active_cc + r, // NR carrier offset
+                         r);
     }
 
     n_reports++;
diff --git a/srsue/src/metrics_stdout.cc b/srsue/src/metrics_stdout.cc
index e7e991285..e2f2ad9f5 100644
--- a/srsue/src/metrics_stdout.cc
+++ b/srsue/src/metrics_stdout.cc
@@ -119,7 +119,7 @@ void metrics_stdout::set_metrics_helper(const phy_metrics_t phy,
 
   cout << float_to_string(phy.dl[r].mcs, 2);
   cout << float_to_string(phy.ch[r].sinr, 2);
-  cout << float_to_string(phy.dl[r].turbo_iters, 2);
+  cout << float_to_string(phy.dl[r].fec_iters, 2);
 
   cout << float_to_eng_string((float)mac[r].rx_brate / (mac[r].nof_tti * 1e-3), 2);
   if (mac[r].rx_pkts > 0) {
@@ -185,7 +185,7 @@ void metrics_stdout::set_metrics(const ue_metrics_t& metrics, const uint32_t per
 
   for (uint32_t r = 0; r < metrics.phy_nr.nof_active_cc; r++) {
     // Assumption LTE is followed by the NR carriers.
-    cout << std::setw(2) << metrics.phy.nof_active_cc + r;
+    cout << std::setw(2) << metrics.phy_nr.nof_active_cc + r;
     set_metrics_helper(metrics.phy_nr, metrics.stack.mac_nr, metrics.stack.rrc, display_neighbours, r);
   }
 
diff --git a/srsue/src/phy/lte/cc_worker.cc b/srsue/src/phy/lte/cc_worker.cc
index e29cdb39b..9b21ef6a1 100644
--- a/srsue/src/phy/lte/cc_worker.cc
+++ b/srsue/src/phy/lte/cc_worker.cc
@@ -482,7 +482,7 @@ int cc_worker::decode_pdsch(srsran_pdsch_ack_resource_t            ack_resource,
     } else {
       dl_metrics.mcs = (ue_dl_cfg.cfg.pdsch.grant.tb[0].mcs_idx + ue_dl_cfg.cfg.pdsch.grant.tb[1].mcs_idx) / 2;
     }
-    dl_metrics.turbo_iters = pdsch_dec->avg_iterations_block / 2;
+    dl_metrics.fec_iters = pdsch_dec->avg_iterations_block / 2;
     phy->set_dl_metrics(cc_idx, dl_metrics);
 
     // Logging
@@ -516,7 +516,7 @@ int cc_worker::decode_pmch(mac_interface_phy_lte::tb_action_dl_t* action, srsran
     // Metrics
     dl_metrics_t dl_metrics = {};
     dl_metrics.mcs          = ue_dl_cfg.cfg.pdsch.grant.tb[0].mcs_idx;
-    dl_metrics.turbo_iters  = pmch_dec.avg_iterations_block / 2;
+    dl_metrics.fec_iters    = pmch_dec.avg_iterations_block / 2;
     phy->set_dl_metrics(cc_idx, dl_metrics);
 
     Info("PMCH: l_crb=%2d, tbs=%d, mcs=%d, crc=%s, snr=%.1f dB, n_iter=%.1f",
diff --git a/srsue/src/phy/lte/sf_worker.cc b/srsue/src/phy/lte/sf_worker.cc
index 828913666..e4eed3802 100644
--- a/srsue/src/phy/lte/sf_worker.cc
+++ b/srsue/src/phy/lte/sf_worker.cc
@@ -316,7 +316,7 @@ int sf_worker::read_pdsch_d(cf_t* pdsch_d)
 
 float sf_worker::get_cfo()
 {
-  sync_metrics_t sync_metrics[SRSRAN_MAX_CARRIERS] = {};
+  sync_metrics_t::array_t sync_metrics = {};
   phy->get_sync_metrics(sync_metrics);
   return sync_metrics[0].cfo;
 }
diff --git a/srsue/src/phy/nr/cc_worker.cc b/srsue/src/phy/nr/cc_worker.cc
index 3a1324caa..2ba043b0a 100644
--- a/srsue/src/phy/nr/cc_worker.cc
+++ b/srsue/src/phy/nr/cc_worker.cc
@@ -262,12 +262,12 @@ bool cc_worker::work_dl()
     srsran_softbuffer_rx_reset(&softbuffer_rx);
 
     // Initialise PDSCH Result
-    std::array<srsran_pdsch_res_nr_t, SRSRAN_MAX_CODEWORDS> pdsch_res = {};
-    pdsch_res[0].payload                                              = data->msg;
-    pdsch_cfg.grant.tb[0].softbuffer.rx                               = &softbuffer_rx;
+    srsran_pdsch_res_nr_t pdsch_res     = {};
+    pdsch_res.tb[0].payload             = data->msg;
+    pdsch_cfg.grant.tb[0].softbuffer.rx = &softbuffer_rx;
 
     // Decode actual PDSCH transmission
-    if (srsran_ue_dl_nr_decode_pdsch(&ue_dl, &dl_slot_cfg, &pdsch_cfg, pdsch_res.data()) < SRSRAN_SUCCESS) {
+    if (srsran_ue_dl_nr_decode_pdsch(&ue_dl, &dl_slot_cfg, &pdsch_cfg, &pdsch_res) < SRSRAN_SUCCESS) {
       ERROR("Error decoding PDSCH");
       return false;
     }
@@ -275,17 +275,17 @@ bool cc_worker::work_dl()
     // Logging
     if (logger.info.enabled()) {
       std::array<char, 512> str;
-      srsran_ue_dl_nr_pdsch_info(&ue_dl, &pdsch_cfg, pdsch_res.data(), str.data(), str.size());
-      logger.info(pdsch_res[0].payload, pdsch_cfg.grant.tb[0].tbs / 8, "PDSCH: cc=%d, %s", cc_idx, str.data());
+      srsran_ue_dl_nr_pdsch_info(&ue_dl, &pdsch_cfg, &pdsch_res, str.data(), str.size());
+      logger.info(pdsch_res.tb[0].payload, pdsch_cfg.grant.tb[0].tbs / 8, "PDSCH: cc=%d, %s", cc_idx, str.data());
     }
 
     // Enqueue PDSCH ACK information only if the RNTI is type C
     if (pdsch_cfg.grant.rnti_type == srsran_rnti_type_c) {
-      phy->set_pending_ack(dl_slot_cfg.idx, ack_resource, pdsch_res[0].crc);
+      phy->set_pending_ack(dl_slot_cfg.idx, ack_resource, pdsch_res.tb[0].crc);
     }
 
     // Notify MAC about PDSCH decoding result
-    if (pdsch_res[0].crc) {
+    if (pdsch_res.tb[0].crc) {
       // Prepare grant
       mac_interface_phy_nr::mac_nr_grant_dl_t mac_nr_grant = {};
       mac_nr_grant.tb[0]                                   = std::move(data);
@@ -299,6 +299,26 @@ bool cc_worker::work_dl()
 
       // Send data to MAC
       phy->stack->tb_decoded(cc_idx, mac_nr_grant);
+
+      // Generate DL metrics
+      dl_metrics_t dl_m = {};
+      dl_m.mcs          = pdsch_cfg.grant.tb[0].mcs;
+      dl_m.fec_iters    = pdsch_res.tb[0].avg_iter;
+      dl_m.evm          = pdsch_res.evm[0];
+      phy->set_dl_metrics(dl_m);
+
+      // Generate Synch metrics
+      sync_metrics_t sync_m = {};
+      sync_m.cfo            = ue_dl.chest.cfo;
+      phy->set_sync_metrics(sync_m);
+
+      // Generate channel metrics
+      ch_metrics_t ch_m = {};
+      ch_m.n            = ue_dl.chest.noise_estimate;
+      ch_m.sinr         = ue_dl.chest.snr_db;
+      ch_m.rsrp         = ue_dl.chest.rsrp_dbm;
+      ch_m.sync_err     = ue_dl.chest.sync_error;
+      phy->set_channel_metrics(ch_m);
     }
   }
 
@@ -356,8 +376,17 @@ bool cc_worker::work_ul()
     mac_ul_grant.rnti                                    = pusch_cfg.grant.rnti;
     mac_ul_grant.tti                                     = ul_slot_cfg.idx;
     mac_ul_grant.tbs                                     = pusch_cfg.grant.tb[0].tbs / 8;
+    mac_ul_grant.ndi                                     = pusch_cfg.grant.tb[0].ndi;
+    mac_ul_grant.rv                                      = pusch_cfg.grant.tb[0].rv;
+    mac_ul_grant.is_rar_grant                            = (pusch_cfg.grant.rnti_type == srsran_rnti_type_ra);
     phy->stack->new_grant_ul(0, mac_ul_grant, &ul_action);
 
+    // Don't process further if MAC can't provide PDU
+    if (not ul_action.tb.enabled) {
+      ERROR("No MAC PDU provided by MAC");
+      return false;
+    }
+
     // Set UCI configuration following procedures
     srsran_ra_ul_set_grant_uci_nr(&phy->cfg.pusch, &uci_data.cfg, &pusch_cfg);
 
@@ -366,7 +395,7 @@ bool cc_worker::work_ul()
 
     // Setup data for encoding
     srsran_pusch_data_nr_t data = {};
-    data.payload                = ul_action.tb.payload->msg;
+    data.payload[0]             = ul_action.tb.payload->msg;
     data.uci                    = uci_data.value;
 
     // Encode PUSCH transmission
@@ -381,11 +410,18 @@ bool cc_worker::work_ul()
       srsran_ue_ul_nr_pusch_info(&ue_ul, &pusch_cfg, &data.uci, str.data(), str.size());
       logger.info(ul_action.tb.payload->msg,
                   pusch_cfg.grant.tb[0].tbs / 8,
-                  "PUSCH (NR): cc=%d, %s, tti_tx=%d",
+                  "PUSCH: cc=%d, %s, tti_tx=%d",
                   cc_idx,
                   str.data(),
                   ul_slot_cfg.idx);
     }
+
+    // Set metrics
+    ul_metrics_t ul_m = {};
+    ul_m.mcs          = pusch_cfg.grant.tb[0].mcs;
+    ul_m.power        = srsran_convert_power_to_dB(srsran_vec_avg_power_cf(tx_buffer[0], ue_ul.ifft.sf_sz));
+    phy->set_ul_metrics(ul_m);
+
   } else if (srsran_uci_nr_total_bits(&uci_data.cfg) > 0) {
     // Get PUCCH resource
     srsran_pucch_nr_resource_t resource = {};
diff --git a/srsue/src/phy/nr/sf_worker.cc b/srsue/src/phy/nr/sf_worker.cc
index 1fbb1570c..c04d4ce19 100644
--- a/srsue/src/phy/nr/sf_worker.cc
+++ b/srsue/src/phy/nr/sf_worker.cc
@@ -106,8 +106,11 @@ void sf_worker::work_imp()
     tx_buffer.set(0, prach_ptr);
 
     // Notify MAC about PRACH transmission
-    phy_state->stack->prach_sent(
-        TTI_TX(tti_rx), 0, SRSRAN_SLOT_NR_MOD(phy_state->carrier.numerology, TTI_TX(tti_rx)), 0, 0);
+    phy_state->stack->prach_sent(TTI_TX(tti_rx),
+                                 srsran_prach_nr_start_symbol_fr1_unpaired(phy_state->cfg.prach.config_idx),
+                                 SRSRAN_SLOT_NR_MOD(phy_state->carrier.numerology, TTI_TX(tti_rx)),
+                                 0,
+                                 0);
 
     // Transmit NR PRACH
     phy->worker_end(this, false, tx_buffer, dummy_ts, true);
diff --git a/srsue/src/phy/nr/worker_pool.cc b/srsue/src/phy/nr/worker_pool.cc
index 391e40571..f12e9d91f 100644
--- a/srsue/src/phy/nr/worker_pool.cc
+++ b/srsue/src/phy/nr/worker_pool.cc
@@ -23,7 +23,7 @@
 namespace srsue {
 namespace nr {
 
-worker_pool::worker_pool(uint32_t max_workers) : pool(max_workers), logger(srslog::fetch_basic_logger("NR-PHY")) {}
+worker_pool::worker_pool(uint32_t max_workers) : pool(max_workers), logger(srslog::fetch_basic_logger("PHY-NR")) {}
 
 bool worker_pool::init(const phy_args_nr_t& args, phy_common* common, stack_interface_phy_nr* stack_, int prio)
 {
@@ -45,7 +45,7 @@ bool worker_pool::init(const phy_args_nr_t& args, phy_common* common, stack_inte
 
   // Add workers to workers pool and start threads
   for (uint32_t i = 0; i < args.nof_phy_threads; i++) {
-    auto& log = srslog::fetch_basic_logger(fmt::format("PHY{}", i));
+    auto& log = srslog::fetch_basic_logger(fmt::format("PHY{}-NR", i));
     log.set_level(srslog::str_to_basic_level(args.log.phy_level));
     log.set_hex_dump_max_size(args.log.phy_hex_limit);
 
@@ -59,7 +59,7 @@ bool worker_pool::init(const phy_args_nr_t& args, phy_common* common, stack_inte
   }
 
   // Initialise PRACH
-  auto& prach_log = srslog::fetch_basic_logger("NR-PRACH");
+  auto& prach_log = srslog::fetch_basic_logger("PRACH-NR");
   prach_log.set_level(srslog::str_to_basic_level(args.log.phy_level));
   prach_buffer = std::unique_ptr<prach>(new prach(prach_log));
   prach_buffer->init(phy_state.args.dl.nof_max_prb);
@@ -164,5 +164,10 @@ void worker_pool::clear_pending_grants()
   phy_state.clear_pending_grants();
 }
 
+void worker_pool::get_metrics(phy_metrics_t& m)
+{
+  phy_state.get_metrics(m);
+}
+
 } // namespace nr
 } // namespace srsue
diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc
index c41e037e5..aefb010e0 100644
--- a/srsue/src/phy/phy.cc
+++ b/srsue/src/phy/phy.cc
@@ -193,24 +193,40 @@ void phy::stop()
   }
 }
 
-void phy::get_metrics(phy_metrics_t* m)
-{
-  uint32_t      dl_earfcn = 0;
-  srsran_cell_t cell      = {};
-  sfsync.get_current_cell(&cell, &dl_earfcn);
-  m->info[0].pci       = cell.id;
-  m->info[0].dl_earfcn = dl_earfcn;
+void phy::get_metrics(const srsran::srsran_rat_t& rat, phy_metrics_t* m)
+{
+  // Zero structure by default
+  *m = {};
+
+  // Get LTE metrics
+  if (rat == srsran::srsran_rat_t::lte && args.nof_lte_carriers > 0) {
+    uint32_t      dl_earfcn = 0;
+    srsran_cell_t cell      = {};
+    sfsync.get_current_cell(&cell, &dl_earfcn);
+    m->info[0].pci       = cell.id;
+    m->info[0].dl_earfcn = dl_earfcn;
+
+    for (uint32_t i = 1; i < args.nof_lte_carriers; i++) {
+      m->info[i].dl_earfcn = common.cell_state.get_earfcn(i);
+      m->info[i].pci       = common.cell_state.get_pci(i);
+    }
+
+    common.get_ch_metrics(m->ch);
+    common.get_dl_metrics(m->dl);
+    common.get_ul_metrics(m->ul);
+    common.get_sync_metrics(m->sync);
+    m->nof_active_cc = args.nof_lte_carriers;
+    return;
+  }
 
-  for (uint32_t i = 1; i < args.nof_lte_carriers; i++) {
-    m->info[i].dl_earfcn = common.cell_state.get_earfcn(i);
-    m->info[i].pci       = common.cell_state.get_pci(i);
+  // Get NR metrics
+  if (rat == srsran::srsran_rat_t::nr && args.nof_nr_carriers > 0) {
+    nr_workers.get_metrics(*m);
+    return;
   }
 
-  common.get_ch_metrics(m->ch);
-  common.get_dl_metrics(m->dl);
-  common.get_ul_metrics(m->ul);
-  common.get_sync_metrics(m->sync);
-  m->nof_active_cc = args.nof_lte_carriers;
+  // Add other RAT here
+  // ...
 }
 
 void phy::set_timeadv_rar(uint32_t tti, uint32_t ta_cmd)
diff --git a/srsue/src/phy/phy_common.cc b/srsue/src/phy/phy_common.cc
index b8a1d4da9..2da8ab7dd 100644
--- a/srsue/src/phy/phy_common.cc
+++ b/srsue/src/phy/phy_common.cc
@@ -801,91 +801,64 @@ void phy_common::update_measurements(uint32_t                 cc_idx,
 void phy_common::set_dl_metrics(uint32_t cc_idx, const dl_metrics_t& m)
 {
   std::unique_lock<std::mutex> lock(metrics_mutex);
-
-  dl_metrics_count[cc_idx]++;
-  dl_metrics[cc_idx].mcs = dl_metrics[cc_idx].mcs + (m.mcs - dl_metrics[cc_idx].mcs) / dl_metrics_count[cc_idx];
-  dl_metrics[cc_idx].turbo_iters =
-      dl_metrics[cc_idx].turbo_iters + (m.turbo_iters - dl_metrics[cc_idx].turbo_iters) / dl_metrics_count[cc_idx];
+  dl_metrics[cc_idx].set(m);
 }
 
-void phy_common::get_dl_metrics(dl_metrics_t m[SRSRAN_MAX_CARRIERS])
+void phy_common::get_dl_metrics(dl_metrics_t::array_t& m)
 {
   std::unique_lock<std::mutex> lock(metrics_mutex);
 
   for (uint32_t i = 0; i < args->nof_lte_carriers; i++) {
-    m[i]                = dl_metrics[i];
-    dl_metrics[i]       = {};
-    dl_metrics_count[i] = 0;
+    m[i] = dl_metrics[i];
+    dl_metrics[i].reset();
   }
 }
 
 void phy_common::set_ch_metrics(uint32_t cc_idx, const ch_metrics_t& m)
 {
   std::unique_lock<std::mutex> lock(metrics_mutex);
-
-  ch_metrics_count[cc_idx]++;
-  ch_metrics[cc_idx].n    = ch_metrics[cc_idx].n + (m.n - ch_metrics[cc_idx].n) / ch_metrics_count[cc_idx];
-  ch_metrics[cc_idx].rsrq = ch_metrics[cc_idx].rsrq + (m.rsrq - ch_metrics[cc_idx].rsrq) / ch_metrics_count[cc_idx];
-  ch_metrics[cc_idx].rssi = ch_metrics[cc_idx].rssi + (m.rssi - ch_metrics[cc_idx].rssi) / ch_metrics_count[cc_idx];
-  ch_metrics[cc_idx].rsrp = ch_metrics[cc_idx].rsrp + (m.rsrp - ch_metrics[cc_idx].rsrp) / ch_metrics_count[cc_idx];
-  ch_metrics[cc_idx].sinr = ch_metrics[cc_idx].sinr + (m.sinr - ch_metrics[cc_idx].sinr) / ch_metrics_count[cc_idx];
-  ch_metrics[cc_idx].sync_err =
-      ch_metrics[cc_idx].sync_err + (m.sync_err - ch_metrics[cc_idx].sync_err) / ch_metrics_count[cc_idx];
-  ch_metrics[cc_idx].pathloss =
-      ch_metrics[cc_idx].pathloss + (m.pathloss - ch_metrics[cc_idx].pathloss) / ch_metrics_count[cc_idx];
+  ch_metrics[cc_idx].set(m);
 }
 
-void phy_common::get_ch_metrics(ch_metrics_t m[SRSRAN_MAX_CARRIERS])
+void phy_common::get_ch_metrics(ch_metrics_t::array_t& m)
 {
   std::unique_lock<std::mutex> lock(metrics_mutex);
 
   for (uint32_t i = 0; i < args->nof_lte_carriers; i++) {
-    m[i]                = ch_metrics[i];
-    ch_metrics[i]       = {};
-    ch_metrics_count[i] = 0;
+    m[i] = ch_metrics[i];
+    ch_metrics[i].reset();
   }
 }
 
 void phy_common::set_ul_metrics(uint32_t cc_idx, const ul_metrics_t& m)
 {
   std::unique_lock<std::mutex> lock(metrics_mutex);
-
-  ul_metrics_count[cc_idx]++;
-  ul_metrics[cc_idx].mcs   = ul_metrics[cc_idx].mcs + (m.mcs - ul_metrics[cc_idx].mcs) / ul_metrics_count[cc_idx];
-  ul_metrics[cc_idx].power = ul_metrics[cc_idx].power + (m.power - ul_metrics[cc_idx].power) / ul_metrics_count[cc_idx];
+  ul_metrics[cc_idx].set(m);
 }
 
-void phy_common::get_ul_metrics(ul_metrics_t m[SRSRAN_MAX_CARRIERS])
+void phy_common::get_ul_metrics(ul_metrics_t::array_t& m)
 {
   std::unique_lock<std::mutex> lock(metrics_mutex);
 
   for (uint32_t i = 0; i < args->nof_lte_carriers; i++) {
-    m[i]                = ul_metrics[i];
-    ul_metrics[i]       = {};
-    ul_metrics_count[i] = 0;
+    m[i] = ul_metrics[i];
+    ul_metrics[i].reset();
   }
 }
 
 void phy_common::set_sync_metrics(const uint32_t& cc_idx, const sync_metrics_t& m)
 {
   std::unique_lock<std::mutex> lock(metrics_mutex);
-
-  sync_metrics_count[cc_idx]++;
-  sync_metrics[cc_idx].cfo = sync_metrics[cc_idx].cfo + (m.cfo - sync_metrics[cc_idx].cfo) / sync_metrics_count[cc_idx];
-  sync_metrics[cc_idx].sfo = sync_metrics[cc_idx].sfo + (m.sfo - sync_metrics[cc_idx].sfo) / sync_metrics_count[cc_idx];
-  sync_metrics[cc_idx].ta_us = m.ta_us;
-  sync_metrics[cc_idx].distance_km = m.distance_km;
-  sync_metrics[cc_idx].speed_kmph  = m.speed_kmph;
+  sync_metrics[cc_idx].set(m);
 }
 
-void phy_common::get_sync_metrics(sync_metrics_t m[SRSRAN_MAX_CARRIERS])
+void phy_common::get_sync_metrics(sync_metrics_t::array_t& m)
 {
   std::unique_lock<std::mutex> lock(metrics_mutex);
 
   for (uint32_t i = 0; i < args->nof_lte_carriers; i++) {
-    m[i]                  = sync_metrics[i];
-    sync_metrics[i]       = {};
-    sync_metrics_count[i] = 0;
+    m[i] = sync_metrics[i];
+    sync_metrics[i].reset();
   }
 }
 
diff --git a/srsue/src/phy/vnf_phy_nr.cc b/srsue/src/phy/vnf_phy_nr.cc
index 297216908..13ce3ae0c 100644
--- a/srsue/src/phy/vnf_phy_nr.cc
+++ b/srsue/src/phy/vnf_phy_nr.cc
@@ -70,7 +70,7 @@ void vnf_phy_nr::start_plot() {}
 
 void vnf_phy_nr::wait_initialize() {}
 
-void vnf_phy_nr::get_metrics(phy_metrics_t* m) {}
+void vnf_phy_nr::get_metrics(const srsran::srsran_rat_t& rat, phy_metrics_t* m) {}
 
 int vnf_phy_nr::tx_request(const tx_request_t& request)
 {
diff --git a/srsue/src/stack/mac_nr/CMakeLists.txt b/srsue/src/stack/mac_nr/CMakeLists.txt
index c864f8b45..7ef2f8b86 100644
--- a/srsue/src/stack/mac_nr/CMakeLists.txt
+++ b/srsue/src/stack/mac_nr/CMakeLists.txt
@@ -18,7 +18,7 @@
 # and at http://www.gnu.org/licenses/.
 #
 
-set(SOURCES mac_nr.cc proc_ra_nr.cc proc_bsr_nr.cc proc_sr_nr.cc mux_nr.cc)
+set(SOURCES mac_nr.cc proc_ra_nr.cc proc_bsr_nr.cc proc_sr_nr.cc mux_nr.cc ul_harq_nr.cc)
 add_library(srsue_mac_nr STATIC ${SOURCES})
 target_link_libraries(srsue_mac_nr srsue_mac_common srsran_mac)
 
diff --git a/srsue/src/stack/mac_nr/mac_nr.cc b/srsue/src/stack/mac_nr/mac_nr.cc
index 84f937b79..bc96c6b9d 100644
--- a/srsue/src/stack/mac_nr/mac_nr.cc
+++ b/srsue/src/stack/mac_nr/mac_nr.cc
@@ -28,13 +28,16 @@ namespace srsue {
 
 mac_nr::mac_nr(srsran::ext_task_sched_handle task_sched_) :
   task_sched(task_sched_),
-  logger(srslog::fetch_basic_logger("MAC")),
+  logger(srslog::fetch_basic_logger("MAC-NR")),
   proc_ra(*this, logger),
   proc_sr(logger),
   proc_bsr(logger),
   mux(*this, logger),
   pcap(nullptr)
-{}
+{
+  // Create PCell HARQ entities
+  ul_harq.at(PCELL_CC_IDX) = ul_harq_entity_nr_ptr(new ul_harq_entity_nr(PCELL_CC_IDX, this, &proc_ra, &mux));
+}
 
 mac_nr::~mac_nr()
 {
@@ -56,7 +59,7 @@ int mac_nr::init(const mac_nr_args_t&  args_,
 
   // Init MAC sub procedures
   proc_ra.init(phy, &task_sched);
-  proc_sr.init(&proc_ra, phy, rrc);
+  proc_sr.init(this, phy, rrc);
 
   if (proc_bsr.init(&proc_sr, &mux, rlc, &task_sched) != SRSRAN_SUCCESS) {
     logger.error("Couldn't initialize BSR procedure.");
@@ -68,14 +71,9 @@ int mac_nr::init(const mac_nr_args_t&  args_,
     return SRSRAN_ERROR;
   }
 
-  if (srsran_softbuffer_tx_init_guru(&softbuffer_tx, SRSRAN_SCH_NR_MAX_NOF_CB_LDPC, SRSRAN_LDPC_MAX_LEN_ENCODED_CB) <
-      SRSRAN_SUCCESS) {
-    ERROR("Error init soft-buffer");
-    return SRSRAN_ERROR;
-  }
-
-  ul_harq_buffer = srsran::make_byte_buffer();
-  if (ul_harq_buffer == nullptr) {
+  // Configure PCell HARQ entities
+  if (ul_harq.at(PCELL_CC_IDX)->init() != SRSRAN_SUCCESS) {
+    logger.error("Couldn't initialize UL HARQ entity.");
     return SRSRAN_ERROR;
   }
 
@@ -94,8 +92,6 @@ void mac_nr::stop()
   if (started) {
     started = false;
   }
-
-  srsran_softbuffer_tx_free(&softbuffer_tx);
 }
 
 // Implement Section 5.9
@@ -176,9 +172,9 @@ mac_interface_phy_nr::sched_rnti_t mac_nr::get_dl_sched_rnti_nr(const uint32_t t
     return {proc_ra.get_rar_rnti(), srsran_rnti_type_ra};
   }
 
-  if (proc_ra.has_temp_rnti() && has_crnti() == false) {
-    logger.debug("SCHED: Searching temp C-RNTI=0x%x (proc_ra)", proc_ra.get_temp_rnti());
-    return {proc_ra.get_temp_rnti(), srsran_rnti_type_c};
+  if (proc_ra.has_temp_crnti() && has_crnti() == false) {
+    logger.debug("SCHED: Searching temp C-RNTI=0x%x (proc_ra)", proc_ra.get_temp_crnti());
+    return {proc_ra.get_temp_crnti(), srsran_rnti_type_c};
   }
 
   if (has_crnti()) {
@@ -200,6 +196,11 @@ uint16_t mac_nr::get_crnti()
   return c_rnti;
 }
 
+uint16_t mac_nr::get_temp_crnti()
+{
+  return proc_ra.get_temp_crnti();
+}
+
 srsran::mac_sch_subpdu_nr::lcg_bsr_t mac_nr::generate_sbsr()
 {
   return proc_bsr.generate_sbsr();
@@ -283,6 +284,19 @@ void mac_nr::tb_decoded(const uint32_t cc_idx, mac_nr_grant_dl_t& grant)
 
 void mac_nr::new_grant_ul(const uint32_t cc_idx, const mac_nr_grant_ul_t& grant, tb_action_ul_t* action)
 {
+  logger.debug("new_grant_ul(): cc_idx=%d, tti=%d, rnti=%d, pid=%d, tbs=%d, ndi=%d, rv=%d, is_rar=%d",
+               cc_idx,
+               grant.tti,
+               grant.rnti,
+               grant.pid,
+               grant.tbs,
+               grant.ndi,
+               grant.rv,
+               grant.is_rar_grant);
+
+  // Clear UL action
+  *action = {};
+
   // if proc ra is in contention resolution and c_rnti == grant.c_rnti resolve contention resolution
   if (proc_ra.is_contention_resolution() && grant.rnti == c_rnti) {
     proc_ra.pdcch_to_crnti();
@@ -291,28 +305,20 @@ void mac_nr::new_grant_ul(const uint32_t cc_idx, const mac_nr_grant_ul_t& grant,
   // Let BSR know there is a new grant, might have to send a BSR
   proc_bsr.new_grant_ul(grant.tbs);
 
-  // TODO: add proper UL-HARQ
-  // The code below assumes a single HARQ entity, no retx, every Tx is always a new transmission
-  ul_harq_buffer = mux.get_pdu(grant.tbs);
-
-  // fill TB action (goes into UL harq eventually)
-  if (ul_harq_buffer != nullptr) {
-    action->tb.payload    = ul_harq_buffer.get(); // pass handle to PDU to PHY
-    action->tb.enabled    = true;
-    action->tb.rv         = 0;
-    action->tb.softbuffer = &softbuffer_tx;
-    srsran_softbuffer_tx_reset(&softbuffer_tx);
-  } else {
-    action->tb.enabled = false;
+  // Assert UL HARQ entity
+  if (ul_harq.at(cc_idx) == nullptr) {
+    logger.error("HARQ entity %d has not been created", cc_idx);
+    return;
   }
 
+  ul_harq.at(cc_idx)->new_grant_ul(grant, action);
+  metrics[cc_idx].tx_pkts++;
+  metrics[cc_idx].tx_brate += grant.tbs * 8;
+
   // store PCAP
-  if (pcap) {
-    pcap->write_ul_crnti_nr(ul_harq_buffer->msg, ul_harq_buffer->N_bytes, grant.rnti, grant.pid, grant.tti);
+  if (action->tb.enabled && pcap) {
+    pcap->write_ul_crnti_nr(action->tb.payload->msg, action->tb.payload->N_bytes, grant.rnti, grant.pid, grant.tti);
   }
-
-  metrics[cc_idx].tx_brate += grant.tbs * 8;
-  metrics[cc_idx].tx_pkts++;
 }
 
 void mac_nr::timer_expired(uint32_t timer_id)
@@ -346,6 +352,24 @@ int mac_nr::setup_lcid(const srsran::logical_channel_config_t& config)
   return SRSRAN_SUCCESS;
 }
 
+int mac_nr::add_tag_config(const srsran::tag_cfg_nr_t& tag_cfg)
+{
+  logger.warning("Add tag config not supported yet");
+  return SRSRAN_SUCCESS;
+}
+
+int mac_nr::remove_tag_config(const uint32_t tag_id)
+{
+  logger.warning("Remove tag config not supported yet");
+  return SRSRAN_SUCCESS;
+}
+
+int mac_nr::set_config(const srsran::phr_cfg_nr_t& phr_cfg)
+{
+  logger.warning("Add phr config not supported yet");
+  return SRSRAN_SUCCESS;
+}
+
 int mac_nr::set_config(const srsran::bsr_cfg_nr_t& bsr_cfg)
 {
   return proc_bsr.set_config(bsr_cfg);
@@ -432,7 +456,9 @@ void mac_nr::handle_pdu(srsran::unique_byte_buffer_t pdu)
   logger.info(pdu->msg, pdu->N_bytes, "Handling MAC PDU (%d B)", pdu->N_bytes);
 
   rx_pdu.init_rx();
-  rx_pdu.unpack(pdu->msg, pdu->N_bytes);
+  if (rx_pdu.unpack(pdu->msg, pdu->N_bytes) != SRSRAN_SUCCESS) {
+    return;
+  }
 
   for (uint32_t i = 0; i < rx_pdu.get_num_subpdus(); ++i) {
     srsran::mac_sch_subpdu_nr subpdu = rx_pdu.get_subpdu(i);
diff --git a/srsue/src/stack/mac_nr/mux_nr.cc b/srsue/src/stack/mac_nr/mux_nr.cc
index a12917bbe..6a30f4f89 100644
--- a/srsue/src/stack/mac_nr/mux_nr.cc
+++ b/srsue/src/stack/mac_nr/mux_nr.cc
@@ -71,14 +71,10 @@ srsran::unique_byte_buffer_t mux_nr::get_pdu(uint32_t max_pdu_len)
   tx_pdu.init_tx(phy_tx_pdu.get(), max_pdu_len, true);
 
   if (msg3_is_pending()) {
-    // If message 3 is pending pack message 3 for uplink transmission
+    // If Msg3 is pending, pack it
     // Use the CRNTI which is provided in the RRC reconfiguration (only for DC mode maybe other)
     tx_pdu.add_crnti_ce(mac.get_crnti());
-    srsran::mac_sch_subpdu_nr::lcg_bsr_t sbsr = {};
-    sbsr.lcg_id                               = 0;
-    sbsr.buffer_size                          = 1;
-    tx_pdu.add_sbsr_ce(sbsr);
-    logger.info("Generated msg3 with RNTI 0x%x", mac.get_crnti());
+    tx_pdu.add_sbsr_ce(mac.generate_sbsr());
     msg3_transmitted();
   } else {
     // Pack normal UL data PDU
diff --git a/srsue/src/stack/mac_nr/proc_bsr_nr.cc b/srsue/src/stack/mac_nr/proc_bsr_nr.cc
index 1ec575b86..4483b1945 100644
--- a/srsue/src/stack/mac_nr/proc_bsr_nr.cc
+++ b/srsue/src/stack/mac_nr/proc_bsr_nr.cc
@@ -123,7 +123,7 @@ void proc_bsr_nr::set_trigger(bsr_trigger_type_t new_trigger)
   // Trigger SR always when Regular BSR is triggered in the current TTI. Will be cancelled if a grant is received
   if (triggered_bsr_type == REGULAR) {
     logger.debug("BSR:   Triggering SR procedure");
-    // sr->start();
+    sr->start();
   }
 }
 
@@ -265,6 +265,10 @@ void proc_bsr_nr::new_grant_ul(uint32_t grant_size)
     // 3> start or restart retxBSR-Timer.
     timer_retx.run();
   }
+
+  // Cancel SR if an UL grant is received
+  logger.debug("BSR:   Cancelling SR procedure due to UL grant");
+  sr->reset();
 }
 
 // This function is called by MUX only if Regular BSR has not been triggered before
diff --git a/srsue/src/stack/mac_nr/proc_ra_nr.cc b/srsue/src/stack/mac_nr/proc_ra_nr.cc
index b3f322fc7..7c46c7363 100644
--- a/srsue/src/stack/mac_nr/proc_ra_nr.cc
+++ b/srsue/src/stack/mac_nr/proc_ra_nr.cc
@@ -121,14 +121,14 @@ bool proc_ra_nr::has_rar_rnti()
   return false;
 }
 
-bool proc_ra_nr::has_temp_rnti()
+bool proc_ra_nr::has_temp_crnti()
 {
-  return temp_rnti != SRSRAN_INVALID_RNTI;
+  return temp_crnti != SRSRAN_INVALID_RNTI;
 }
 
-uint16_t proc_ra_nr::get_temp_rnti()
+uint16_t proc_ra_nr::get_temp_crnti()
 {
-  return temp_rnti;
+  return temp_crnti;
 }
 
 void proc_ra_nr::timer_expired(uint32_t timer_id)
@@ -210,11 +210,11 @@ void proc_ra_nr::ra_response_reception(const mac_interface_phy_nr::mac_nr_grant_
 
       for (auto& subpdu : pdu.get_subpdus()) {
         if (subpdu.has_rapid() && subpdu.get_rapid() == preamble_index) {
-          logger.info("PROC RA NR: Setting ul grant and prepare msg3");
-          temp_rnti = subpdu.get_temp_crnti();
+          logger.debug("PROC RA NR: Setting UL grant and prepare Msg3");
+          temp_crnti = subpdu.get_temp_crnti();
 
           // Set Temporary-C-RNTI if provided, otherwise C-RNTI is ok
-          phy->set_ul_grant(subpdu.get_ul_grant(), temp_rnti, srsran_rnti_type_c);
+          phy->set_ul_grant(subpdu.get_ul_grant(), temp_crnti, srsran_rnti_type_ra);
 
           // reset all parameters that are used before rar
           rar_rnti = SRSRAN_INVALID_RNTI;
@@ -281,13 +281,13 @@ void proc_ra_nr::ra_completion()
   }
   srsran::console("Random Access Complete.     c-rnti=0x%x, ta=%d\n", mac.get_crnti(), current_ta);
   logger.info("Random Access Complete.     c-rnti=0x%x, ta=%d", mac.get_crnti(), current_ta);
-  temp_rnti = SRSRAN_INVALID_RNTI;
+  temp_crnti = SRSRAN_INVALID_RNTI;
   reset();
 }
 
 void proc_ra_nr::ra_error()
 {
-  temp_rnti = 0;
+  temp_crnti = SRSRAN_INVALID_RNTI;
   preamble_transmission_counter++;
   contention_resolution_timer.stop();
   uint32_t backoff_wait;
diff --git a/srsue/src/stack/mac_nr/proc_sr_nr.cc b/srsue/src/stack/mac_nr/proc_sr_nr.cc
index 6cfde69a0..f6fb1ec84 100644
--- a/srsue/src/stack/mac_nr/proc_sr_nr.cc
+++ b/srsue/src/stack/mac_nr/proc_sr_nr.cc
@@ -23,19 +23,20 @@
 #include "srsran/common/standard_streams.h"
 #include "srsran/interfaces/ue_phy_interfaces.h"
 #include "srsran/interfaces/ue_rrc_interfaces.h"
-#include "srsue/hdr/stack/mac_nr/proc_ra_nr.h"
 
 namespace srsue {
 
 proc_sr_nr::proc_sr_nr(srslog::basic_logger& logger) : logger(logger) {}
 
-int32_t proc_sr_nr::init(proc_ra_nr* ra_, phy_interface_mac_nr* phy_, rrc_interface_mac* rrc_)
+int32_t proc_sr_nr::init(mac_interface_sr_nr* mac_, phy_interface_mac_nr* phy_, rrc_interface_mac* rrc_)
 {
   rrc        = rrc_;
-  ra         = ra_;
+  mac        = mac_;
   phy        = phy_;
-  initiated  = true;
   sr_counter = 0;
+
+  initiated = true;
+
   return SRSRAN_SUCCESS;
 }
 
@@ -66,6 +67,8 @@ int32_t proc_sr_nr::set_config(const srsran::sr_cfg_nr_t& cfg_)
 
   if (cfg_.enabled) {
     logger.info("SR:    Set sr-TransMax=%d", cfg_.item[0].trans_max);
+  } else {
+    logger.info("SR:    Disabling procedure");
   }
 
   // store config
@@ -86,10 +89,10 @@ void proc_sr_nr::step(uint32_t tti)
   }
 
   // 1> if the MAC entity has no valid PUCCH resource configured for the pending SR:
-  if (!cfg.enabled || not phy->has_valid_sr_resource(0)) {
+  if (not phy->has_valid_sr_resource(cfg.item[0].sched_request_id)) {
     // 2> initiate a Random Access procedure (see clause 5.1) on the SpCell and cancel the pending SR.
     logger.info("SR:    PUCCH not configured. Starting RA procedure");
-    ra->start_by_mac();
+    mac->start_ra();
     reset();
     return;
   }
@@ -107,7 +110,7 @@ void proc_sr_nr::step(uint32_t tti)
     // 4> clear any PUSCH resources for semi-persistent CSI reporting;
     // ... TODO
 
-    ra->start_by_mac();
+    mac->start_ra();
     reset();
   }
 }
@@ -154,10 +157,15 @@ bool proc_sr_nr::sr_opportunity(uint32_t tti, uint32_t sr_id, bool meas_gap, boo
 void proc_sr_nr::start()
 {
   if (initiated) {
-    if (!is_pending_sr) {
+    if (not is_pending_sr) {
+      logger.info("SR:    Starting procedure");
       sr_counter    = 0;
       is_pending_sr = true;
+    } else {
+      logger.debug("SR:    Already pending for Tx");
     }
+  } else {
+    logger.warning("SR:    Procedure not initiated");
   }
 }
 
diff --git a/srsue/src/stack/mac_nr/test/CMakeLists.txt b/srsue/src/stack/mac_nr/test/CMakeLists.txt
index 380f8af1e..32c054aee 100644
--- a/srsue/src/stack/mac_nr/test/CMakeLists.txt
+++ b/srsue/src/stack/mac_nr/test/CMakeLists.txt
@@ -26,6 +26,10 @@ add_executable(proc_bsr_nr_test proc_bsr_nr_test.cc)
 target_link_libraries(proc_bsr_nr_test srsue_mac_nr srsran_common)
 add_test(proc_bsr_nr_test proc_bsr_nr_test)
 
+add_executable(proc_sr_nr_test proc_sr_nr_test.cc)
+target_link_libraries(proc_sr_nr_test srsue_mac_nr srsran_common)
+add_test(proc_sr_nr_test proc_sr_nr_test)
+
 add_executable(mac_nr_test mac_nr_test.cc)
 target_link_libraries(mac_nr_test srsue_mac_nr srsran_common)
 add_test(mac_nr_test mac_nr_test)
\ No newline at end of file
diff --git a/srsue/src/stack/mac_nr/test/mac_nr_test.cc b/srsue/src/stack/mac_nr/test/mac_nr_test.cc
index f382c6d2d..3f7d40954 100644
--- a/srsue/src/stack/mac_nr/test/mac_nr_test.cc
+++ b/srsue/src/stack/mac_nr/test/mac_nr_test.cc
@@ -150,9 +150,61 @@ private:
   std::map<uint32_t, uint32_t> ul_queues;
 };
 
-// TODO: Add test
 int msg3_test()
 {
+  // UL-SCH PDU for Msg3 with C-RNTI CE and SBSR
+  const uint8_t tv[] = {0x3a, 0x10, 0x01, 0x3d, 0x00, 0x3f, 0x00, 0x00, 0x00};
+
+  // dummy layers
+  dummy_phy   phy;
+  rlc_dummy   rlc;
+  rrc_dummy   rrc;
+  stack_dummy stack;
+
+  // the actual MAC
+  mac_nr mac(&stack.task_sched);
+
+  mac_nr_args_t args = {};
+  mac.init(args, &phy, &rlc, &rrc);
+  const uint16_t crnti = 0x1001;
+
+  // set C-RNTI and tell MAC to prepare Msg3 transmission
+  mac.set_crnti(crnti);
+  mac.msg3_prepare();
+
+  stack.init(&mac, &phy);
+
+  // create UL action and grant and read MAC PDU
+  {
+    mac_interface_phy_nr::tb_action_ul_t    ul_action = {};
+    mac_interface_phy_nr::mac_nr_grant_ul_t mac_grant = {};
+
+    mac_grant.rnti = crnti; // make sure MAC picks it up as valid UL grant
+    mac_grant.pid  = 0;
+    mac_grant.tti  = 0;
+    mac_grant.tbs  = 9;
+    int cc_idx     = 0;
+
+    // Send grant to MAC and get action for this TB, 0x
+    mac.new_grant_ul(cc_idx, mac_grant, &ul_action);
+
+    TESTASSERT(ul_action.tb.enabled == true);
+
+    // print generated PDU
+    srslog::fetch_basic_logger("MAC").info(
+        ul_action.tb.payload->msg, mac_grant.tbs, "Generated PDU (%d B)", mac_grant.tbs);
+#if HAVE_PCAP
+    pcap_handle->write_ul_crnti_nr(
+        ul_action.tb.payload->msg, mac_grant.tbs, mac_grant.rnti, UE_ID, mac_grant.pid, mac_grant.tti);
+#endif
+
+    TESTASSERT(memcmp(ul_action.tb.payload->msg, tv, sizeof(tv)) == 0);
+  }
+
+  // make sure MAC PDU thread picks up before stopping
+  stack.run_tti(0);
+  mac.stop();
+
   return SRSRAN_SUCCESS;
 }
 
@@ -181,6 +233,7 @@ int mac_nr_ul_logical_channel_prioritization_test1()
 
   stack.init(&mac, &phy);
   const uint16_t crnti = 0x1001;
+  mac.set_crnti(crnti);
 
   // generate config (default DRB2 config for EN-DC)
   std::vector<srsran::logical_channel_config_t> lcids;
@@ -211,14 +264,46 @@ int mac_nr_ul_logical_channel_prioritization_test1()
 
     mac_grant.rnti = crnti; // make sure MAC picks it up as valid UL grant
     mac_grant.pid  = 0;
-    mac_grant.rnti = 0x1001;
     mac_grant.tti  = 0;
     mac_grant.tbs  = 20;
+    mac_grant.ndi  = 0;
+    int cc_idx     = 0;
+
+    // Send grant to MAC and get action for this TB, 0x
+    mac.new_grant_ul(cc_idx, mac_grant, &ul_action);
+
+    TESTASSERT(ul_action.tb.enabled == true);
+
+    // print generated PDU
+    srslog::fetch_basic_logger("MAC").info(
+        ul_action.tb.payload->msg, mac_grant.tbs, "Generated PDU (%d B)", mac_grant.tbs);
+#if HAVE_PCAP
+    pcap_handle->write_ul_crnti_nr(
+        ul_action.tb.payload->msg, mac_grant.tbs, mac_grant.rnti, UE_ID, mac_grant.pid, mac_grant.tti);
+#endif
+
+    TESTASSERT(memcmp(ul_action.tb.payload->msg, tv, sizeof(tv)) == 0);
+  }
+
+  stack.run_tti(0);
+
+  // create new grant that indicates/requests a retx of the previous PDU
+  {
+    mac_interface_phy_nr::tb_action_ul_t    ul_action = {};
+    mac_interface_phy_nr::mac_nr_grant_ul_t mac_grant = {};
+
+    mac_grant.rnti = crnti; // make sure MAC picks it up as valid UL grant
+    mac_grant.pid  = 0;     // same PID as above
+    mac_grant.tbs  = 20;
+    mac_grant.rv   = 1; // this is RV=1
+    mac_grant.ndi  = 0; // NDI keeps zero to request retx
     int cc_idx     = 0;
 
     // Send grant to MAC and get action for this TB, 0x
     mac.new_grant_ul(cc_idx, mac_grant, &ul_action);
 
+    TESTASSERT(ul_action.tb.enabled == true);
+
     // print generated PDU
     srslog::fetch_basic_logger("MAC").info(
         ul_action.tb.payload->msg, mac_grant.tbs, "Generated PDU (%d B)", mac_grant.tbs);
@@ -269,11 +354,12 @@ int mac_nr_ul_logical_channel_prioritization_test2()
   // the actual MAC
   mac_nr mac(&stack.task_sched);
 
+  const uint16_t crnti = 0x1001;
   mac_nr_args_t args = {};
   mac.init(args, &phy, &rlc, &rrc);
+  mac.set_crnti(crnti);
 
   stack.init(&mac, &phy);
-  const uint16_t crnti = 0x1001;
 
   // generate config (default DRB2 config for EN-DC)
   std::vector<srsran::logical_channel_config_t> lcids;
@@ -304,7 +390,6 @@ int mac_nr_ul_logical_channel_prioritization_test2()
 
     mac_grant.rnti = crnti; // make sure MAC picks it up as valid UL grant
     mac_grant.pid  = 0;
-    mac_grant.rnti = 0x1001;
     mac_grant.tti  = 0;
     mac_grant.tbs  = 260;
     int cc_idx     = 0;
@@ -352,10 +437,11 @@ int mac_nr_ul_periodic_bsr_test()
   mac_nr mac(&stack.task_sched);
 
   mac_nr_args_t args = {};
+  const uint16_t crnti = 0x1001;
   mac.init(args, &phy, &rlc, &rrc);
+  mac.set_crnti(crnti);
 
   stack.init(&mac, &phy);
-  const uint16_t crnti = 0x1001;
 
   // generate config (default DRB2 config for EN-DC)
   std::vector<srsran::logical_channel_config_t> lcids;
@@ -388,6 +474,8 @@ int mac_nr_ul_periodic_bsr_test()
   stack.run_tti(tti++);
   usleep(100);
 
+  int ul_ndi = 0;
+
   // create UL action and grant and read MAC PDU
   {
     mac_interface_phy_nr::tb_action_ul_t    ul_action = {};
@@ -395,14 +483,16 @@ int mac_nr_ul_periodic_bsr_test()
 
     mac_grant.rnti = crnti; // make sure MAC picks it up as valid UL grant
     mac_grant.pid  = 0;
-    mac_grant.rnti = 0x1001;
     mac_grant.tti  = 0;
     mac_grant.tbs  = 10;
+    mac_grant.ndi  = (ul_ndi++) % 2;
     int cc_idx     = 0;
 
     // Send grant to MAC and get action for this TB
     mac.new_grant_ul(cc_idx, mac_grant, &ul_action);
 
+    TESTASSERT(ul_action.tb.enabled == true);
+
     // print generated PDU
     srslog::fetch_basic_logger("MAC").info(
         ul_action.tb.payload->msg, mac_grant.tbs, "Generated PDU (%d B)", mac_grant.tbs);
@@ -426,14 +516,16 @@ int mac_nr_ul_periodic_bsr_test()
 
       mac_grant.rnti = crnti; // make sure MAC picks it up as valid UL grant
       mac_grant.pid  = 0;
-      mac_grant.rnti = 0x1001;
       mac_grant.tti  = 0;
       mac_grant.tbs  = 10;
+      mac_grant.ndi  = (ul_ndi++) % 2;
       int cc_idx     = 0;
 
       // Send grant to MAC and get action for this TB
       mac.new_grant_ul(cc_idx, mac_grant, &ul_action);
 
+      TESTASSERT(ul_action.tb.enabled == true);
+
       // print generated PDU
       srslog::fetch_basic_logger("MAC").info(
           ul_action.tb.payload->msg, mac_grant.tbs, "Generated PDU (%d B)", mac_grant.tbs);
@@ -457,7 +549,6 @@ int mac_nr_ul_periodic_bsr_test()
 
     mac_grant.rnti = crnti; // make sure MAC picks it up as valid UL grant
     mac_grant.pid  = 0;
-    mac_grant.rnti = 0x1001;
     mac_grant.tti  = 0;
     mac_grant.tbs  = 10;
     int cc_idx     = 0;
@@ -465,6 +556,8 @@ int mac_nr_ul_periodic_bsr_test()
     // Send grant to MAC and get action for this TB
     mac.new_grant_ul(cc_idx, mac_grant, &ul_action);
 
+    TESTASSERT(ul_action.tb.enabled == true);
+
     // print generated PDU
     srslog::fetch_basic_logger("MAC").info(
         ul_action.tb.payload->msg, mac_grant.tbs, "Generated PDU (%d B)", mac_grant.tbs);
diff --git a/srsue/src/stack/mac_nr/test/proc_sr_nr_test.cc b/srsue/src/stack/mac_nr/test/proc_sr_nr_test.cc
new file mode 100644
index 000000000..666bce515
--- /dev/null
+++ b/srsue/src/stack/mac_nr/test/proc_sr_nr_test.cc
@@ -0,0 +1,104 @@
+/**
+ *
+ * \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/buffer_pool.h"
+#include "srsran/common/common.h"
+#include "srsran/common/test_common.h"
+#include "srsue/hdr/stack/mac_nr/proc_sr_nr.h"
+#include "srsran/interfaces/ue_rrc_interfaces.h"
+
+using namespace srsue;
+
+class dummy_phy : public phy_interface_mac_nr
+{
+public:
+  dummy_phy() {}
+  void send_prach(const uint32_t prach_occasion_,
+                  const int      preamble_index_,
+                  const float    preamble_received_target_power_,
+                  const float    ta_base_sec_ = 0.0f) override
+  {
+    prach_occasion                 = prach_occasion_;
+    preamble_index                 = preamble_index_;
+    preamble_received_target_power = preamble_received_target_power_;
+  }
+  int tx_request(const tx_request_t& request) override { return 0; }
+  int set_ul_grant(std::array<uint8_t, SRSRAN_RAR_UL_GRANT_NBITS>, uint16_t rnti, srsran_rnti_type_t rnti_type) override
+  {
+    return 0;
+  }
+
+  void get_last_send_prach(uint32_t* prach_occasion_, uint32_t* preamble_index_, int* preamble_received_target_power_)
+  {
+    *prach_occasion_                 = prach_occasion;
+    *preamble_index_                 = preamble_index;
+    *preamble_received_target_power_ = preamble_received_target_power;
+  }
+  bool has_valid_sr_resource(uint32_t sr_id) override { return false; }
+  void clear_pending_grants() override {}
+
+private:
+  uint32_t prach_occasion                 = 0;
+  uint32_t preamble_index                 = 0;
+  int      preamble_received_target_power = 0;
+};
+
+class dummy_rrc : public rrc_interface_mac
+{
+  void ra_completed() {}
+  void ra_problem() {}
+  void release_pucch_srs() {}
+};
+
+class dummy_mac : public mac_interface_sr_nr
+{
+public:
+  void start_ra() { ra_started = true; }
+  bool check_ra_started() { return ra_started; }
+  void reset_ra_started() { ra_started = false; }
+
+private:
+  bool ra_started = false;
+};
+
+int proc_sr_basic_test()
+{
+  proc_sr_nr proc_sr_nr(srslog::fetch_basic_logger("MAC"));
+  dummy_rrc dummy_rrc; 
+  dummy_phy dummy_phy;
+  dummy_mac dummy_mac; 
+  
+  srsran::sr_cfg_nr_t cfg; 
+  cfg.enabled = true;  
+  cfg.num_items = 1; 
+  cfg.item[0].prohibit_timer = 0; 
+  cfg.item[0].sched_request_id = 0; 
+  cfg.item[0].trans_max = 64; 
+  proc_sr_nr.init(&dummy_mac, &dummy_phy, &dummy_rrc);
+  proc_sr_nr.set_config(cfg);
+  proc_sr_nr.start();
+
+  proc_sr_nr.step(0);
+  TESTASSERT(dummy_mac.check_ra_started() == true);
+
+  return SRSRAN_SUCCESS;
+}
+
+int main()
+{
+  srslog::init();
+
+  auto& mac_logger = srslog::fetch_basic_logger("MAC");
+  mac_logger.set_level(srslog::basic_levels::debug);
+  mac_logger.set_hex_dump_max_size(-1);
+  TESTASSERT(proc_sr_basic_test() == SRSRAN_SUCCESS);
+  return SRSRAN_SUCCESS;
+}
\ No newline at end of file
diff --git a/srsue/src/stack/mac_nr/ul_harq_nr.cc b/srsue/src/stack/mac_nr/ul_harq_nr.cc
new file mode 100644
index 000000000..8a20da657
--- /dev/null
+++ b/srsue/src/stack/mac_nr/ul_harq_nr.cc
@@ -0,0 +1,268 @@
+/**
+ *
+ * \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 "srsue/hdr/stack/mac_nr/ul_harq_nr.h"
+#include "srsran/common/buffer_pool.h"
+#include "srsran/common/interfaces_common.h"
+#include "srsran/common/timers.h"
+
+namespace srsue {
+
+ul_harq_entity_nr::ul_harq_entity_nr(const uint8_t          cc_idx_,
+                                     mac_interface_harq_nr* mac_,
+                                     proc_ra_nr*            ra_proc_,
+                                     mux_nr*                mux_) :
+  mac(mac_), mux(mux_), logger(srslog::fetch_basic_logger("MAC"))
+{}
+
+int ul_harq_entity_nr::init()
+{
+  uint32_t proc_count = 0;
+  for (auto& proc : harq_procs) {
+    if (not proc.init(proc_count++, this)) {
+      logger.error("Couldn't initialize HARQ procedure");
+      return SRSRAN_ERROR;
+    }
+  }
+  return SRSRAN_SUCCESS;
+}
+
+void ul_harq_entity_nr::reset()
+{
+  for (auto& proc : harq_procs) {
+    proc.reset();
+  }
+}
+
+void ul_harq_entity_nr::reset_ndi()
+{
+  for (auto& proc : harq_procs) {
+    proc.reset_ndi();
+  }
+}
+
+void ul_harq_entity_nr::set_config(srsran::ul_harq_cfg_t& harq_cfg_)
+{
+  harq_cfg = harq_cfg_;
+}
+
+/***************** PHY->MAC interface for UL processes **************************/
+void ul_harq_entity_nr::new_grant_ul(const mac_interface_phy_nr::mac_nr_grant_ul_t& grant,
+                                     mac_interface_phy_nr::tb_action_ul_t*          action)
+{
+  if (grant.pid >= harq_procs.size()) {
+    logger.error("Invalid PID: %d", grant.pid);
+    return;
+  }
+
+  // Determine whether the NDI has been toggled for this process
+  bool ndi_toggled = (grant.ndi != harq_procs.at(grant.pid).get_ndi());
+
+  // 1> if UL MAC entity's C-RNTI or Temporary C-RNTI; or Received in RAR;
+  if (mac->get_crnti() == grant.rnti || mac->get_temp_crnti() == grant.rnti || grant.is_rar_grant) {
+    // 2> if UL grant for C-RNTI and if the previous grant delivered to the HARQ entity
+    // for the same HARQ process was either an uplink grant received for the MAC entity's CS-RNTI or a
+    // configured uplink grant:
+    if (mac->get_crnti() == grant.rnti && harq_procs.at(grant.pid).has_grant()) {
+      // 3> consider the NDI to have been toggled regardless of the value of the NDI.
+      ndi_toggled = true;
+    }
+
+    // 2> if the UL grant is for MAC entity's C-RNTI, and the HARQ process is configured for a configured uplink grant:
+    if (mac->get_crnti() == grant.rnti && harq_procs.at(grant.pid).has_grant()) {
+      // TODO: add handling for configuredGrantTimer
+    }
+
+    // 2> deliver the uplink grant and the associated HARQ information to the HARQ entity
+    harq_procs.at(grant.pid).new_grant_ul(grant, ndi_toggled, action);
+  } else if (mac->get_csrnti() == grant.rnti) {
+    // SPS not supported
+    logger.warning("Ignoring grant for CS-RNTI=0x%x", grant.rnti);
+  } else {
+    logger.warning("Received grant for unknown rnti=0x%x", grant.rnti);
+    printf("Received grant for unknown rnti=0x%x\n", grant.rnti);
+  }
+
+  srsran_expect(action->tb.enabled ? action->tb.payload != nullptr : true,
+                "UL action enabled but no HARQ buffer provided");
+}
+
+int ul_harq_entity_nr::get_current_tbs(uint32_t pid)
+{
+  if (pid >= harq_procs.size()) {
+    logger.error("Invalid PID: %d", pid);
+    return 0;
+  }
+  return harq_procs.at(pid).get_current_tbs();
+}
+
+float ul_harq_entity_nr::get_average_retx()
+{
+  return average_retx;
+}
+
+ul_harq_entity_nr::ul_harq_process_nr::ul_harq_process_nr() : logger(srslog::fetch_basic_logger("MAC")) {}
+
+ul_harq_entity_nr::ul_harq_process_nr::~ul_harq_process_nr()
+{
+  if (is_initiated) {
+    srsran_softbuffer_tx_free(&softbuffer);
+  }
+}
+
+bool ul_harq_entity_nr::ul_harq_process_nr::init(uint32_t pid_, ul_harq_entity_nr* entity_)
+{
+  if (srsran_softbuffer_tx_init_guru(&softbuffer, SRSRAN_SCH_NR_MAX_NOF_CB_LDPC, SRSRAN_LDPC_MAX_LEN_ENCODED_CB) <
+      SRSRAN_SUCCESS) {
+    logger.error("Couldn't initialize softbuffer");
+    return false;
+  }
+
+  pid         = pid_;
+  harq_entity = entity_;
+
+  is_initiated = true;
+
+  return true;
+}
+
+void ul_harq_entity_nr::ul_harq_process_nr::reset()
+{
+  nof_retx      = 0;
+  harq_buffer   = nullptr;
+  current_grant = {};
+}
+
+void ul_harq_entity_nr::ul_harq_process_nr::reset_ndi()
+{
+  current_grant.ndi = false;
+}
+
+uint8_t ul_harq_entity_nr::ul_harq_process_nr::get_ndi()
+{
+  return current_grant.ndi;
+}
+
+bool ul_harq_entity_nr::ul_harq_process_nr::has_grant()
+{
+  return grant_configured;
+}
+
+// Basic handling of new grant
+void ul_harq_entity_nr::ul_harq_process_nr::new_grant_ul(const mac_interface_phy_nr::mac_nr_grant_ul_t& grant,
+                                                         const bool&                                    ndi_toggled,
+                                                         mac_interface_phy_nr::tb_action_ul_t*          action)
+{
+  // Get maximum retransmissions
+  uint32_t max_retx = harq_entity->harq_cfg.max_harq_tx;
+
+  // Check maximum retransmissions, do not consider last retx ACK
+  if (nof_retx >= max_retx) {
+    logger.info("UL %d:  Maximum number of ReTX reached (%d). Discarding TB.", pid, max_retx);
+    if (grant.rnti == harq_entity->mac->get_temp_crnti()) {
+      // FIXME: signal maxRetx to RA?
+      // harq_entity->ra_proc->harq_max_retx();
+    }
+    reset();
+  }
+
+  // Checks in 5.4.2.1
+  if ((grant.rnti != harq_entity->mac->get_temp_crnti() &&
+       ndi_toggled) || // If not addressed to T-CRNTI and NDI toggled
+      (grant.rnti == harq_entity->mac->get_crnti() &&
+       harq_buffer == nullptr) || // If addressed to C-RNTI and buffer is empty
+      (grant.is_rar_grant)        // Grant received in a RAR
+  ) {
+    // new transmission
+
+    // generate new PDU (Msg3 or normal UL)
+    harq_buffer = harq_entity->mux->get_pdu(grant.tbs);
+
+    // 3> if a MAC PDU to transmit has been obtained
+    if (harq_buffer != nullptr) {
+      // 4> deliver the MAC PDU
+      // 4> instruct the identified HARQ process to trigger a new transmission;
+      generate_new_tx(grant, action);
+
+      // TODO: add handling of configuredGrantTimer
+    } else {
+      // HARQ buffer is automatically flushed
+    }
+  } else {
+    // retransmission
+    if (harq_buffer == nullptr) {
+      // ignore the UL grant
+      logger.info("UL %d:  HARQ buffer empty. Ignoring grant.");
+      return;
+    }
+
+    // 4> instruct the identified HARQ process to trigger a retransmission;
+    generate_retx(grant, action);
+
+    // TODO: add handling of configuredGrantTimer
+  }
+}
+
+uint32_t ul_harq_entity_nr::ul_harq_process_nr::get_nof_retx()
+{
+  return nof_retx;
+}
+
+int ul_harq_entity_nr::ul_harq_process_nr::get_current_tbs()
+{
+  return current_grant.tbs;
+}
+
+// New transmission (Section 5.4.2.2)
+void ul_harq_entity_nr::ul_harq_process_nr::generate_new_tx(const mac_interface_phy_nr::mac_nr_grant_ul_t& grant,
+                                                            mac_interface_phy_nr::tb_action_ul_t*          action)
+{
+  // Compute average number of retransmissions per packet considering previous packet
+  harq_entity->average_retx = SRSRAN_VEC_CMA((float)nof_retx, harq_entity->average_retx, harq_entity->nof_pkts++);
+  current_grant             = grant;
+  nof_retx                  = 0;
+
+  logger.info("UL %d:  New TX%s, rv=%d, tbs=%d",
+              pid,
+              grant.rnti == harq_entity->mac->get_temp_crnti() ? " for Msg3" : "",
+              grant.rv,
+              grant.tbs);
+
+  generate_tx(action);
+}
+
+// Retransmission (Section 5.4.2.2)
+void ul_harq_entity_nr::ul_harq_process_nr::generate_retx(const mac_interface_phy_nr::mac_nr_grant_ul_t& grant,
+                                                          mac_interface_phy_nr::tb_action_ul_t*          action)
+{
+  logger.info("UL %d:  Retx=%d, rv=%d, tbs=%d", pid, nof_retx, grant.rv, grant.tbs);
+
+  // overwrite original grant
+  current_grant = grant;
+
+  generate_tx(action);
+}
+
+// Transmission of pending frame (Section 5.4.2.2)
+void ul_harq_entity_nr::ul_harq_process_nr::generate_tx(mac_interface_phy_nr::tb_action_ul_t* action)
+{
+  nof_retx++;
+
+  action->tb.rv         = current_grant.rv;
+  action->tb.enabled    = true;
+  action->tb.payload    = harq_buffer.get();
+  action->tb.softbuffer = &softbuffer;
+
+  srsran_softbuffer_tx_reset(&softbuffer);
+}
+
+} // namespace srsue
diff --git a/srsue/src/stack/rrc/rrc.cc b/srsue/src/stack/rrc/rrc.cc
index 69594587b..4fb927daf 100644
--- a/srsue/src/stack/rrc/rrc.cc
+++ b/srsue/src/stack/rrc/rrc.cc
@@ -45,6 +45,8 @@ bool simulate_rlf = false;
 
 using namespace srsran;
 using namespace asn1::rrc;
+using srsran::lte_srb;
+using srsran::srb_to_lcid;
 
 namespace srsue {
 
@@ -757,7 +759,7 @@ void rrc::timer_expired(uint32_t timeout_id)
   }
 }
 
-bool rrc::nr_reconfiguration_proc(const rrc_conn_recfg_r8_ies_s& rx_recfg, bool *has_5g_nr_reconfig)
+bool rrc::nr_reconfiguration_proc(const rrc_conn_recfg_r8_ies_s& rx_recfg, bool* has_5g_nr_reconfig)
 {
   if (!(rx_recfg.non_crit_ext_present && rx_recfg.non_crit_ext.non_crit_ext_present &&
         rx_recfg.non_crit_ext.non_crit_ext.non_crit_ext_present &&
@@ -788,7 +790,7 @@ bool rrc::nr_reconfiguration_proc(const rrc_conn_recfg_r8_ies_s& rx_recfg, bool
 
   switch (rrc_conn_recfg_v1510_ies->nr_cfg_r15.type()) {
     case setup_opts::options::release:
-      logger.info("NR config R15 of type release"); 
+      logger.info("NR config R15 of type release");
       break;
     case setup_opts::options::setup:
       endc_release_and_add_r15 = rrc_conn_recfg_v1510_ies->nr_cfg_r15.setup().endc_release_and_add_r15;
@@ -810,14 +812,14 @@ bool rrc::nr_reconfiguration_proc(const rrc_conn_recfg_r8_ies_s& rx_recfg, bool
     nr_radio_bearer_cfg1_r15_present = true;
     nr_radio_bearer_cfg1_r15         = rrc_conn_recfg_v1510_ies->nr_radio_bearer_cfg1_r15;
   }
-    *has_5g_nr_reconfig = true;
-    return rrc_nr->rrc_reconfiguration(endc_release_and_add_r15,
-                                       nr_secondary_cell_group_cfg_r15_present,
-                                       nr_secondary_cell_group_cfg_r15,
-                                       sk_counter_r15_present,
-                                       sk_counter_r15,
-                                       nr_radio_bearer_cfg1_r15_present,
-                                       nr_radio_bearer_cfg1_r15);
+  *has_5g_nr_reconfig = true;
+  return rrc_nr->rrc_reconfiguration(endc_release_and_add_r15,
+                                     nr_secondary_cell_group_cfg_r15_present,
+                                     nr_secondary_cell_group_cfg_r15,
+                                     sk_counter_r15_present,
+                                     sk_counter_r15,
+                                     nr_radio_bearer_cfg1_r15_present,
+                                     nr_radio_bearer_cfg1_r15);
 }
 /*******************************************************************************
  *
@@ -949,7 +951,7 @@ void rrc::send_con_restablish_complete()
   ul_dcch_msg.msg.set_c1().set_rrc_conn_reest_complete().crit_exts.set_rrc_conn_reest_complete_r8();
   ul_dcch_msg.msg.c1().rrc_conn_reest_complete().rrc_transaction_id = transaction_id;
 
-  send_ul_dcch_msg(RB_ID_SRB1, ul_dcch_msg);
+  send_ul_dcch_msg(srb_to_lcid(lte_srb::srb1), ul_dcch_msg);
 
   reestablishment_successful = true;
 }
@@ -969,12 +971,12 @@ void rrc::send_con_setup_complete(srsran::unique_byte_buffer_t nas_msg)
   rrc_conn_setup_complete->ded_info_nas.resize(nas_msg->N_bytes);
   memcpy(rrc_conn_setup_complete->ded_info_nas.data(), nas_msg->msg, nas_msg->N_bytes); // TODO Check!
 
-  send_ul_dcch_msg(RB_ID_SRB1, ul_dcch_msg);
+  send_ul_dcch_msg(srb_to_lcid(lte_srb::srb1), ul_dcch_msg);
 }
 
 void rrc::send_ul_info_transfer(unique_byte_buffer_t nas_msg)
 {
-  uint32_t lcid = rlc->has_bearer(RB_ID_SRB2) ? RB_ID_SRB2 : RB_ID_SRB1;
+  uint32_t lcid = (uint32_t)(rlc->has_bearer(srb_to_lcid(lte_srb::srb2)) ? lte_srb::srb2 : lte_srb::srb1);
 
   // Prepare UL INFO packet
   asn1::rrc::ul_dcch_msg_s   ul_dcch_msg;
@@ -997,7 +999,7 @@ void rrc::send_security_mode_complete()
   ul_dcch_msg.msg.set_c1().set_security_mode_complete().crit_exts.set_security_mode_complete_r8();
   ul_dcch_msg.msg.c1().security_mode_complete().rrc_transaction_id = transaction_id;
 
-  send_ul_dcch_msg(RB_ID_SRB1, ul_dcch_msg);
+  send_ul_dcch_msg(srb_to_lcid(lte_srb::srb1), ul_dcch_msg);
 }
 
 void rrc::send_rrc_con_reconfig_complete(bool contains_nr_complete)
@@ -1025,7 +1027,7 @@ void rrc::send_rrc_con_reconfig_complete(bool contains_nr_complete)
     rrc_conn_recfg_complete_v1430_ies->non_crit_ext.scg_cfg_resp_nr_r15_present = true;
     rrc_conn_recfg_complete_v1430_ies->non_crit_ext.scg_cfg_resp_nr_r15.from_string("00");
   }
-  send_ul_dcch_msg(RB_ID_SRB1, ul_dcch_msg);
+  send_ul_dcch_msg(srb_to_lcid(lte_srb::srb1), ul_dcch_msg);
 }
 
 void rrc::ra_completed()
@@ -1177,12 +1179,12 @@ void rrc::start_con_restablishment(reest_cause_e cause)
 bool rrc::srbs_flushed()
 {
   // Check SRB1
-  if (rlc->has_data(RB_ID_SRB1) && not rlc->is_suspended(RB_ID_SRB1)) {
+  if (rlc->has_data(srb_to_lcid(lte_srb::srb1)) && not rlc->is_suspended(srb_to_lcid(lte_srb::srb1))) {
     return false;
   }
 
   // Check SRB2
-  if (rlc->has_data(RB_ID_SRB2) && not rlc->is_suspended(RB_ID_SRB2)) {
+  if (rlc->has_data(srb_to_lcid(lte_srb::srb2)) && not rlc->is_suspended(srb_to_lcid(lte_srb::srb2))) {
     return false;
   }
 
@@ -1196,7 +1198,7 @@ bool rrc::srbs_flushed()
  *******************************************************************************/
 void rrc::send_srb1_msg(const ul_dcch_msg_s& msg)
 {
-  send_ul_dcch_msg(RB_ID_SRB1, msg);
+  send_ul_dcch_msg(srb_to_lcid(lte_srb::srb1), msg);
 }
 
 std::set<uint32_t> rrc::get_cells(const uint32_t earfcn)
@@ -1570,8 +1572,8 @@ void rrc::send_ul_ccch_msg(const ul_ccch_msg_s& msg)
   logger.debug("Setting UE contention resolution ID: %" PRIu64 "", uecri);
   mac->set_contention_id(uecri);
 
-  uint32_t lcid = RB_ID_SRB0;
-  log_rrc_message(get_rb_name(lcid).c_str(), Tx, pdcp_buf.get(), msg, msg.msg.c1().type().to_string());
+  uint32_t lcid = srb_to_lcid(lte_srb::srb0);
+  log_rrc_message(get_rb_name(lcid), Tx, pdcp_buf.get(), msg, msg.msg.c1().type().to_string());
 
   rlc->write_sdu(lcid, std::move(pdcp_buf));
 }
@@ -1592,13 +1594,12 @@ void rrc::send_ul_dcch_msg(uint32_t lcid, const ul_dcch_msg_s& msg)
   pdcp_buf->set_timestamp();
 
   if (msg.msg.type() == ul_dcch_msg_type_c::types_opts::options::c1) {
-    log_rrc_message(get_rb_name(lcid).c_str(), Tx, pdcp_buf.get(), msg, msg.msg.c1().type().to_string());
+    log_rrc_message(get_rb_name(lcid), Tx, pdcp_buf.get(), msg, msg.msg.c1().type().to_string());
   } else if (msg.msg.type() == ul_dcch_msg_type_c::types_opts::options::msg_class_ext) {
     if (msg.msg.msg_class_ext().type() == ul_dcch_msg_type_c::msg_class_ext_c_::types_opts::options::c2) {
-      log_rrc_message(
-          get_rb_name(lcid).c_str(), Tx, pdcp_buf.get(), msg, msg.msg.msg_class_ext().c2().type().to_string());
+      log_rrc_message(get_rb_name(lcid), Tx, pdcp_buf.get(), msg, msg.msg.msg_class_ext().c2().type().to_string());
     } else {
-      log_rrc_message(get_rb_name(lcid).c_str(), Tx, pdcp_buf.get(), msg, msg.msg.msg_class_ext().type().to_string());
+      log_rrc_message(get_rb_name(lcid), Tx, pdcp_buf.get(), msg, msg.msg.msg_class_ext().type().to_string());
     }
   }
 
@@ -1622,12 +1623,12 @@ void rrc::write_pdu(uint32_t lcid, unique_byte_buffer_t pdu)
 void rrc::process_pdu(uint32_t lcid, srsran::unique_byte_buffer_t pdu)
 {
   logger.debug("RX PDU, LCID: %d", lcid);
-  switch (lcid) {
-    case RB_ID_SRB0:
+  switch (static_cast<lte_srb>(lcid)) {
+    case lte_srb::srb0:
       parse_dl_ccch(std::move(pdu));
       break;
-    case RB_ID_SRB1:
-    case RB_ID_SRB2:
+    case lte_srb::srb1:
+    case lte_srb::srb2:
       parse_dl_dcch(lcid, std::move(pdu));
       break;
     default:
@@ -1645,7 +1646,8 @@ void rrc::parse_dl_ccch(unique_byte_buffer_t pdu)
     logger.error(pdu->msg, pdu->N_bytes, "Failed to unpack DL-CCCH message (%d B)", pdu->N_bytes);
     return;
   }
-  log_rrc_message(get_rb_name(RB_ID_SRB0).c_str(), Rx, pdu.get(), dl_ccch_msg, dl_ccch_msg.msg.c1().type().to_string());
+  log_rrc_message(
+      get_rb_name(srb_to_lcid(lte_srb::srb0)), Rx, pdu.get(), dl_ccch_msg, dl_ccch_msg.msg.c1().type().to_string());
 
   dl_ccch_msg_type_c::c1_c_* c1 = &dl_ccch_msg.msg.c1();
   switch (dl_ccch_msg.msg.c1().type().value) {
@@ -1699,7 +1701,7 @@ void rrc::parse_dl_dcch(uint32_t lcid, unique_byte_buffer_t pdu)
     logger.error(pdu->msg, pdu->N_bytes, "Failed to unpack DL-DCCH message (%d B)", pdu->N_bytes);
     return;
   }
-  log_rrc_message(get_rb_name(lcid).c_str(), Rx, pdu.get(), dl_dcch_msg, dl_dcch_msg.msg.c1().type().to_string());
+  log_rrc_message(get_rb_name(lcid), Rx, pdu.get(), dl_dcch_msg, dl_dcch_msg.msg.c1().type().to_string());
 
   dl_dcch_msg_type_c::c1_c_* c1 = &dl_dcch_msg.msg.c1();
   switch (dl_dcch_msg.msg.c1().type().value) {
@@ -2122,7 +2124,7 @@ void rrc::handle_ue_capability_enquiry(const ue_cap_enquiry_s& enquiry)
     }
   }
 
-  send_ul_dcch_msg(RB_ID_SRB1, ul_dcch_msg);
+  send_ul_dcch_msg(srb_to_lcid(lte_srb::srb1), ul_dcch_msg);
 }
 
 /*******************************************************************************
@@ -2559,7 +2561,7 @@ void rrc::add_srb(const srb_to_add_mod_s& srb_cnfg)
 {
   // Setup PDCP
   pdcp->add_bearer(srb_cnfg.srb_id, make_srb_pdcp_config_t(srb_cnfg.srb_id, true));
-  if (RB_ID_SRB2 == srb_cnfg.srb_id) {
+  if (lte_srb::srb2 == static_cast<lte_srb>(srb_cnfg.srb_id)) {
     pdcp->config_security(srb_cnfg.srb_id, sec_cfg);
     pdcp->enable_integrity(srb_cnfg.srb_id, DIRECTION_TXRX);
     pdcp->enable_encryption(srb_cnfg.srb_id, DIRECTION_TXRX);
@@ -2580,20 +2582,23 @@ void rrc::add_srb(const srb_to_add_mod_s& srb_cnfg)
   if (srb_cnfg.lc_ch_cfg_present) {
     if (srb_cnfg.lc_ch_cfg.type() == srb_to_add_mod_s::lc_ch_cfg_c_::types::default_value) {
       // Set default SRB values as defined in Table 9.2.1
-      switch (srb_cnfg.srb_id) {
-        case RB_ID_SRB0:
+      switch (static_cast<lte_srb>(srb_cnfg.srb_id)) {
+        case lte_srb::srb0:
           logger.error("Setting SRB0: Should not be set by RRC");
           break;
-        case RB_ID_SRB1:
+        case lte_srb::srb1:
           priority             = 1;
           prioritized_bit_rate = -1;
           bucket_size_duration = 0;
           break;
-        case RB_ID_SRB2:
+        case lte_srb::srb2:
           priority             = 3;
           prioritized_bit_rate = -1;
           bucket_size_duration = 0;
           break;
+        default:
+          logger.error("Invalid SRB configuration");
+          return;
       }
     } else {
       if (srb_cnfg.lc_ch_cfg.explicit_value().lc_ch_sr_mask_r9_present) {
@@ -2612,7 +2617,7 @@ void rrc::add_srb(const srb_to_add_mod_s& srb_cnfg)
   }
 
   srbs[srb_cnfg.srb_id] = srb_cnfg;
-  logger.info("Added radio bearer %s", get_rb_name(srb_cnfg.srb_id).c_str());
+  logger.info("Added radio bearer %s", get_rb_name(srb_cnfg.srb_id));
 }
 
 void rrc::add_drb(const drb_to_add_mod_s& drb_cnfg)
@@ -2625,7 +2630,7 @@ void rrc::add_drb(const drb_to_add_mod_s& drb_cnfg)
   if (drb_cnfg.lc_ch_id_present) {
     lcid = drb_cnfg.lc_ch_id;
   } else {
-    lcid = RB_ID_SRB2 + drb_cnfg.drb_id;
+    lcid = srsran::MAX_LTE_SRB_ID + drb_cnfg.drb_id;
     logger.warning("LCID not present, using %d", lcid);
   }
 
@@ -2659,7 +2664,7 @@ void rrc::add_drb(const drb_to_add_mod_s& drb_cnfg)
   drb_up                = true;
   logger.info("Added DRB Id %d (LCID=%d)", drb_cnfg.drb_id, lcid);
   // Update LCID if gw is running
-  if(gw->is_running()){
+  if (gw->is_running()) {
     gw->update_lcid(drb_cnfg.eps_bearer_id, lcid);
   }
 }
@@ -2684,7 +2689,7 @@ uint32_t rrc::get_lcid_for_eps_bearer(const uint32_t& eps_bearer_id)
   if (drb_cnfg.lc_ch_id_present) {
     lcid = drb_cnfg.lc_ch_id;
   } else {
-    lcid = RB_ID_SRB2 + drb_cnfg.drb_id;
+    lcid = srsran::MAX_LTE_SRB_ID + drb_cnfg.drb_id;
     logger.warning("LCID not present, using %d", lcid);
   }
   return lcid;
@@ -2789,7 +2794,7 @@ void rrc::nr_scg_failure_information(const scg_failure_cause_t cause)
   scg_fail_info_nr.crit_exts.c1().scg_fail_info_nr_r15().fail_report_scg_nr_r15_present = true;
   scg_fail_info_nr.crit_exts.c1().scg_fail_info_nr_r15().fail_report_scg_nr_r15.fail_type_r15 =
       (fail_report_scg_nr_r15_s::fail_type_r15_opts::options)cause;
-  send_ul_dcch_msg(RB_ID_SRB1, ul_dcch_msg);
+  send_ul_dcch_msg(srb_to_lcid(lte_srb::srb1), ul_dcch_msg);
 }
 
 } // namespace srsue
diff --git a/srsue/src/stack/rrc/rrc_meas.cc b/srsue/src/stack/rrc/rrc_meas.cc
index 830b63b5e..b513f322b 100644
--- a/srsue/src/stack/rrc/rrc_meas.cc
+++ b/srsue/src/stack/rrc/rrc_meas.cc
@@ -1070,6 +1070,7 @@ void rrc::rrc_meas::var_meas_cfg::remove_varmeas_report(const uint32_t meas_id)
 {
   meas_report->remove_varmeas_report(meas_id);
   trigger_state.erase(meas_id);
+  trigger_state_nr.erase(meas_id);
 }
 
 std::list<meas_obj_to_add_mod_s> rrc::rrc_meas::var_meas_cfg::get_active_objects()
diff --git a/srsue/src/stack/rrc/rrc_nr.cc b/srsue/src/stack/rrc/rrc_nr.cc
index a688aa222..8f693aee4 100644
--- a/srsue/src/stack/rrc/rrc_nr.cc
+++ b/srsue/src/stack/rrc/rrc_nr.cc
@@ -39,7 +39,7 @@ namespace srsue {
 const char* rrc_nr::rrc_nr_state_text[] = {"IDLE", "CONNECTED", "CONNECTED-INACTIVE"};
 
 rrc_nr::rrc_nr(srsran::task_sched_handle task_sched_) :
-  logger(srslog::fetch_basic_logger("RRC")), task_sched(task_sched_), conn_recfg_proc(this)
+  logger(srslog::fetch_basic_logger("RRC-NR")), task_sched(task_sched_), conn_recfg_proc(this)
 {}
 
 rrc_nr::~rrc_nr() = default;
@@ -493,6 +493,7 @@ bool rrc_nr::apply_mac_cell_group(const mac_cell_group_cfg_s& mac_cell_group_cfg
         const sched_request_to_add_mod_s& asn1_cfg =
             mac_cell_group_cfg.sched_request_cfg.sched_request_to_add_mod_list[0];
         sr_cfg_nr_t sr_cfg              = {};
+        sr_cfg.enabled                  = true;
         sr_cfg.num_items                = 1;
         sr_cfg.item[0].sched_request_id = asn1_cfg.sched_request_id;
         sr_cfg.item[0].trans_max        = asn1_cfg.sr_trans_max.to_number();
@@ -527,11 +528,40 @@ bool rrc_nr::apply_mac_cell_group(const mac_cell_group_cfg_s& mac_cell_group_cfg
     }
 
   if (mac_cell_group_cfg.tag_cfg_present) {
-    logger.warning("Not handling tag cfg in MAC cell group config");
+    if (mac_cell_group_cfg.tag_cfg.tag_to_add_mod_list_present) {
+      for (uint32_t i = 0; i < mac_cell_group_cfg.tag_cfg.tag_to_add_mod_list.size(); i++) {
+        tag_cfg_nr_t tag_cfg_nr     = {};
+        tag_cfg_nr.tag_id           = mac_cell_group_cfg.tag_cfg.tag_to_add_mod_list[i].tag_id;
+        tag_cfg_nr.time_align_timer = mac_cell_group_cfg.tag_cfg.tag_to_add_mod_list[i].time_align_timer.to_number();
+        if (mac->add_tag_config(tag_cfg_nr) != SRSRAN_SUCCESS) {
+          logger.warning("Unable to add TAG config with tag_id %d", tag_cfg_nr.tag_id);
+          return false;
+        }
+      }
+    }
+    if (mac_cell_group_cfg.tag_cfg.tag_to_release_list_present) {
+      for (uint32_t i = 0; i < mac_cell_group_cfg.tag_cfg.tag_to_release_list.size(); i++) {
+        uint32_t tag_id = mac_cell_group_cfg.tag_cfg.tag_to_release_list[i];
+        if (mac->remove_tag_config(tag_id) != SRSRAN_SUCCESS) {
+          logger.warning("Unable to release TAG config with tag_id %d", tag_id);
+          return false;
+        }
+      }
+    }
   }
 
   if (mac_cell_group_cfg.phr_cfg_present) {
-    logger.warning("Not handling phr cfg in MAC cell group config");
+    if (mac_cell_group_cfg.phr_cfg.type() == setup_release_c<asn1::rrc_nr::phr_cfg_s>::types_opts::setup) {
+      phr_cfg_nr_t phr_cfg_nr;
+      if (make_mac_phr_cfg_t(mac_cell_group_cfg.phr_cfg.setup(), &phr_cfg_nr) != true) {
+        logger.warning("Unable to build PHR config");
+        return false;
+      }
+      if (mac->set_config(phr_cfg_nr) != SRSRAN_SUCCESS) {
+        logger.warning("Unable to set PHR config");
+        return false;
+      }
+    }
   }
 
   if (mac_cell_group_cfg.skip_ul_tx_dynamic) {
@@ -674,7 +704,7 @@ bool rrc_nr::apply_dl_common_cfg(const asn1::rrc_nr::dl_cfg_common_s& dl_cfg_com
             srsran_sch_time_ra_t common_time_ra;
             if (make_phy_common_time_ra(pdsch_cfg_common.pdsch_time_domain_alloc_list[i], &common_time_ra) == true) {
               phy_cfg.pdsch.common_time_ra[i]  = common_time_ra;
-              phy_cfg.pdsch.nof_common_time_ra = i;
+              phy_cfg.pdsch.nof_common_time_ra = i + 1;
             } else {
               logger.warning("Warning while building common_time_ra structure");
               return false;
@@ -822,6 +852,21 @@ bool rrc_nr::apply_sp_cell_ded_ul_pucch(const asn1::rrc_nr::pucch_cfg_s& pucch_c
       if (make_phy_sr_resource(pucch_cfg.sched_request_res_to_add_mod_list[i], &srsran_pucch_nr_sr_resource) ==
           true) { // TODO: fix that if indexing is solved
         phy_cfg.pucch.sr_resources[res_id] = srsran_pucch_nr_sr_resource;
+
+        // Set PUCCH resource
+        if (pucch_cfg.sched_request_res_to_add_mod_list[i].res_present) {
+          uint32_t pucch_res_id = pucch_cfg.sched_request_res_to_add_mod_list[i].res;
+          if (res_list_present[res_id]) {
+            phy_cfg.pucch.sr_resources[res_id].resource = res_list[pucch_res_id];
+          } else {
+            logger.warning("Warning SR's PUCCH resource is invalid (%d)", pucch_res_id);
+            phy_cfg.pucch.sr_resources[res_id].configured = false;
+          }
+        } else {
+          logger.warning("Warning SR resource is present but no PUCCH resource is assigned to it");
+          phy_cfg.pucch.sr_resources[res_id].configured = false;
+        }
+
       } else {
         logger.warning("Warning while building srsran_pucch_nr_sr_resource structure");
         return false;
diff --git a/srsue/src/stack/rrc/rrc_procedures.cc b/srsue/src/stack/rrc/rrc_procedures.cc
index de1e00e94..b2c13d9fe 100644
--- a/srsue/src/stack/rrc/rrc_procedures.cc
+++ b/srsue/src/stack/rrc/rrc_procedures.cc
@@ -1046,7 +1046,7 @@ srsran::proc_outcome_t rrc::connection_reconf_no_ho_proc::react(const bool& conf
     if (nas_pdu != nullptr) {
       memcpy(nas_pdu->msg, pdu.data(), pdu.size());
       nas_pdu->N_bytes = pdu.size();
-      rrc_ptr->nas->write_pdu(RB_ID_SRB1, std::move(nas_pdu));
+      rrc_ptr->nas->write_pdu(srsran::srb_to_lcid(srsran::lte_srb::srb1), std::move(nas_pdu));
     } else {
       rrc_ptr->logger.error("Couldn't allocate PDU in %s.", __FUNCTION__);
       return proc_outcome_t::error;
@@ -1341,9 +1341,9 @@ proc_outcome_t rrc::connection_reest_proc::init(asn1::rrc::reest_cause_e cause)
   reest_cellid = rrc_ptr->meas_cells.find_cell(reest_source_freq, reest_source_pci)->get_cell_id();
 
   Info("Starting... cause: \"%s\", UE context: {C-RNTI=0x%x, PCI=%d, CELL ID=%d}",
-       reest_cause == asn1::rrc::reest_cause_opts::recfg_fail ? "Reconfiguration failure"
-       : cause == asn1::rrc::reest_cause_opts::ho_fail        ? "Handover failure"
-                                                              : "Other failure",
+       reest_cause == asn1::rrc::reest_cause_opts::recfg_fail
+           ? "Reconfiguration failure"
+           : cause == asn1::rrc::reest_cause_opts::ho_fail ? "Handover failure" : "Other failure",
        reest_rnti,
        reest_source_pci,
        reest_cellid);
diff --git a/srsue/src/stack/ue_stack_lte.cc b/srsue/src/stack/ue_stack_lte.cc
index f5e4eddaf..83e2787ea 100644
--- a/srsue/src/stack/ue_stack_lte.cc
+++ b/srsue/src/stack/ue_stack_lte.cc
@@ -43,6 +43,8 @@ ue_stack_lte::ue_stack_lte() :
   rrc_logger(srslog::fetch_basic_logger("RRC", false)),
   usim_logger(srslog::fetch_basic_logger("USIM", false)),
   nas_logger(srslog::fetch_basic_logger("NAS", false)),
+  mac_nr_logger(srslog::fetch_basic_logger("MAC-NR", false)),
+  rrc_nr_logger(srslog::fetch_basic_logger("RRC-NR", false)),
   mac_pcap(),
   mac_nr_pcap(),
   usim(nullptr),
@@ -122,6 +124,11 @@ int ue_stack_lte::init(const stack_args_t& args_)
   nas_logger.set_level(srslog::str_to_basic_level(args.log.nas_level));
   nas_logger.set_hex_dump_max_size(args.log.nas_hex_limit);
 
+  mac_nr_logger.set_level(srslog::str_to_basic_level(args.log.mac_level));
+  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;
diff --git a/srsue/src/stack/ue_stack_nr.cc b/srsue/src/stack/ue_stack_nr.cc
index ab9d08ff3..d6caabd73 100644
--- a/srsue/src/stack/ue_stack_nr.cc
+++ b/srsue/src/stack/ue_stack_nr.cc
@@ -29,14 +29,14 @@ namespace srsue {
 ue_stack_nr::ue_stack_nr() :
   thread("STACK"),
   task_sched(64, 64),
-  mac_logger(srslog::fetch_basic_logger("MAC")),
-  rlc_logger(srslog::fetch_basic_logger("RLC", false)),
-  pdcp_logger(srslog::fetch_basic_logger("PDCP", false))
+  mac_logger(srslog::fetch_basic_logger("MAC-NR")),
+  rlc_logger(srslog::fetch_basic_logger("RLC-NR", false)),
+  pdcp_logger(srslog::fetch_basic_logger("PDCP-NR", false))
 {
   get_background_workers().set_nof_workers(2);
   mac.reset(new mac_nr(&task_sched));
-  pdcp.reset(new srsran::pdcp(&task_sched, "PDCP"));
-  rlc.reset(new srsran::rlc("RLC"));
+  pdcp.reset(new srsran::pdcp(&task_sched, "PDCP-NR"));
+  rlc.reset(new srsran::rlc("RLC-NR"));
   rrc.reset(new rrc_nr(&task_sched));
 
   // setup logging for pool, RLC and PDCP
diff --git a/srsue/src/stack/upper/nas.cc b/srsue/src/stack/upper/nas.cc
index f3e910429..86d769354 100644
--- a/srsue/src/stack/upper/nas.cc
+++ b/srsue/src/stack/upper/nas.cc
@@ -467,7 +467,7 @@ void nas::write_pdu(uint32_t lcid, unique_byte_buffer_t pdu)
   uint8 msg_type     = 0;
   uint8 sec_hdr_type = 0;
 
-  logger.info(pdu->msg, pdu->N_bytes, "DL %s PDU", rrc->get_rb_name(lcid).c_str());
+  logger.info(pdu->msg, pdu->N_bytes, "DL %s PDU", rrc->get_rb_name(lcid));
 
   // Parse the message security header
   liblte_mme_parse_msg_sec_header((LIBLTE_BYTE_MSG_STRUCT*)pdu.get(), &pd, &sec_hdr_type);
@@ -501,7 +501,7 @@ void nas::write_pdu(uint32_t lcid, unique_byte_buffer_t pdu)
 
   // Parse the message header
   liblte_mme_parse_msg_header((LIBLTE_BYTE_MSG_STRUCT*)pdu.get(), &pd, &msg_type);
-  logger.info(pdu->msg, pdu->N_bytes, "DL %s Decrypted PDU", rrc->get_rb_name(lcid).c_str());
+  logger.info(pdu->msg, pdu->N_bytes, "DL %s Decrypted PDU", rrc->get_rb_name(lcid));
 
   // drop messages if integrity protection isn't applied (see TS 24.301 Sec. 4.4.4.2)
   if (sec_hdr_type == LIBLTE_MME_SECURITY_HDR_TYPE_PLAIN_NAS) {
@@ -1423,9 +1423,8 @@ void nas::parse_security_mode_command(uint32_t lcid, unique_byte_buffer_t pdu)
     return;
   }
 
-  logger.info("Sending Security Mode Complete nas_current_ctxt.tx_count=%d, RB=%s",
-              ctxt.tx_count,
-              rrc->get_rb_name(lcid).c_str());
+  logger.info(
+      "Sending Security Mode Complete nas_current_ctxt.tx_count=%d, RB=%s", ctxt.tx_count, rrc->get_rb_name(lcid));
   rrc->write_sdu(std::move(pdu));
   ctxt.tx_count++;
 
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index 30cf0a1d0..30c4e2ef9 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -339,12 +339,12 @@ void ue::start_plot()
 bool ue::get_metrics(ue_metrics_t* m)
 {
   bzero(m, sizeof(ue_metrics_t));
-  phy->get_metrics(&m->phy);
+  phy->get_metrics(srsran::srsran_rat_t::lte, &m->phy);
+  phy->get_metrics(srsran::srsran_rat_t::nr, &m->phy_nr);
   radio->get_metrics(&m->rf);
   stack->get_metrics(&m->stack);
   gw_inst->get_metrics(m->gw, m->stack.mac[0].nof_tti);
   m->sys = sys_proc.get_metrics();
-  m->phy_nr.nof_active_cc = args.phy.nof_nr_carriers; // FIXME: temporary until PHY metrics are complete
   return true;
 }
 
diff --git a/srsue/test/metrics_test.cc b/srsue/test/metrics_test.cc
index 1a35df021..ec107c5fd 100644
--- a/srsue/test/metrics_test.cc
+++ b/srsue/test/metrics_test.cc
@@ -63,15 +63,15 @@ public:
     // random neighbour cells
     if (rand() % 2 == 0) {
       phy_meas_t neighbor = {};
-      neighbor.pci                               = 8;
-      neighbor.rsrp                              = -33;
+      neighbor.pci        = 8;
+      neighbor.rsrp       = -33;
       m->stack.rrc.neighbour_cells.push_back(neighbor);
       m->stack.rrc.neighbour_cells.push_back(neighbor); // need to add twice since we use CA
     }
 
-    m->phy_nr.nof_active_cc      = 1;
-    m->phy_nr.ch[0].rsrp         = -10.0f;
-    m->phy_nr.ch[0].pathloss     = 32;
+    m->phy.nof_active_cc         = 1;
+    m->phy.ch[0].rsrp            = -10.0f;
+    m->phy.ch[0].pathloss        = 32;
     m->stack.mac_nr[0].rx_pkts   = 100;
     m->stack.mac_nr[0].rx_errors = 2;
     m->stack.mac_nr[0].rx_brate  = 223;
diff --git a/srsue/test/ttcn3/hdr/lte_ttcn3_phy.h b/srsue/test/ttcn3/hdr/lte_ttcn3_phy.h
index 611123141..ad4cf7d1d 100644
--- a/srsue/test/ttcn3/hdr/lte_ttcn3_phy.h
+++ b/srsue/test/ttcn3/hdr/lte_ttcn3_phy.h
@@ -57,7 +57,7 @@ public:
   void        stop() override;
   void        wait_initialize() override;
   void        start_plot() override;
-  void        get_metrics(phy_metrics_t* m) override;
+  void        get_metrics(const srsran::srsran_rat_t& rat, phy_metrics_t* m) override;
   std::string get_type() override;
 
   // The interface for the SS
diff --git a/srsue/test/ttcn3/hdr/ttcn3_syssim.h b/srsue/test/ttcn3/hdr/ttcn3_syssim.h
index fab4a58cf..e8d599903 100644
--- a/srsue/test/ttcn3/hdr/ttcn3_syssim.h
+++ b/srsue/test/ttcn3/hdr/ttcn3_syssim.h
@@ -166,7 +166,7 @@ public:
   void write_pdu_mch(uint32_t lcid, unique_byte_buffer_t pdu);
   void max_retx_attempted();
 
-  std::string get_rb_name(uint32_t lcid);
+  const char* get_rb_name(uint32_t lcid);
 
   void write_sdu(uint32_t lcid, unique_byte_buffer_t sdu);
 
diff --git a/srsue/test/ttcn3/src/lte_ttcn3_phy.cc b/srsue/test/ttcn3/src/lte_ttcn3_phy.cc
index a20a24590..1afb36fcd 100644
--- a/srsue/test/ttcn3/src/lte_ttcn3_phy.cc
+++ b/srsue/test/ttcn3/src/lte_ttcn3_phy.cc
@@ -59,7 +59,7 @@ void lte_ttcn3_phy::wait_initialize() {}
 
 void lte_ttcn3_phy::start_plot() {}
 
-void lte_ttcn3_phy::get_metrics(phy_metrics_t* m) {}
+void lte_ttcn3_phy::get_metrics(const srsran::srsran_rat_t& rat, phy_metrics_t* m) {}
 
 // The interface for the SS
 void lte_ttcn3_phy::set_cell_map(const cell_list_t& cells_)
diff --git a/srsue/test/ttcn3/src/ttcn3_syssim.cc b/srsue/test/ttcn3/src/ttcn3_syssim.cc
index 8c73d3cb1..a1d3e7ab5 100644
--- a/srsue/test/ttcn3/src/ttcn3_syssim.cc
+++ b/srsue/test/ttcn3/src/ttcn3_syssim.cc
@@ -1144,12 +1144,12 @@ void ttcn3_syssim::max_retx_attempted()
   logger.error("%s not implemented.", __FUNCTION__);
 }
 
-std::string ttcn3_syssim::get_rb_name(uint32_t lcid)
+const char* ttcn3_syssim::get_rb_name(uint32_t lcid)
 {
   if (lcid < rb_id_vec.size()) {
-    return rb_id_vec.at(lcid);
+    return rb_id_vec.at(lcid).c_str();
   }
-  return std::string("RB");
+  return "RB";
 };
 
 void ttcn3_syssim::write_sdu(uint32_t lcid, unique_byte_buffer_t sdu)
diff --git a/srsue/test/upper/nas_test.cc b/srsue/test/upper/nas_test.cc
index 586d30473..1bfcb36d1 100644
--- a/srsue/test/upper/nas_test.cc
+++ b/srsue/test/upper/nas_test.cc
@@ -79,7 +79,7 @@ public:
   void        write_pdu_bcch_dlsch(unique_byte_buffer_t pdu) {}
   void        write_pdu_pcch(unique_byte_buffer_t pdu) {}
   void        write_pdu_mch(uint32_t lcid, srsran::unique_byte_buffer_t sdu) {}
-  std::string get_rb_name(uint32_t lcid) { return std::string("lcid"); }
+  const char* get_rb_name(uint32_t lcid) { return "lcid"; }
   void        write_sdu(uint32_t lcid, srsran::unique_byte_buffer_t sdu) {}
   bool        is_lcid_enabled(uint32_t lcid) { return false; }
 };
@@ -99,7 +99,7 @@ public:
     // printf("NAS generated SDU (len=%d):\n", sdu->N_bytes);
     // srsran_vec_fprint_byte(stdout, sdu->msg, sdu->N_bytes);
   }
-  std::string get_rb_name(uint32_t lcid) { return std::string("lcid"); }
+  const char* get_rb_name(uint32_t lcid) { return "lcid"; }
   uint32_t    get_last_sdu_len() { return last_sdu_len; }
   void        reset() { last_sdu_len = 0; }