diff --git a/lib/include/srslte/common/block_queue.h b/lib/include/srslte/common/block_queue.h
index b04a91985..1d17e9339 100644
--- a/lib/include/srslte/common/block_queue.h
+++ b/lib/include/srslte/common/block_queue.h
@@ -24,6 +24,15 @@
  *
  */
 
+
+/******************************************************************************
+ *  File:         block_queue.h
+ *  Description:  General-purpose blocking queue. It can behave as a bounded or
+ *                unbounded blocking queue and allows blocking and non-blocking
+ *                operations in both push and pop
+ *****************************************************************************/
+
+
 #ifndef SRSLTE_BLOCK_QUEUE_H
 #define SRSLTE_BLOCK_QUEUE_H
 
@@ -32,21 +41,47 @@
 #include <utility>
 #include <pthread.h>
 #include <stdio.h>
+#include <stdint.h>
+
 namespace srslte {
 
 template<typename myobj>
 class block_queue {
 
 public:
-  block_queue<myobj>() {
+  block_queue<myobj>(int capacity = -1) {
     pthread_mutex_init(&mutex, NULL);
-    pthread_cond_init(&cvar, NULL);
+    pthread_cond_init(&cv_empty, NULL);
+    pthread_cond_init(&cv_full, NULL);
+    this->capacity = capacity;
   }
-  void push(const myobj& value) {
+  void resize(int new_capacity) {
+    capacity = new_capacity;
+  }
+  bool push_(const myobj& value, bool block) {
     pthread_mutex_lock(&mutex);
+    if (capacity > 0) {
+      if (block) {
+        while(q.size() > (uint32_t) capacity) {
+          pthread_cond_wait(&cv_full, &mutex);
+        }
+      } else {
+        pthread_mutex_unlock(&mutex);
+        return false;
+      }
+    }
     q.push(value);
-    pthread_cond_signal(&cvar);    
+    pthread_cond_signal(&cv_empty);
     pthread_mutex_unlock(&mutex);
+    return true;
+  }
+
+  void push(const myobj& value) {
+    push_(value, true);
+  }
+
+  bool try_push(const myobj& value) {
+    return push_(value, false);
   }
 
   bool try_pop(myobj *value) { 
@@ -59,6 +94,7 @@ public:
       *value = q.front(); 
       q.pop();
     }
+    pthread_cond_signal(&cv_full);
     pthread_mutex_unlock(&mutex);
     return true;
   }
@@ -66,15 +102,16 @@ public:
   myobj wait_pop() { // blocking pop
     pthread_mutex_lock(&mutex);
     while(q.empty()) {
-      pthread_cond_wait(&cvar, &mutex);
+      pthread_cond_wait(&cv_empty, &mutex);
     }
     myobj value = q.front();
     q.pop();
+    pthread_cond_signal(&cv_full);
     pthread_mutex_unlock(&mutex);
     return value;
   }
 
-  bool empty() const { // queue is empty?
+  bool empty() { // queue is empty?
     pthread_mutex_lock(&mutex);
     bool ret = q.empty();
     pthread_mutex_unlock(&mutex);
@@ -86,10 +123,20 @@ public:
     while (try_pop(item));
   }
 
+  myobj front() {
+    return q.front();
+  }
+
+  size_t size() {
+    return q.size();
+  }
+
 private:
   std::queue<myobj> q; 
   pthread_mutex_t mutex;
-  pthread_cond_t  cvar;
+  pthread_cond_t  cv_empty;
+  pthread_cond_t  cv_full;
+  int capacity;
 };
 
 }
diff --git a/lib/include/srslte/common/common.h b/lib/include/srslte/common/common.h
index 5668454e0..749765f17 100644
--- a/lib/include/srslte/common/common.h
+++ b/lib/include/srslte/common/common.h
@@ -66,7 +66,7 @@
 #define SRSLTE_BUFFER_POOL_LOG_ENABLED
 
 #ifdef SRSLTE_BUFFER_POOL_LOG_ENABLED
-#define pool_allocate (pool->allocate(__FUNCTION__))
+#define pool_allocate (pool->allocate(__PRETTY_FUNCTION__))
 #define SRSLTE_BUFFER_POOL_LOG_NAME_LEN 128
 #else
 #define pool_allocate (pool->allocate())
diff --git a/lib/include/srslte/common/msg_queue.h b/lib/include/srslte/common/msg_queue.h
deleted file mode 100644
index 0dcdc2a55..000000000
--- a/lib/include/srslte/common/msg_queue.h
+++ /dev/null
@@ -1,155 +0,0 @@
-/**
- *
- * \section COPYRIGHT
- *
- * Copyright 2013-2015 Software Radio Systems Limited
- *
- * \section LICENSE
- *
- * This file is part of the srsUE library.
- *
- * srsUE is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Affero General Public License as
- * published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- *
- * srsUE is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Affero General Public License for more details.
- *
- * A copy of the GNU Affero General Public License can be found in
- * the LICENSE file in the top-level directory of this distribution
- * and at http://www.gnu.org/licenses/.
- *
- */
-
-/******************************************************************************
- *  File:         msg_queue.h
- *  Description:  Thread-safe bounded circular buffer of srsue_byte_buffer pointers.
- *  Reference:
- *****************************************************************************/
-
-#ifndef SRSLTE_MSG_QUEUE_H
-#define SRSLTE_MSG_QUEUE_H
-
-#include "srslte/common/common.h"
-#include <pthread.h>
-
-namespace srslte {
-
-class msg_queue
-{
-public:
-  msg_queue(uint32_t capacity_ = 128)
-    :head(0)
-    ,tail(0)
-    ,unread(0)
-    ,unread_bytes(0)
-    ,capacity(capacity_)
-  {
-    buf = new byte_buffer_t*[capacity];
-    pthread_mutex_init(&mutex, NULL);
-    pthread_cond_init(&not_empty, NULL);
-    pthread_cond_init(&not_full, NULL);
-  }
-
-  ~msg_queue()
-  {
-    pthread_mutex_destroy(&mutex);
-    pthread_cond_destroy(&not_empty);
-    pthread_cond_destroy(&not_full);
-    delete [] buf;
-  }
-
-  void write(byte_buffer_t *msg)
-  {
-    pthread_mutex_lock(&mutex);
-    while(is_full()) {
-      pthread_cond_wait(&not_full, &mutex);
-    }
-    buf[head] = msg;
-    head = (head+1)%capacity;
-    unread++;
-    unread_bytes += msg->N_bytes;
-    
-    pthread_cond_signal(&not_empty);
-    pthread_mutex_unlock(&mutex);
-  }
-
-  void read(byte_buffer_t **msg)
-  {
-    pthread_mutex_lock(&mutex);
-    while(is_empty()) {
-      pthread_cond_wait(&not_empty, &mutex);
-    }
-    *msg = buf[tail];
-    tail = (tail+1)%capacity;
-    unread--;
-    unread_bytes -= (*msg)->N_bytes;
-
-    pthread_cond_signal(&not_full);
-    pthread_mutex_unlock(&mutex);
-  }
-
-  bool try_read(byte_buffer_t **msg)
-  {
-    pthread_mutex_lock(&mutex);
-    if(is_empty())
-    {
-      pthread_mutex_unlock(&mutex);
-      return false;
-    }else{
-      *msg = buf[tail];
-      tail = (tail+1)%capacity;
-      unread--;
-      unread_bytes -= (*msg)->N_bytes;
-      pthread_cond_signal(&not_full);
-      pthread_mutex_unlock(&mutex);
-      return true;
-    }
-  }
-
-  uint32_t size()
-  {
-    pthread_mutex_lock(&mutex);
-    uint32_t r = unread;
-    pthread_mutex_unlock(&mutex);
-    return r; 
-  }
-
-  uint32_t size_bytes()
-  {
-    pthread_mutex_lock(&mutex);
-    uint32_t r = unread_bytes;
-    pthread_mutex_unlock(&mutex);
-    return r; 
-  }
-
-  uint32_t size_tail_bytes()
-  {
-    pthread_mutex_lock(&mutex);
-    uint32_t r = buf[tail]->N_bytes;
-    pthread_mutex_unlock(&mutex);
-    return r; 
-  }
-
-private:
-  bool     is_empty() const { return unread == 0; }
-  bool     is_full() const { return unread == capacity; }
-
-  pthread_cond_t        not_empty;
-  pthread_cond_t        not_full;
-  pthread_mutex_t       mutex;
-  byte_buffer_t **buf;
-  uint32_t              capacity;
-  uint32_t              unread;
-  uint32_t              unread_bytes;
-  uint32_t              head;
-  uint32_t              tail;
-};
-
-} // namespace srslte
-
-
-#endif // SRSLTE_MSG_QUEUE_H
diff --git a/lib/include/srslte/common/security.h b/lib/include/srslte/common/security.h
index 80b4c0c0e..f88d48a17 100644
--- a/lib/include/srslte/common/security.h
+++ b/lib/include/srslte/common/security.h
@@ -109,7 +109,7 @@ uint8_t security_generate_k_up( uint8_t                       *k_enb,
 
 uint8_t security_128_eia1( uint8_t  *key,
                            uint32_t  count,
-                           uint8_t   bearer,
+                           uint32_t   bearer,
                            uint8_t   direction,
                            uint8_t  *msg,
                            uint32_t  msg_len,
@@ -117,7 +117,7 @@ uint8_t security_128_eia1( uint8_t  *key,
 
 uint8_t security_128_eia2( uint8_t  *key,
                            uint32_t  count,
-                           uint8_t   bearer,
+                           uint32_t   bearer,
                            uint8_t   direction,
                            uint8_t  *msg,
                            uint32_t  msg_len,
diff --git a/lib/include/srslte/upper/rlc_am.h b/lib/include/srslte/upper/rlc_am.h
index d36a81178..3a8c2cc16 100644
--- a/lib/include/srslte/upper/rlc_am.h
+++ b/lib/include/srslte/upper/rlc_am.h
@@ -31,7 +31,7 @@
 #include "srslte/common/log.h"
 #include "srslte/common/common.h"
 #include "srslte/interfaces/ue_interfaces.h"
-#include "srslte/common/msg_queue.h"
+#include "srslte/upper/rlc_tx_queue.h"
 #include "srslte/common/timeout.h"
 #include "srslte/upper/rlc_common.h"
 #include <map>
@@ -104,7 +104,7 @@ private:
   srsue::rrc_interface_rlc  *rrc;
 
   // TX SDU buffers
-  msg_queue      tx_sdu_queue;
+  rlc_tx_queue      tx_sdu_queue;
   byte_buffer_t *tx_sdu;
 
   // PDU being resegmented
diff --git a/lib/include/srslte/upper/rlc_tm.h b/lib/include/srslte/upper/rlc_tm.h
index 5408cb835..d3a8aa1ae 100644
--- a/lib/include/srslte/upper/rlc_tm.h
+++ b/lib/include/srslte/upper/rlc_tm.h
@@ -31,7 +31,7 @@
 #include "srslte/common/log.h"
 #include "srslte/common/common.h"
 #include "srslte/interfaces/ue_interfaces.h"
-#include "srslte/common/msg_queue.h"
+#include "srslte/upper/rlc_tx_queue.h"
 #include "srslte/upper/rlc_common.h"
 
 namespace srslte {
@@ -72,7 +72,7 @@ private:
   srsue::rrc_interface_rlc  *rrc;
 
   // Thread-safe queues for MAC messages
-  msg_queue    ul_queue;
+  rlc_tx_queue    ul_queue;
 };
 
 } // namespace srsue
diff --git a/lib/include/srslte/upper/rlc_tx_queue.h b/lib/include/srslte/upper/rlc_tx_queue.h
new file mode 100644
index 000000000..dee326ab6
--- /dev/null
+++ b/lib/include/srslte/upper/rlc_tx_queue.h
@@ -0,0 +1,117 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsUE library.
+ *
+ * srsUE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsUE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+/******************************************************************************
+ *  File:         rlc_tx_queue.h
+ *  Description:  Queue used in RLC TM/UM/AM TX queues.
+ *                Uses a blocking queue with bounded capacity to block higher layers
+ *                when pushing Uplink traffic
+ *  Reference:
+ *****************************************************************************/
+
+#ifndef SRSLTE_MSG_QUEUE_H
+#define SRSLTE_MSG_QUEUE_H
+
+#include "srslte/common/block_queue.h"
+#include "srslte/common/common.h"
+#include <pthread.h>
+
+namespace srslte {
+
+class rlc_tx_queue
+{
+public:
+  rlc_tx_queue(uint32_t capacity = 128) : queue((int) capacity) {
+    unread_bytes = 0;
+  }
+  void write(byte_buffer_t *msg)
+  {
+    queue.push(msg);
+    unread_bytes += msg->N_bytes;
+  }
+
+  void read(byte_buffer_t **msg)
+  {
+    byte_buffer_t *m = queue.wait_pop();
+    *msg = m;
+    if (unread_bytes > (*msg)->N_bytes) {
+      unread_bytes -= (*msg)->N_bytes;
+    } else {
+      unread_bytes = 0;
+    }
+  }
+
+  bool try_read(byte_buffer_t **msg)
+  {
+    if (queue.try_pop(msg)) {
+      if (unread_bytes > (*msg)->N_bytes) {
+        unread_bytes -= (*msg)->N_bytes;
+      } else {
+        unread_bytes = 0;
+      }
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  uint32_t size()
+  {
+    return (uint32_t) queue.size();
+  }
+
+  uint32_t size_bytes()
+  {
+    return unread_bytes;
+  }
+
+  uint32_t size_tail_bytes()
+  {
+    if (!queue.empty()) {
+      byte_buffer_t *m = queue.front();
+      if (m) {
+        return m->N_bytes;
+      }
+    }
+    return 0; 
+  }
+
+  // This is a hack to reset N_bytes counter when queue is corrupted (see line 89)
+  void reset() {
+    unread_bytes = 0;
+  }
+
+private:
+  bool     is_empty() { return queue.empty(); }
+
+  block_queue<byte_buffer_t*> queue;
+  uint32_t              unread_bytes;
+};
+
+} // namespace srslte
+
+
+#endif // SRSLTE_MSG_QUEUE_H
diff --git a/lib/include/srslte/upper/rlc_um.h b/lib/include/srslte/upper/rlc_um.h
index 8e6f527e2..d41192d6d 100644
--- a/lib/include/srslte/upper/rlc_um.h
+++ b/lib/include/srslte/upper/rlc_um.h
@@ -31,7 +31,7 @@
 #include "srslte/common/log.h"
 #include "srslte/common/common.h"
 #include "srslte/interfaces/ue_interfaces.h"
-#include "srslte/common/msg_queue.h"
+#include "srslte/upper/rlc_tx_queue.h"
 #include "srslte/upper/rlc_common.h"
 #include <pthread.h>
 #include <map>
@@ -89,7 +89,7 @@ private:
   mac_interface_timers        *mac_timers; 
 
   // TX SDU buffers
-  msg_queue           tx_sdu_queue;
+  rlc_tx_queue           tx_sdu_queue;
   byte_buffer_t      *tx_sdu;
 
   // Rx window
diff --git a/lib/src/common/logger_file.cc b/lib/src/common/logger_file.cc
index a911ae20e..abbea8f93 100644
--- a/lib/src/common/logger_file.cc
+++ b/lib/src/common/logger_file.cc
@@ -43,8 +43,8 @@ logger_file::logger_file()
 
 logger_file::~logger_file() {
   not_done = false;
-  log(new std::string("Closing log\n"));
   if(inited) {
+    log(new std::string("Closing log\n"));
     wait_thread_finish();
     flush();
     if (logfile) {
diff --git a/lib/src/common/pdu_queue.cc b/lib/src/common/pdu_queue.cc
index 8e9656116..4a45f0721 100644
--- a/lib/src/common/pdu_queue.cc
+++ b/lib/src/common/pdu_queue.cc
@@ -48,7 +48,7 @@ uint8_t* pdu_queue::request(uint32_t len)
     fprintf(stderr, "Error request buffer of invalid size %d. Max bytes %d\n", len, MAX_PDU_LEN);
     return NULL; 
   }
-  pdu_t *pdu = pool.allocate();  
+  pdu_t *pdu = pool.allocate("pdu_queue::request");
   if (!pdu) {
     if (log_h) {
       log_h->error("Not enough buffers for MAC PDU\n");      
diff --git a/lib/src/common/security.cc b/lib/src/common/security.cc
index fba1b37d9..b10b5beab 100644
--- a/lib/src/common/security.cc
+++ b/lib/src/common/security.cc
@@ -126,7 +126,7 @@ uint8_t security_generate_k_up( uint8_t                       *k_enb,
 
 uint8_t security_128_eia1( uint8_t  *key,
                            uint32_t  count,
-                           uint8_t   bearer,
+                           uint32_t   bearer,
                            uint8_t   direction,
                            uint8_t  *msg,
                            uint32_t  msg_len,
@@ -151,7 +151,7 @@ uint8_t security_128_eia1( uint8_t  *key,
 
 uint8_t security_128_eia2( uint8_t  *key,
                            uint32_t  count,
-                           uint8_t   bearer,
+                           uint32_t   bearer,
                            uint8_t   direction,
                            uint8_t  *msg,
                            uint32_t  msg_len,
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 6e7a3b33c..59f5bf58b 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -620,7 +620,7 @@ static int srslte_pdsch_codeword_decode(srslte_pdsch_t *q, srslte_pdsch_cfg_t *c
   uint32_t rv = cfg->rv[tb_idx];
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
-  if (softbuffer && data && ack) {
+  if (softbuffer && data && ack && nbits->nof_bits && nbits->nof_re) {
     INFO("Decoding PDSCH SF: %d (CW%d -> TB%d), Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
          cfg->sf_idx, codeword_idx, tb_idx, srslte_mod_string(mcs->mod), mcs->tbs,
          nbits->nof_re, nbits->nof_bits, rv);
@@ -687,7 +687,8 @@ static int srslte_pdsch_codeword_decode(srslte_pdsch_t *q, srslte_pdsch_cfg_t *c
       ret = SRSLTE_ERROR;
     }
   } else {
-    ERROR("Detected NULL pointer in TB%d &softbuffer=%p &data=%p &ack=%p", codeword_idx, softbuffer, (void*)data, ack);
+    ERROR("Detected NULL pointer in TB%d &softbuffer=%p &data=%p &ack=%p, nbits=%d, nof_re=%d",
+          codeword_idx, softbuffer, (void*)data, ack, nbits->nof_bits, nbits->nof_re);
   }
 
   return ret;
diff --git a/lib/src/upper/rlc_tm.cc b/lib/src/upper/rlc_tm.cc
index a76ebee13..0dd16bf4d 100644
--- a/lib/src/upper/rlc_tm.cc
+++ b/lib/src/upper/rlc_tm.cc
@@ -59,10 +59,8 @@ void rlc_tm::empty_queue()
 {
   // Drop all messages in TX queue
   byte_buffer_t *buf;
-  while(ul_queue.size() > 0) {
-    if (ul_queue.try_read(&buf)) {
-      pool->deallocate(buf);
-    }
+  while(ul_queue.try_read(&buf)) {
+    pool->deallocate(buf);
   }
 }
 
@@ -89,8 +87,11 @@ uint32_t rlc_tm::get_bearer()
 // PDCP interface
 void rlc_tm::write_sdu(byte_buffer_t *sdu)
 {
-  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU", rrc->get_rb_name(lcid).c_str());
+  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU, before: queue size=%d, bytes=%d",
+                rrc->get_rb_name(lcid).c_str(), ul_queue.size(), ul_queue.size_bytes());
   ul_queue.write(sdu);
+  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU, after: queue size=%d, bytes=%d",
+                rrc->get_rb_name(lcid).c_str(), ul_queue.size(), ul_queue.size_bytes());
 }
 
 // MAC interface
@@ -119,10 +120,15 @@ int rlc_tm::read_pdu(uint8_t *payload, uint32_t nof_bytes)
     log->debug("%s Complete SDU scheduled for tx. Stack latency: %ld us\n",
                rrc->get_rb_name(lcid).c_str(), buf->get_latency_us());
     pool->deallocate(buf);
-    log->info_hex(payload, pdu_size, "TX %s, %s PDU", rrc->get_rb_name(lcid).c_str(), rlc_mode_text[RLC_MODE_TM]);
+    log->info_hex(payload, pdu_size, "TX %s, %s PDU, queue size=%d, bytes=%d",
+                  rrc->get_rb_name(lcid).c_str(), rlc_mode_text[RLC_MODE_TM], ul_queue.size(), ul_queue.size_bytes());
     return pdu_size;
   } else {
     log->warning("Queue empty while trying to read\n");
+    if (ul_queue.size_bytes() > 0) {
+      log->warning("Corrupted queue: empty but size_bytes > 0. Resetting queue\n");
+      ul_queue.reset();
+    }
     return 0;
   }
 }
diff --git a/lib/test/common/msg_queue_test.cc b/lib/test/common/msg_queue_test.cc
index 393931ab5..5a4e2cc23 100644
--- a/lib/test/common/msg_queue_test.cc
+++ b/lib/test/common/msg_queue_test.cc
@@ -27,12 +27,12 @@
 #define NMSGS    1000000
 
 #include <stdio.h>
-#include "srslte/common/msg_queue.h"
+#include "srslte/upper/rlc_tx_queue.h"
 
 using namespace srslte;
 
 typedef struct {
-  msg_queue   *q;
+  rlc_tx_queue   *q;
 }args_t;
 
 void* write_thread(void *a) {
@@ -49,27 +49,26 @@ void* write_thread(void *a) {
 
 int main(int argc, char **argv) {
   bool                 result;
-  msg_queue            *q = new msg_queue;
+  rlc_tx_queue            q;
   byte_buffer_t *b;
   pthread_t            thread;
   args_t               args;
   u_int32_t            r;
 
   result = true;
-  args.q = q;
+  args.q = &q;
 
   pthread_create(&thread, NULL, &write_thread, &args);
 
   for(uint32_t i=0;i<NMSGS;i++)
   {
-    q->read(&b);
+    q.read(&b);
     memcpy(&r, b->msg, 4);
     delete b;
     if(r != i)
       result = false;
   }
 
-  delete q;
   pthread_join(thread, NULL);
 
   if(result) {
diff --git a/srsenb/hdr/enb.h b/srsenb/hdr/enb.h
index 745096daa..3ab6e9a4c 100644
--- a/srsenb/hdr/enb.h
+++ b/srsenb/hdr/enb.h
@@ -126,6 +126,7 @@ typedef struct {
   mac_args_t mac; 
   uint32_t   rrc_inactivity_timer;
   float      metrics_period_secs;
+  bool       print_buffer_state;
 }expert_args_t;
 
 typedef struct { 
@@ -156,6 +157,8 @@ public:
 
   void start_plot();
 
+  void print_pool();
+
   static void rf_msg(srslte_rf_error_t error);
 
   void handle_rf_msg(srslte_rf_error_t error);
diff --git a/srsenb/hdr/upper/rrc.h b/srsenb/hdr/upper/rrc.h
index cadb4af43..871c9ebb3 100644
--- a/srsenb/hdr/upper/rrc.h
+++ b/srsenb/hdr/upper/rrc.h
@@ -243,7 +243,8 @@ public:
     bool connect_notified; 
     
   private:
-    
+    srslte::byte_buffer_pool  *pool;
+
     struct timeval t_last_activity; 
 
     // S-TMSI for this UE
diff --git a/srsenb/src/enb.cc b/srsenb/src/enb.cc
index e91aea7bf..fdf959a95 100644
--- a/srsenb/src/enb.cc
+++ b/srsenb/src/enb.cc
@@ -256,6 +256,10 @@ void enb::start_plot() {
   phy.start_plot();
 }
 
+void enb::print_pool() {
+  srslte::byte_buffer_pool::get_instance()->print_all_buffers();
+}
+
 bool enb::get_metrics(enb_metrics_t &m)
 {
   m.rf = rf_metrics;
diff --git a/srsenb/src/mac/mac.cc b/srsenb/src/mac/mac.cc
index 463da2e25..204e7f74e 100644
--- a/srsenb/src/mac/mac.cc
+++ b/srsenb/src/mac/mac.cc
@@ -301,7 +301,7 @@ int mac::crc_info(uint32_t tti, uint16_t rnti, uint32_t nof_bytes, bool crc)
     
     // push the pdu through the queue if received correctly
     if (crc) {
-       ue_db[rnti]->push_pdu(tti, nof_bytes); 
+      ue_db[rnti]->push_pdu(tti, nof_bytes);
       pdu_process_thread.notify();      
       if (nof_bytes > 64) { // do not count RLC status messages only
         rrc_h->set_activity_user(rnti); 
@@ -732,7 +732,7 @@ void mac::timer_thread::tti_clock()
 /********************************************************
  *
  * Class that runs a thread to process DL MAC PDUs from
- * DEMU unit
+ * DEMUX unit
  *
  *******************************************************/
 mac::pdu_process::pdu_process(pdu_process_handler *h) : running(false) {
diff --git a/srsenb/src/mac/ue.cc b/srsenb/src/mac/ue.cc
index 975d6e9b1..0ddd5afea 100644
--- a/srsenb/src/mac/ue.cc
+++ b/srsenb/src/mac/ue.cc
@@ -152,6 +152,8 @@ void ue::process_pdu(uint8_t* pdu, uint32_t nof_bytes, srslte::pdu_queue::channe
     pcap->write_ul_crnti(pdu, nof_bytes, rnti, true, last_tti);
   }
 
+  pdus.deallocate(pdu);
+
   uint32_t lcid_most_data = 0;
   int most_data = -99;
   
diff --git a/srsenb/src/main.cc b/srsenb/src/main.cc
index 45dd028b0..a4d7532f0 100644
--- a/srsenb/src/main.cc
+++ b/srsenb/src/main.cc
@@ -184,6 +184,9 @@ void parse_args(all_args_t *args, int argc, char* argv[]) {
         bpo::value<uint32_t>(&args->expert.rrc_inactivity_timer)->default_value(10000),
         "Inactivity timer in ms")
 
+    ("expert.print_buffer_state",
+        bpo::value<bool>(&args->expert.print_buffer_state)->default_value(false),
+       "Prints on the console the buffer state every 10 seconds")
 
     ("rf_calibration.tx_corr_dc_gain",  bpo::value<float>(&args->rf_cal.tx_corr_dc_gain)->default_value(0.0),  "TX DC offset gain correction")
     ("rf_calibration.tx_corr_dc_phase", bpo::value<float>(&args->rf_cal.tx_corr_dc_phase)->default_value(0.0), "TX DC offset phase correction")
@@ -385,11 +388,21 @@ int main(int argc, char *argv[])
 
   bool plot_started         = false; 
   bool signals_pregenerated = false; 
-  while(running) {
+  if(running) {
     if (!plot_started && args.gui.enable) {
       enb->start_plot();
       plot_started = true; 
     }
+  }
+  int cnt=0;
+  while (running) {
+    if (args.expert.print_buffer_state) {
+      cnt++;
+      if (cnt==10) {
+        cnt=0;
+        enb->print_pool();
+      }
+    }
     sleep(1);
   }
   pthread_cancel(input);
diff --git a/srsenb/src/upper/rrc.cc b/srsenb/src/upper/rrc.cc
index 031322c5d..8abb1ed08 100644
--- a/srsenb/src/upper/rrc.cc
+++ b/srsenb/src/upper/rrc.cc
@@ -731,6 +731,7 @@ rrc::ue::ue()
   cqi_sched_sf_idx = 0;
   cqi_sched_prb_idx = 0;
   state            = RRC_STATE_IDLE;
+  pool             = srslte::byte_buffer_pool::get_instance();
 }
 
 rrc_state_t rrc::ue::get_state()
@@ -1505,7 +1506,7 @@ void rrc::ue::send_connection_reconf(srslte::byte_buffer_t *pdu)
 
 void rrc::ue::send_connection_reconf_new_bearer(LIBLTE_S1AP_E_RABTOBESETUPLISTBEARERSUREQ_STRUCT *e)
 {
-  srslte::byte_buffer_t *pdu = parent->pool->allocate(__FUNCTION__);
+  srslte::byte_buffer_t *pdu = pool_allocate;
 
   LIBLTE_RRC_DL_DCCH_MSG_STRUCT dl_dcch_msg;
   dl_dcch_msg.msg_type = LIBLTE_RRC_DL_DCCH_MSG_TYPE_RRC_CON_RECONFIG;
@@ -1592,7 +1593,7 @@ void rrc::ue::send_ue_cap_enquiry()
 void rrc::ue::send_dl_ccch(LIBLTE_RRC_DL_CCCH_MSG_STRUCT *dl_ccch_msg) 
 {
   // Allocate a new PDU buffer, pack the message and send to PDCP 
-  byte_buffer_t *pdu = parent->pool->allocate(__FUNCTION__);
+  byte_buffer_t *pdu = pool_allocate;
   if (pdu) {
     liblte_rrc_pack_dl_ccch_msg(dl_ccch_msg, (LIBLTE_BIT_MSG_STRUCT*) &parent->bit_buf);
     srslte_bit_pack_vector(parent->bit_buf.msg, pdu->msg, parent->bit_buf.N_bits);
@@ -1612,7 +1613,7 @@ void rrc::ue::send_dl_ccch(LIBLTE_RRC_DL_CCCH_MSG_STRUCT *dl_ccch_msg)
 void rrc::ue::send_dl_dcch(LIBLTE_RRC_DL_DCCH_MSG_STRUCT *dl_dcch_msg, byte_buffer_t *pdu) 
 {  
   if (!pdu) {
-    pdu = parent->pool->allocate(__FUNCTION__);
+    pdu = pool_allocate;
   }
   if (pdu) {
     liblte_rrc_pack_dl_dcch_msg(dl_dcch_msg, (LIBLTE_BIT_MSG_STRUCT*) &parent->bit_buf);
diff --git a/srsenb/src/upper/s1ap.cc b/srsenb/src/upper/s1ap.cc
index b48e8a248..6eafe5030 100644
--- a/srsenb/src/upper/s1ap.cc
+++ b/srsenb/src/upper/s1ap.cc
@@ -87,7 +87,7 @@ void s1ap::get_metrics(s1ap_metrics_t &m)
 
 void s1ap::run_thread()
 {
-  srslte::byte_buffer_t *pdu = pool_allocate;
+  srslte::byte_buffer_t *pdu = pool->allocate("s1ap::run_thread");
   if (!pdu) {
     s1ap_log->error("Fatal Error: Couldn't allocate buffer in s1ap::run_thread().\n");
     return;
diff --git a/srsepc/src/mme/mme.cc b/srsepc/src/mme/mme.cc
index 1a385d8dc..21554bcdc 100644
--- a/srsepc/src/mme/mme.cc
+++ b/srsepc/src/mme/mme.cc
@@ -115,7 +115,7 @@ mme::stop()
 void
 mme::run_thread()
 {
-  srslte::byte_buffer_t *pdu = m_pool->allocate();
+  srslte::byte_buffer_t *pdu = m_pool->allocate("mme::run_thread");
   uint32_t sz = SRSLTE_MAX_BUFFER_SIZE_BYTES - SRSLTE_BUFFER_HEADER_OFFSET;
 
   struct sockaddr_in enb_addr;
diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index c56031ef3..47ebb209f 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -280,7 +280,10 @@ private:
           memcpy(&cur_grant, &grant, sizeof(Tgrant));
 
           if (payload_buffer_ptr) {
-            Warning("DL PID %d: Allocating buffer already allocated\n", pid);
+            Warning("DL PID %d: Allocating buffer already allocated. Deallocating.\n", pid);
+            if (pid != HARQ_BCCH_PID) {
+              harq_entity->demux_unit->deallocate(payload_buffer_ptr);
+            }
           }
 
           // Instruct the PHY To combine the received data and attempt to decode it
@@ -296,7 +299,7 @@ private:
             pthread_mutex_unlock(&mutex);
             return;
           }
-          action->decode_enabled[tid]= true;
+          action->decode_enabled[tid] = true;
           action->rv[tid] = cur_grant.rv[tid];
           action->softbuffers[tid] = &softbuffer;
           memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(Tphygrant));
@@ -327,11 +330,13 @@ private:
           }
         }
 
-        pthread_mutex_unlock(&mutex);
+        if (!action->decode_enabled[tid]) {
+          pthread_mutex_unlock(&mutex);
+        }
+
       }
 
       void tb_decoded(bool ack_) {
-        pthread_mutex_lock(&mutex);
         ack = ack_;
         if (ack) {
           if (pid == HARQ_BCCH_PID) {
diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index fb89fbbe9..a381940ec 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -440,6 +440,7 @@ private:
   bool initiated;
   bool ho_start;
   bool go_idle;
+  bool go_rlf;
 
   // Measurements sub-class
   class rrc_meas {
@@ -546,7 +547,7 @@ private:
 
   void process_phy_meas();
   void process_new_phy_meas(phy_meas_t meas);
-  std::queue<phy_meas_t> phy_meas_q;
+  srslte::block_queue<phy_meas_t> phy_meas_q;
 
   // Cell selection/reselection functions/variables
   typedef struct {
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index b74139bf7..ca00a07ff 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -642,12 +642,12 @@ float phch_recv::get_tx_cfo()
 
   if (worker_com->args->cfo_is_doppler) {
     ret *= -1;
-  }
-
-  if (radio_h->get_freq_offset() != 0.0f) {
-    /* Compensates the radio frequency offset applied equally to DL and UL */
-    const float offset_hz = (float) radio_h->get_freq_offset() * (1.0f - ul_dl_factor);
-    ret = cfo - offset_hz;
+  } else {
+    /* Compensates the radio frequency offset applied equally to DL and UL. Does not work in doppler mode */
+    if (radio_h->get_freq_offset() != 0.0f) {
+      const float offset_hz = (float) radio_h->get_freq_offset() * (1.0f - ul_dl_factor);
+      ret = cfo - offset_hz;
+    }
   }
 
   return ret/15000;
diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc
index eb1b87aa7..bf8ccffaa 100644
--- a/srsue/src/phy/phy.cc
+++ b/srsue/src/phy/phy.cc
@@ -222,7 +222,7 @@ void phy::set_timeadv_rar(uint32_t ta_cmd) {
 
 void phy::set_timeadv(uint32_t ta_cmd) {
   uint32_t new_nta = srslte_N_ta_new(n_ta, ta_cmd);
-  //sf_recv.set_time_adv_sec(((float) new_nta)*SRSLTE_LTE_TS);
+  sf_recv.set_time_adv_sec(((float) new_nta)*SRSLTE_LTE_TS);
   Info("PHY:   Set TA: ta_cmd: %d, n_ta: %d, old_n_ta: %d, ta_usec: %.1f\n", ta_cmd, new_nta, n_ta, ((float) new_nta)*SRSLTE_LTE_TS*1e6);
   n_ta = new_nta;
 }
diff --git a/srsue/src/phy/prach.cc b/srsue/src/phy/prach.cc
index 278f19eb5..05aa56a7f 100644
--- a/srsue/src/phy/prach.cc
+++ b/srsue/src/phy/prach.cc
@@ -175,7 +175,8 @@ int prach::tx_tti() {
 
 cf_t *prach::generate(float cfo, uint32_t *nof_sf, float *target_power) {
 
-  if (cell_initiated && preamble_idx >= 0 && nof_sf) {
+  if (cell_initiated && preamble_idx >= 0 && nof_sf && preamble_idx <= 64 &&
+      srslte_cell_isvalid(&cell) && len < MAX_LEN_SF * 30720 && len > 0) {
 
     // Correct CFO before transmission FIXME: UL SISO Only
     srslte_cfo_correct(&cfo_h, buffer[preamble_idx], signal_buffer, cfo / srslte_symbol_sz(cell.nof_prb));
@@ -196,6 +197,8 @@ cf_t *prach::generate(float cfo, uint32_t *nof_sf, float *target_power) {
 
     return signal_buffer;
   } else {
+    Error("PRACH: Invalid parameters: cell_initiated=%d, preamble_idx=%d, cell.nof_prb=%d, len=%d\n",
+          cell_initiated, preamble_idx, cell.nof_prb, len);
     return NULL;
   }
 }
diff --git a/srsue/src/upper/gw.cc b/srsue/src/upper/gw.cc
index 4c3cf6301..4fd439f77 100644
--- a/srsue/src/upper/gw.cc
+++ b/srsue/src/upper/gw.cc
@@ -126,7 +126,7 @@ void gw::write_pdu(uint32_t lcid, srslte::byte_buffer_t *pdu)
     int n = write(tun_fd, pdu->msg, pdu->N_bytes); 
     if(n > 0 && (pdu->N_bytes != (uint32_t)n))
     {
-      gw_log->warning("DL TUN/TAP write failure\n");
+      gw_log->warning("DL TUN/TAP write failure. Wanted to write %d B but only wrote %d B.\n", pdu->N_bytes, n);
     } 
   }
   pool->deallocate(pdu);
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 4e09ba3f9..eebbd38b8 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -57,6 +57,8 @@ rrc::rrc()
   neighbour_cells.reserve(NOF_NEIGHBOUR_CELLS);
   initiated = false;
   running = false;
+  go_idle = false;
+  go_rlf  = false;
 }
 
 rrc::~rrc()
@@ -249,6 +251,11 @@ void rrc::run_tti(uint32_t tti) {
           go_idle = false;
           leave_connected();
         }
+        if (go_rlf) {
+          go_rlf = false;
+          // Initiate connection re-establishment procedure after RLF
+          send_con_restablish_request(LIBLTE_RRC_CON_REEST_REQ_CAUSE_OTHER_FAILURE);
+        }
         break;
       default:break;
     }
@@ -527,10 +534,10 @@ void rrc::new_phy_meas(float rsrp, float rsrq, uint32_t tti, int earfcn_i, int p
 /* Processes all pending PHY measurements in queue. Must be called from a mutexed function
  */
 void rrc::process_phy_meas() {
-  while(!phy_meas_q.empty()) {
+  phy_meas_t m;
+  while(phy_meas_q.try_pop(&m)) {
     rrc_log->debug("MEAS:  Processing measurement. %lu measurements in queue\n", phy_meas_q.size());
-    process_new_phy_meas(phy_meas_q.front());
-    phy_meas_q.pop();
+    process_new_phy_meas(m);
   }
 }
 
@@ -823,6 +830,15 @@ rrc::cs_ret_t rrc::cell_selection()
     }
   }
   if (serving_cell->in_sync) {
+    if (!phy->cell_is_camping()) {
+      rrc_log->info("Serving cell is in-sync but not camping. Selecting it...\n");
+      if (phy->cell_select(&serving_cell->phy_cell)) {
+        rrc_log->info("Selected serving cell OK.\n");
+      } else {
+        serving_cell->in_sync = false;
+        rrc_log->error("Could not camp on serving cell.\n");
+      }
+    }
     return SAME_CELL;
   }
   // If can not find any suitable cell, search again
@@ -836,7 +852,7 @@ rrc::cs_ret_t rrc::cell_selection()
 // Cell selection criteria Section 5.2.3.2 of 36.304
 bool rrc::cell_selection_criteria(float rsrp, float rsrq)
 {
-  if (get_srxlev(rsrp) > 0) {
+  if (get_srxlev(rsrp) > 0 || !serving_cell->has_sib3()) {
     return true;
   } else {
     return false;
@@ -1057,12 +1073,10 @@ int rrc::find_neighbour_cell(uint32_t earfcn, uint32_t pci) {
  */
 void rrc::radio_link_failure() {
   // TODO: Generate and store failure report
-
   rrc_log->warning("Detected Radio-Link Failure\n");
   rrc_log->console("Warning: Detected Radio-Link Failure\n");
   if (state == RRC_STATE_CONNECTED) {
-    // Initiate connection re-establishment procedure
-    send_con_restablish_request(LIBLTE_RRC_CON_REEST_REQ_CAUSE_OTHER_FAILURE);
+    go_rlf = true;
   }
 }
 
@@ -1162,12 +1176,18 @@ void rrc::send_con_restablish_request(LIBLTE_RRC_CON_REEST_REQ_CAUSE_ENUM cause)
   bzero(&ul_ccch_msg, sizeof(LIBLTE_RRC_UL_CCCH_MSG_STRUCT));
 
   uint16_t crnti;
+  uint16_t pci;
+  uint32_t cellid;
   if (cause == LIBLTE_RRC_CON_REEST_REQ_CAUSE_HANDOVER_FAILURE) {
-    crnti = ho_src_rnti;
+    crnti  = ho_src_rnti;
+    pci    = ho_src_cell.get_pci();
+    cellid = ho_src_cell.get_cell_id();
   } else {
     mac_interface_rrc::ue_rnti_t uernti;
     mac->get_rntis(&uernti);
-    crnti = uernti.crnti;
+    crnti  = uernti.crnti;
+    pci    = serving_cell->get_pci();
+    cellid = serving_cell->get_cell_id();
   }
 
   // Compute shortMAC-I
@@ -1176,36 +1196,38 @@ void rrc::send_con_restablish_request(LIBLTE_RRC_CON_REEST_REQ_CAUSE_ENUM cause)
   bzero(varShortMAC_packed, 16);
   uint8_t *msg_ptr = varShortMAC;
 
-  // ASN.1 encode byte-aligned VarShortMAC-Input
-  liblte_rrc_pack_cell_identity_ie(serving_cell->get_cell_id(), &msg_ptr);
-  msg_ptr = &varShortMAC[4];
-  liblte_rrc_pack_phys_cell_id_ie(phy->get_current_pci(), &msg_ptr);
-  msg_ptr = &varShortMAC[4+2];
+  // ASN.1 encode VarShortMAC-Input
+  liblte_rrc_pack_cell_identity_ie(cellid, &msg_ptr);
+  liblte_rrc_pack_phys_cell_id_ie(pci, &msg_ptr);
   liblte_rrc_pack_c_rnti_ie(crnti, &msg_ptr);
-  srslte_bit_pack_vector(varShortMAC, varShortMAC_packed, (4+2+4)*8);
 
-  rrc_log->info("Generated varShortMAC: cellId=0x%x, PCI=%d, rnti=%d\n",
-                serving_cell->get_cell_id(), phy->get_current_pci(), crnti);
+  // byte align (already zero-padded)
+  uint32_t N_bits  = (uint32_t) (msg_ptr-varShortMAC);
+  uint32_t N_bytes = ((N_bits-1)/8+1);
+  srslte_bit_pack_vector(varShortMAC, varShortMAC_packed, N_bytes*8);
+
+  rrc_log->info("Encoded varShortMAC: cellId=0x%x, PCI=%d, rnti=0x%x (%d bytes, %d bits)\n",
+                cellid, pci, crnti, N_bytes, N_bits);
 
   // Compute MAC-I
   uint8_t mac_key[4];
   switch(integ_algo) {
     case INTEGRITY_ALGORITHM_ID_128_EIA1:
       security_128_eia1(&k_rrc_int[16],
-                        1,
-                        1,
-                        1,
+                        0xffffffff,    // 32-bit all to ones
+                        0x1f,          // 5-bit all to ones
+                        1,             // 1-bit to one
                         varShortMAC_packed,
-                        10,
+                        N_bytes,
                         mac_key);
       break;
     case INTEGRITY_ALGORITHM_ID_128_EIA2:
       security_128_eia2(&k_rrc_int[16],
-                        1,
-                        1,
-                        1,
+                        0xffffffff,    // 32-bit all to ones
+                        0x1f,          // 5-bit all to ones
+                        1,             // 1-bit to one
                         varShortMAC_packed,
-                        10,
+                        N_bytes,
                         mac_key);
       break;
     default:
@@ -1215,8 +1237,8 @@ void rrc::send_con_restablish_request(LIBLTE_RRC_CON_REEST_REQ_CAUSE_ENUM cause)
   // Prepare ConnectionRestalishmentRequest packet
   ul_ccch_msg.msg_type = LIBLTE_RRC_UL_CCCH_MSG_TYPE_RRC_CON_REEST_REQ;
   ul_ccch_msg.msg.rrc_con_reest_req.ue_id.c_rnti = crnti;
-  ul_ccch_msg.msg.rrc_con_reest_req.ue_id.phys_cell_id = phy->get_current_pci();
-  ul_ccch_msg.msg.rrc_con_reest_req.ue_id.short_mac_i = mac_key[1] << 8 | mac_key[0];
+  ul_ccch_msg.msg.rrc_con_reest_req.ue_id.phys_cell_id = pci;
+  ul_ccch_msg.msg.rrc_con_reest_req.ue_id.short_mac_i = mac_key[2] << 8 | mac_key[3];
   ul_ccch_msg.msg.rrc_con_reest_req.cause = cause;
 
   rrc_log->info("Initiating RRC Connection Reestablishment Procedure\n");
@@ -1249,9 +1271,10 @@ void rrc::send_con_restablish_request(LIBLTE_RRC_CON_REEST_REQ_CAUSE_ENUM cause)
       }
     } else {
       rrc_log->warning("Could not re-synchronize with cell.\n");
+      go_idle = true;
     }
   } else {
-    rrc_log->info("Selected cell no longer suitable for camping. Going to IDLE\n");
+    rrc_log->info("Selected cell no longer suitable for camping (in_sync=%s). Going to IDLE\n", serving_cell->in_sync?"yes":"no");
     go_idle = true;
   }
 }
@@ -1439,6 +1462,7 @@ void rrc::ho_ra_completed(bool ra_successful) {
 bool rrc::con_reconfig_ho(LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig)
 {
   if (reconfig->mob_ctrl_info.target_pci == phy->get_current_pci()) {
+    rrc_log->console("Warning: Received HO command to own cell\n");
     rrc_log->warning("Received HO command to own cell\n");
     return false;
   }
@@ -1500,7 +1524,7 @@ void rrc::con_reconfig_failed()
 
   if (security_is_activated) {
     // Start the Reestablishment Procedure
-    send_con_restablish_request(LIBLTE_RRC_CON_REEST_REQ_CAUSE_OTHER_FAILURE);
+    send_con_restablish_request(LIBLTE_RRC_CON_REEST_REQ_CAUSE_RECONFIG_FAILURE);
   } else {
     go_idle = true;
   }
diff --git a/srsue/src/upper/usim.cc b/srsue/src/upper/usim.cc
index ca9075edb..3e0e086e5 100644
--- a/srsue/src/upper/usim.cc
+++ b/srsue/src/upper/usim.cc
@@ -49,8 +49,8 @@ void usim::init(usim_args_t *args, srslte::log *usim_log_)
   if(32 == args->op.length()) {
     str_to_hex(args->op, op);
   } else {
-    usim_log->error("Invalid length for OP: %zu should be %d", args->op.length(), 32);
-    usim_log->console("Invalid length for OP: %zu should be %d", args->op.length(), 32);
+    usim_log->error("Invalid length for OP: %zu should be %d\n", args->op.length(), 32);
+    usim_log->console("Invalid length for OP: %zu should be %d\n", args->op.length(), 32);
   }
 
   if(15 == args->imsi.length()) {
@@ -61,8 +61,8 @@ void usim::init(usim_args_t *args, srslte::log *usim_log_)
       imsi += imsi_c[i] - '0';
     }
   } else {
-    usim_log->error("Invalid length for ISMI: %zu should be %d", args->imsi.length(), 15);
-    usim_log->console("Invalid length for IMSI: %zu should be %d", args->imsi.length(), 15);
+    usim_log->error("Invalid length for ISMI: %zu should be %d\n", args->imsi.length(), 15);
+    usim_log->console("Invalid length for IMSI: %zu should be %d\n", args->imsi.length(), 15);
   }
 
   if(15 == args->imei.length()) {
@@ -73,15 +73,15 @@ void usim::init(usim_args_t *args, srslte::log *usim_log_)
       imei += imei_c[i] - '0';
     }
   } else {
-    usim_log->error("Invalid length for IMEI: %zu should be %d", args->imei.length(), 15);
-    usim_log->console("Invalid length for IMEI: %zu should be %d", args->imei.length(), 15);
+    usim_log->error("Invalid length for IMEI: %zu should be %d\n", args->imei.length(), 15);
+    usim_log->console("Invalid length for IMEI: %zu should be %d\n", args->imei.length(), 15);
   }
 
   if(32 == args->k.length()) {
     str_to_hex(args->k, k);
   } else {
-    usim_log->error("Invalid length for K: %zu should be %d", args->k.length(), 32);
-    usim_log->console("Invalid length for K: %zu should be %d", args->k.length(), 32);
+    usim_log->error("Invalid length for K: %zu should be %d\n", args->k.length(), 32);
+    usim_log->console("Invalid length for K: %zu should be %d\n", args->k.length(), 32);
   }
 
   auth_algo = auth_algo_milenage;
@@ -115,7 +115,7 @@ bool usim::get_imsi_vec(uint8_t* imsi_, uint32_t n)
   }
 
   if(NULL == imsi_ || n < 15) {
-    usim_log->error("Invalid parameters to get_imsi_vec");
+    usim_log->error("Invalid parameters to get_imsi_vec\n");
     return false;
   }
 
@@ -135,7 +135,7 @@ bool usim::get_imei_vec(uint8_t* imei_, uint32_t n)
   }
 
   if(NULL == imei_ || n < 15) {
-    usim_log->error("Invalid parameters to get_imei_vec");
+    usim_log->error("Invalid parameters to get_imei_vec\n");
     return false;
   }