diff --git a/lib/src/upper/rlc_am.cc b/lib/src/upper/rlc_am.cc
index 6905151f0..a5d7f19e7 100644
--- a/lib/src/upper/rlc_am.cc
+++ b/lib/src/upper/rlc_am.cc
@@ -315,8 +315,10 @@ int rlc_am::read_pdu(uint8_t *payload, uint32_t nof_bytes)
   // RETX if required
   if(retx_queue.size() > 0) {
     int ret = build_retx_pdu(payload, nof_bytes);
-    pthread_mutex_unlock(&mutex);
-    return ret; 
+    if (ret) {
+      pthread_mutex_unlock(&mutex);
+      return ret;
+    }
   }
 
   // Build a PDU from SDUs
@@ -471,8 +473,8 @@ int  rlc_am::build_retx_pdu(uint8_t *payload, uint32_t nof_bytes)
     if (!retx_queue.empty()) {
       retx = retx_queue.front();
     } else {
-      log->error("In build_retx_pdu(): retx_queue is empty during sanity check\n");
-      return -1;
+      log->info("In build_retx_pdu(): retx_queue is empty during sanity check, sn=%d\n", retx.sn);
+      return 0;
     }
   }
 
@@ -549,7 +551,7 @@ int rlc_am::build_segment(uint8_t *payload, uint32_t nof_bytes, rlc_amd_retx_t r
                  rrc->get_rb_name(lcid).c_str(), nof_bytes, head_len);
     return 0;
   }
-  pdu_space = nof_bytes-head_len;
+  pdu_space = nof_bytes-head_len-2;
   if(pdu_space < (retx.so_end-retx.so_start))
     retx.so_end = retx.so_start+pdu_space;
 
@@ -568,7 +570,7 @@ int rlc_am::build_segment(uint8_t *payload, uint32_t nof_bytes, rlc_amd_retx_t r
     upper += old_header.li[i];
 
     head_len    = rlc_am_packed_length(&new_header);
-    pdu_space   = nof_bytes-head_len;
+    pdu_space   = nof_bytes-head_len-2;
     if(pdu_space < (retx.so_end-retx.so_start))
       retx.so_end = retx.so_start+pdu_space;
 
@@ -1014,10 +1016,11 @@ void rlc_am::handle_control_pdu(uint8_t *payload, uint32_t nof_bytes)
         if(it->second.buf) {
           pool->deallocate(it->second.buf);
           it->second.buf = 0;
+          log->info("SN=%d removed from tx_window\n", i);
         }
+        tx_window.erase(it);
         if(update_vt_a)
         {
-          tx_window.erase(it);
           vt_a = (vt_a + 1)%MOD;
           vt_ms = (vt_ms + 1)%MOD;
         }