From d0b1f3b08417cfc0d6e6f215471957a2e2d3ddc3 Mon Sep 17 00:00:00 2001
From: ismagom <gomezi@tcd.ie>
Date: Fri, 28 Aug 2015 11:08:19 +0200
Subject: [PATCH] Working iperf 1 Mbps UL/DL

---
 matlab/tests/pusch_decode_test.m              |  12 +-
 .../include/srsapps/common/thread_pool.h      |  24 ++-
 srsapps/common/src/thread_pool.cc             | 185 ++++++++++++------
 srsapps/radio/include/srsapps/radio/radio.h   |   4 +-
 .../radio/include/srsapps/radio/radio_uhd.h   |   4 +-
 srsapps/radio/src/radio_uhd.cc                |  14 +-
 srsapps/ue/mac/src/demux.cc                   |   2 +-
 srsapps/ue/mac/src/dl_harq.cc                 |   2 +-
 srsapps/ue/mac/src/mac.cc                     | 100 +++++-----
 srsapps/ue/mac/src/mux.cc                     |  20 +-
 srsapps/ue/mac/src/pdu.cc                     |   9 +-
 srsapps/ue/mac/src/ul_harq.cc                 |   2 +-
 srsapps/ue/mac/test/mac_test.cc               |  13 +-
 .../phy/include/srsapps/ue/phy/phch_common.h  |   8 +-
 .../phy/include/srsapps/ue/phy/phch_worker.h  |  10 +
 srsapps/ue/phy/include/srsapps/ue/phy/phy.h   |   7 +-
 srsapps/ue/phy/src/phch_common.cc             |  24 +--
 srsapps/ue/phy/src/phch_recv.cc               |  12 +-
 srsapps/ue/phy/src/phch_worker.cc             |  43 +++-
 srsapps/ue/phy/src/phy.cc                     |  40 ++--
 srslte/examples/usrp_txrx.c                   |   2 +-
 srslte/include/srslte/fec/softbuffer.h        |  14 +-
 srslte/lib/cuhd/src/cuhd_imp.cpp              |   3 +-
 srslte/lib/fec/src/softbuffer.c               |  32 ++-
 24 files changed, 379 insertions(+), 207 deletions(-)

diff --git a/matlab/tests/pusch_decode_test.m b/matlab/tests/pusch_decode_test.m
index 345e1f967..014eacc90 100644
--- a/matlab/tests/pusch_decode_test.m
+++ b/matlab/tests/pusch_decode_test.m
@@ -1,12 +1,12 @@
-ueConfig=struct('NCellID',1,'NULRB',25,'NSubframe',2,'RNTI',62,'CyclicPrefixUL','Normal','NTxAnts',1,'Shortened',0);
-puschConfig=struct('NTurboDecIts',5,'NLayers',1,'OrthCover','Off','PRBSet',(0:24)','Modulation','16QAM','RV',3);
+ueConfig=struct('NCellID',1,'NULRB',25,'NSubframe',8,'RNTI',62,'CyclicPrefixUL','Normal','NTxAnts',1,'Shortened',0);
+puschConfig=struct('NTurboDecIts',5,'NLayers',1,'OrthCover','Off','PRBSet',(0:24)','Modulation','16QAM','RV',0);
 
 TBS=9144;
-cfo=1000;
-%t0=1;
-%x=[rx(t0:end); zeros(t0-1,1)];
+cfo=1500;
+t0=1;
+x=[rx(t0:end); zeros(t0-1,1)];
 
-x=rx;
+%x=rx;
 
 subframe_rx=lteSCFDMADemodulate(ueConfig,x.*exp(-1i*2*pi*cfo/15000*transpose(1:length(x))/512));
 idx=ltePUSCHIndices(ueConfig,puschConfig);
diff --git a/srsapps/common/include/srsapps/common/thread_pool.h b/srsapps/common/include/srsapps/common/thread_pool.h
index 13899769d..e4abcec64 100644
--- a/srsapps/common/include/srsapps/common/thread_pool.h
+++ b/srsapps/common/include/srsapps/common/thread_pool.h
@@ -57,6 +57,7 @@ public:
     void setup(uint32_t id, thread_pool *parent, uint32_t prio=0);
     void stop();
     uint32_t get_id();
+    void release();
   protected: 
     virtual void work_imp() = 0;
   private: 
@@ -65,7 +66,7 @@ public:
     bool running; 
     void run_thread();  
     void wait_to_start();
-    void finished();
+    void finished();    
   };
     
   
@@ -73,6 +74,7 @@ public:
   void    init_worker(uint32_t id, worker*, uint32_t prio = 0);              
   void    stop();
   worker* wait_worker();              
+  worker* wait_worker(uint32_t tti);              
   void    start_worker(worker*);              
   void    start_worker(uint32_t id);              
   worker* get_worker(uint32_t id);
@@ -80,13 +82,25 @@ public:
   
 
 private:
+
+  bool find_finished_worker(uint32_t tti, uint32_t *id);
+  
+  typedef enum {
+    IDLE, 
+    START_WORK,
+    WORKER_READY,
+    WORKING
+  }worker_status;
+  
   std::vector<worker*> workers; 
-  std::vector<bool> begin;
-  std::stack<worker*> available_workers;
-  pthread_cond_t cvar_start, cvar_stop;
-  pthread_mutex_t mutex_start, mutex_stop;
   uint32_t nof_workers; 
   bool running;
+  pthread_cond_t cvar_queue;
+  pthread_mutex_t mutex_queue;
+  std::vector<worker_status> status;
+  std::vector<pthread_cond_t> cvar;
+  std::vector<pthread_mutex_t> mutex;
+  std::stack<worker*> available_workers;
 };
 }
   
diff --git a/srsapps/common/src/thread_pool.cc b/srsapps/common/src/thread_pool.cc
index 41de8114f..f79dfac88 100644
--- a/srsapps/common/src/thread_pool.cc
+++ b/srsapps/common/src/thread_pool.cc
@@ -28,6 +28,11 @@
 #include <stdio.h>
 #include "srsapps/common/thread_pool.h"
 
+#define DEBUG 0
+#define debug_thread(fmt, ...) do { if(DEBUG) printf(fmt, __VA_ARGS__); } while(0)
+
+#define USE_QUEUE
+
 namespace srslte {
  
   
@@ -58,50 +63,38 @@ uint32_t thread_pool::worker::get_id()
 void thread_pool::worker::stop()
 {
   running = false; 
+  pthread_cond_signal(&my_parent->cvar[my_id]);
   wait_thread_finish();
 }
 
-void thread_pool::worker::wait_to_start()
+thread_pool::thread_pool(uint32_t nof_workers_)  : 
+                                  workers(nof_workers_),
+                                  status(nof_workers_),
+                                  cvar(nof_workers_),
+                                  mutex(nof_workers_)
+                                  
 {
-  pthread_mutex_lock(&my_parent->mutex_start); 
-  while(!my_parent->begin[my_id]) {
-    pthread_cond_wait(&my_parent->cvar_start, &my_parent->mutex_start);
-  }
-  my_parent->begin[my_id] = false; 
-  pthread_mutex_unlock(&my_parent->mutex_start);
-}
-
-void thread_pool::worker::finished()
-{
-  pthread_mutex_lock(&my_parent->mutex_stop);   
-  my_parent->available_workers.push(this); 
-  pthread_cond_signal(&my_parent->cvar_stop);
-  pthread_mutex_unlock(&my_parent->mutex_stop);
-}
-
-thread_pool::thread_pool(uint32_t nof_workers_)  : workers(nof_workers_), begin(nof_workers_) {
   nof_workers = nof_workers_;
   for (int i=0;i<nof_workers;i++) {
     workers[i] = NULL;
+    status[i] = IDLE; 
+    pthread_mutex_init(&mutex[i], NULL);
+    pthread_cond_init(&cvar[i], NULL);
   }
+  pthread_mutex_init(&mutex_queue, NULL);
+  pthread_cond_init(&cvar_queue, NULL);
   running = true; 
-  pthread_mutex_init(&mutex_start, NULL);
-  pthread_mutex_init(&mutex_stop, NULL);
-  pthread_cond_init(&cvar_start, NULL);
-  pthread_cond_init(&cvar_stop, NULL);
 }
 
 void thread_pool::init_worker(uint32_t id, worker *obj, uint32_t prio)
 {
   if (id < nof_workers) {
-    pthread_mutex_lock(&mutex_stop);   
-    begin[id] = false; 
+    pthread_mutex_lock(&mutex_queue);   
     workers[id] = obj; 
     available_workers.push(obj);    
-    printf("Added worker to available_workers, len=%lu\n", available_workers.size());
     obj->setup(id, this, prio);
-    pthread_cond_signal(&cvar_stop);
-    pthread_mutex_unlock(&mutex_stop);  
+    pthread_cond_signal(&cvar_queue);
+    pthread_mutex_unlock(&mutex_queue);  
   }
 }
 
@@ -109,9 +102,6 @@ void thread_pool::stop()
 {
   /* Stop any thread waiting for available worker */
   running = false; 
-  pthread_mutex_lock(&mutex_stop);
-  pthread_cond_signal(&cvar_start);
-  pthread_mutex_unlock(&mutex_stop);
   
   /* Now stop all workers */
   for (uint32_t i=0;i<nof_workers;i++) {
@@ -121,24 +111,120 @@ void thread_pool::stop()
       start_worker(i);
       workers[i]->wait_thread_finish();
     }
+    pthread_cond_destroy(&cvar[i]);
+    pthread_mutex_destroy(&mutex[i]);
   }
-  
-  /* And destroy mutexes */ 
-  pthread_mutex_destroy(&mutex_start);
-  pthread_mutex_destroy(&mutex_stop);
-  pthread_cond_destroy(&cvar_start);
-  pthread_cond_destroy(&cvar_stop);
+  pthread_cond_destroy(&cvar_queue);
+  pthread_mutex_destroy(&mutex_queue);
 }
 
+
+void thread_pool::worker::release()
+{
+  finished();
+}
+
+void thread_pool::worker::wait_to_start()
+{
+  
+  debug_thread("wait_to_start() id=%d, status=%d, enter\n", my_id, my_parent->status[my_id]);
+
+  pthread_mutex_lock(&my_parent->mutex[my_id]); 
+  while(my_parent->status[my_id] != START_WORK && running) {
+    pthread_cond_wait(&my_parent->cvar[my_id], &my_parent->mutex[my_id]);
+  }
+  my_parent->status[my_id] = WORKING; 
+  pthread_mutex_unlock(&my_parent->mutex[my_id]);
+
+  debug_thread("wait_to_start() id=%d, status=%d, exit\n", my_id, my_parent->status[my_id]);
+}
+
+void thread_pool::worker::finished()
+{
+#ifdef USE_QUEUE
+  //my_parent->available_workers.push(this); 
+  pthread_mutex_lock(&my_parent->mutex[my_id]); 
+  my_parent->status[my_id] = IDLE; 
+  pthread_mutex_unlock(&my_parent->mutex[my_id]); 
+
+  pthread_mutex_lock(&my_parent->mutex_queue); 
+  pthread_cond_signal(&my_parent->cvar_queue);
+  pthread_mutex_unlock(&my_parent->mutex_queue); 
+#else
+  pthread_mutex_lock(&my_parent->mutex[my_id]); 
+  my_parent->status[my_id] = IDLE; 
+  pthread_cond_signal(&my_parent->cvar[my_id]);
+  pthread_mutex_unlock(&my_parent->mutex[my_id]);   
+#endif
+}
+
+
+thread_pool::worker* thread_pool::wait_worker()
+{
+  wait_worker(0);
+}
+
+bool thread_pool::find_finished_worker(uint32_t tti, uint32_t *id) {
+  for(int i=0;i<nof_workers;i++) {
+    if (status[i] == IDLE) {
+      *id = i; 
+      return true; 
+    }
+  }
+  return false; 
+}
+
+thread_pool::worker* thread_pool::wait_worker(uint32_t tti)
+{
+  thread_pool::worker *x; 
+  
+#ifdef USE_QUEUE
+  debug_thread("wait_worker() - enter - tti=%d, state0=%d, state1=%d\n", tti, status[0], status[1]);
+  pthread_mutex_lock(&mutex_queue); 
+  uint32_t id = 0; 
+  while(!find_finished_worker(tti, &id) && running) {
+    pthread_cond_wait(&cvar_queue, &mutex_queue);    
+  }
+  pthread_mutex_unlock(&mutex_queue);
+  if (running) {
+    x = workers[id];
+    pthread_mutex_lock(&mutex[id]); 
+    status[id] = WORKER_READY;
+    pthread_mutex_unlock(&mutex[id]); 
+} else {
+    x = NULL; 
+  }  
+  debug_thread("wait_worker() - exit - id=%d, x=0x%x\n", id, x);
+#else
+  
+  uint32_t id = tti%nof_workers;
+  pthread_mutex_lock(&mutex[id]); 
+  while(status[id] != IDLE && running) {
+    pthread_cond_wait(&cvar[id], &mutex[id]);    
+  }
+  if (running) {
+    x = (worker*) workers[id];
+    status[id] = WORKER_READY;
+  } else {
+    x = NULL; 
+  }
+  pthread_mutex_unlock(&mutex[id]);
+#endif  
+  return x; 
+}
+
+
 void thread_pool::start_worker(uint32_t id) {
-  if (workers[id]) {
-    pthread_mutex_lock(&mutex_start); 
-    begin[id%nof_workers] = true; 
-    pthread_cond_signal(&cvar_start);
-    pthread_mutex_unlock(&mutex_start);
+  if (id < nof_workers) {
+    pthread_mutex_lock(&mutex[id]); 
+    status[id] = START_WORK;
+    pthread_cond_signal(&cvar[id]);
+    pthread_mutex_unlock(&mutex[id]);
+    debug_thread("start_worker() id=%d, status=%d\n", id, status[id]);
   }
 }
 
+
 void thread_pool::start_worker(worker* x)
 {
   for (uint32_t i=0;i<nof_workers;i++) {
@@ -161,23 +247,6 @@ uint32_t thread_pool::get_nof_workers()
   return nof_workers;
 }
 
-thread_pool::worker* thread_pool::wait_worker()
-{
-  thread_pool::worker *x; 
-  pthread_mutex_lock(&mutex_stop); 
-  while(available_workers.empty() && running) {
-    pthread_cond_wait(&cvar_stop, &mutex_stop);    
-  }
-  if (running) {
-    x = (worker*) available_workers.top();
-    available_workers.pop();
-  } else {
-    x = NULL; 
-  }
-  pthread_mutex_unlock(&mutex_stop);
-  return x; 
-}
-
 }
 
 
diff --git a/srsapps/radio/include/srsapps/radio/radio.h b/srsapps/radio/include/srsapps/radio/radio.h
index 8f192a3a6..a5cf206ec 100644
--- a/srsapps/radio/include/srsapps/radio/radio.h
+++ b/srsapps/radio/include/srsapps/radio/radio.h
@@ -61,7 +61,9 @@ namespace srslte {
 
       virtual float get_tx_gain() = 0; 
       virtual float get_rx_gain() = 0;
-      
+
+      // This is used for debugging/trace purposes
+      virtual void set_tti(uint32_t tti) = 0;      
   }; 
 }
 
diff --git a/srsapps/radio/include/srsapps/radio/radio_uhd.h b/srsapps/radio/include/srsapps/radio/radio_uhd.h
index c8be0aabb..a1805fbd3 100644
--- a/srsapps/radio/include/srsapps/radio/radio_uhd.h
+++ b/srsapps/radio/include/srsapps/radio/radio_uhd.h
@@ -74,6 +74,8 @@ namespace srslte {
       void start_rx();
       void stop_rx();
       
+      void set_tti(uint32_t tti);
+      
     private:
       
       void save_trace(uint32_t is_eob, srslte_timestamp_t *usrp_time);
@@ -96,7 +98,7 @@ namespace srslte {
       trace<uint32_t> tr_tx_time;
       trace<uint32_t> tr_is_eob;
       bool trace_enabled;
-      uint32_t my_tti;
+      uint32_t tti;
   }; 
 }
 
diff --git a/srsapps/radio/src/radio_uhd.cc b/srsapps/radio/src/radio_uhd.cc
index 58b621eea..88b0ef530 100644
--- a/srsapps/radio/src/radio_uhd.cc
+++ b/srsapps/radio/src/radio_uhd.cc
@@ -125,7 +125,10 @@ bool radio_uhd::tx_end()
 
 void radio_uhd::start_trace() {
   trace_enabled = true; 
-  my_tti = 0; 
+}
+
+void radio_uhd::set_tti(uint32_t tti_) {
+  tti = tti_; 
 }
 
 void radio_uhd::write_trace(std::string filename)
@@ -138,13 +141,12 @@ void radio_uhd::write_trace(std::string filename)
 
 void radio_uhd::save_trace(uint32_t is_eob, srslte_timestamp_t *tx_time) {
   if (trace_enabled) {
-    tr_local_time.push_cur_time_us(my_tti);
+    tr_local_time.push_cur_time_us(tti);
     srslte_timestamp_t usrp_time; 
     cuhd_get_time(uhd, &usrp_time.full_secs, &usrp_time.frac_secs);
-    tr_usrp_time.push(my_tti, srslte_timestamp_uint32(&usrp_time));
-    tr_tx_time.push(my_tti, srslte_timestamp_uint32(tx_time));
-    tr_is_eob.push(my_tti, is_eob);
-    my_tti++;
+    tr_usrp_time.push(tti, srslte_timestamp_uint32(&usrp_time));
+    tr_tx_time.push(tti, srslte_timestamp_uint32(tx_time));
+    tr_is_eob.push(tti, is_eob);
   }
 }
 
diff --git a/srsapps/ue/mac/src/demux.cc b/srsapps/ue/mac/src/demux.cc
index cae35dbdf..ad7afb0b7 100644
--- a/srsapps/ue/mac/src/demux.cc
+++ b/srsapps/ue/mac/src/demux.cc
@@ -95,7 +95,7 @@ uint8_t* demux::request_buffer(uint32_t len)
   uint8_t idx=0;
   if(find_unused_queue(&idx)) {
     if (idx > 0) {
-      printf("Using queue %d for MAC PDU\n", idx);
+      //printf("Using queue %d for MAC PDU\n", idx);
     }
     used_q[idx] = true; 
     uint8_t *buff = (uint8_t*) pdu_q[idx].request();
diff --git a/srsapps/ue/mac/src/dl_harq.cc b/srsapps/ue/mac/src/dl_harq.cc
index 1a24b5f9a..51b5288b3 100644
--- a/srsapps/ue/mac/src/dl_harq.cc
+++ b/srsapps/ue/mac/src/dl_harq.cc
@@ -220,7 +220,7 @@ void dl_harq_entity::dl_harq_process::new_grant_dl(mac_interface_phy::mac_grant_
   
   if (is_new_transmission(grant)) {
     ack = false; 
-    srslte_softbuffer_rx_reset(&softbuffer);
+    srslte_softbuffer_rx_reset_tbs(&softbuffer, cur_grant.n_bytes*8);
   }
   
   // Save grant 
diff --git a/srsapps/ue/mac/src/mac.cc b/srsapps/ue/mac/src/mac.cc
index 4a7c01dde..f7e9182a3 100644
--- a/srsapps/ue/mac/src/mac.cc
+++ b/srsapps/ue/mac/src/mac.cc
@@ -77,6 +77,7 @@ bool mac::init(phy_interface *phy, rlc_interface_mac *rlc, log *log_h_)
 void mac::stop()
 {
   started = false;   
+  ttisync.increase();
   wait_thread_finish();
 }
 
@@ -139,59 +140,62 @@ void mac::run_thread() {
     /* Warning: Here order of invocation of procedures is important!! */
     tti = (ttisync.wait() + 1)%10240;
     
-    log_h->step(tti);
+    if (started) {
+      log_h->step(tti);
+        
+      search_si_rnti();
       
-    search_si_rnti();
-    
-    // Step all procedures 
-    bsr_procedure.step(tti);
-    
-    // Check if BSR procedure need to start SR 
-    
-    if (bsr_procedure.need_to_send_sr()) {
-      Debug("Starting SR procedure by BSR request, PHY TTI=%d\n", phy_h->get_current_tti());
-      sr_procedure.start();
-    }
-    if (bsr_procedure.need_to_reset_sr()) {
-      Debug("Resetting SR procedure by BSR request\n");
-      sr_procedure.reset();
-    }
-    sr_procedure.step(tti);
-
-    // Check SR if we need to start RA 
-    if (sr_procedure.need_random_access()) {
-      Warning("Starting RA procedure by MAC order is DISABLED\n");
-      //ra_procedure.start_mac_order();
-    }
-    
-    // Check if there is pending CCCH SDU in Mux unit 
-    if (mux_unit.is_pending_ccch_sdu()) {
-      if (!ra_procedure.in_progress() && !ra_procedure.is_successful()) {
-        ra_procedure.start_rlc_order();
+      // Step all procedures 
+      bsr_procedure.step(tti);
+      
+      // Check if BSR procedure need to start SR 
+      
+      if (bsr_procedure.need_to_send_sr()) {
+        Debug("Starting SR procedure by BSR request, PHY TTI=%d\n", phy_h->get_current_tti());
+        sr_procedure.start();
       }
-    }
-    
-    ra_procedure.step(tti);
-    //phr_procedure.step(tti);
+      if (bsr_procedure.need_to_reset_sr()) {
+        Debug("Resetting SR procedure by BSR request\n");
+        sr_procedure.reset();
+      }
+      sr_procedure.step(tti);
 
-    // FIXME: Do here DTX and look for UL grants only when needed
-    if (ra_procedure.is_successful() && !signals_pregenerated) {
-      // Configure PHY to look for UL C-RNTI grants
-      uint16_t crnti = params_db.get_param(mac_interface_params::RNTI_C);
-      phy_h->pdcch_ul_search(SRSLTE_RNTI_USER, crnti);
-      phy_h->pdcch_dl_search(SRSLTE_RNTI_USER, crnti);
+      // Check SR if we need to start RA 
+      if (sr_procedure.need_random_access()) {
+        Warning("Starting RA procedure by MAC order is DISABLED\n");
+        //ra_procedure.start_mac_order();
+      }
+      
+      // Check if there is pending CCCH SDU in Mux unit 
+      if (mux_unit.is_pending_ccch_sdu()) {
+        if (!ra_procedure.in_progress() && !ra_procedure.is_successful()) {
+          ra_procedure.start_rlc_order();
+        }
+      }
+      
+      ra_procedure.step(tti);
+      //phr_procedure.step(tti);
+
+      // FIXME: Do here DTX and look for UL grants only when needed
+      if (ra_procedure.is_successful() && !signals_pregenerated) {
+        // Configure PHY to look for UL C-RNTI grants
+        uint16_t crnti = params_db.get_param(mac_interface_params::RNTI_C);
+        phy_h->pdcch_ul_search(SRSLTE_RNTI_USER, crnti);
+        phy_h->pdcch_dl_search(SRSLTE_RNTI_USER, crnti);
+        
+        // Pregenerate UL signals and C-RNTI scrambling sequences
+        Info("Pre-generating UL signals and C-RNTI scrambling sequences\n");
+        ((phy*) phy_h)->enable_pregen_signals(true);
+        ((phy*) phy_h)->set_crnti(crnti);
+        Info("Done\n");
+        signals_pregenerated = true; 
+      }
+      
+      timers_db.step_all();
+      
+      demux_unit.process_pdus();
       
-      // Pregenerate UL signals and C-RNTI scrambling sequences
-      Info("Pre-generating UL signals and C-RNTI scrambling sequences\n");
-      ((phy*) phy_h)->enable_pregen_signals(true);
-      ((phy*) phy_h)->set_crnti(crnti);
-      Info("Done\n");
-      signals_pregenerated = true; 
     }
-    
-    timers_db.step_all();
-    
-    demux_unit.process_pdus();
   }  
 }
 
diff --git a/srsapps/ue/mac/src/mux.cc b/srsapps/ue/mac/src/mux.cc
index 08b53060f..39d794163 100644
--- a/srsapps/ue/mac/src/mux.cc
+++ b/srsapps/ue/mac/src/mux.cc
@@ -133,8 +133,13 @@ sch_subh::cetype bsr_format_convert(bsr_proc::bsr_format_t format) {
 // Multiplexing and logical channel priorization as defined in Section 5.4.3
 uint8_t* mux::pdu_get(uint8_t *payload, uint32_t pdu_sz)
 {
+  
+  pdu_msg.init_tx(payload, pdu_sz, true);
 
-  pthread_mutex_lock(&mutex);
+  if (pthread_mutex_trylock(&mutex)) {
+    printf("M");fflush(stdout);
+    pthread_mutex_lock(&mutex);
+  }
     
   // Update Bj
   for (int i=0;i<NOF_UL_LCH;i++) {    
@@ -149,8 +154,6 @@ uint8_t* mux::pdu_get(uint8_t *payload, uint32_t pdu_sz)
   
 // Logical Channel Procedure
    
-  pdu_msg.init_tx(payload, pdu_sz, true);
-  
   // MAC control element for C-RNTI or data from UL-CCCH
   bool is_first = true; 
   if (!allocate_sdu(0, &pdu_msg, &is_first)) {
@@ -208,6 +211,8 @@ uint8_t* mux::pdu_get(uint8_t *payload, uint32_t pdu_sz)
     while (allocate_sdu(lchid_sorted[i], &pdu_msg));   
   }
 
+  pthread_mutex_unlock(&mutex);
+
   bool send_bsr = bsr_procedure->generate_bsr_on_ul_grant(pdu_msg.rem_size(), &bsr);
   // Insert Padding BSR if not inserted Regular/Periodic BSR 
   if (!bsr_payload_sz && send_bsr) {
@@ -223,11 +228,12 @@ uint8_t* mux::pdu_get(uint8_t *payload, uint32_t pdu_sz)
   }
 
   Debug("Assembled MAC PDU msg size %d/%d bytes\n", pdu_msg.size(), pdu_sz);
-  //pdu_msg.fprint(stdout);  
-  pthread_mutex_unlock(&mutex);
 
   /* Generate MAC PDU and save to buffer */
-  return pdu_msg.write_packet();   
+  uint8_t *ret = pdu_msg.write_packet();   
+  
+
+  return ret; 
 }
 
 void mux::append_crnti_ce_next_tx(uint16_t crnti) {
@@ -262,7 +268,7 @@ bool mux::allocate_sdu(uint32_t lcid, sch_pdu *pdu_msg, int max_sdu_sz, uint32_t
         pdu_msg->next();
         int sdu_len2 = sdu_len; 
         sdu_len = pdu_msg->get()->set_sdu(lcid, sdu_len, rlc, is_first?*is_first:false);
-        if (sdu_len >= 0) { // new SDU could be added
+        if (sdu_len > 0) { // new SDU could be added
           if (is_first) {
             *is_first = false;           
           }
diff --git a/srsapps/ue/mac/src/pdu.cc b/srsapps/ue/mac/src/pdu.cc
index f260546a5..dac87f15c 100644
--- a/srsapps/ue/mac/src/pdu.cc
+++ b/srsapps/ue/mac/src/pdu.cc
@@ -140,7 +140,8 @@ uint8_t* sch_pdu::write_packet()
     head_and_ce_sz -= rem_len - 2; 
   }
   if (head_and_ce_sz >= sdu_offset_start) {
-    fprintf(stderr, "Writting PDU: head_and_ce_sz<sdu_offset_start (%d<%d)\n", head_and_ce_sz, sdu_offset_start);
+    fprintf(stderr, "Writting PDU: head_and_ce_sz>=sdu_offset_start (%d>=%d). pdu_len=%d, total_sdu_len=%d\n", 
+            head_and_ce_sz, sdu_offset_start, pdu_len, total_sdu_len);
     return NULL; 
   }
   
@@ -471,13 +472,15 @@ int sch_subh::set_sdu(uint32_t lcid_, uint32_t requested_bytes, rlc_interface_ma
     // Copy data and get final number of bytes written to the MAC PDU 
     int sdu_sz = rlc->read_pdu(lcid, payload, requested_bytes);
     
-    if (sdu_sz < 0) {
+    if (sdu_sz < 0 || sdu_sz > requested_bytes) {
       return -1;
+    } else if (sdu_sz == 0) {
+      return 0; 
     }
 
     // Save final number of written bytes
     nof_bytes = sdu_sz;
-    
+
     ((sch_pdu*)parent)->add_sdu(nof_bytes);
     ((sch_pdu*)parent)->update_space_sdu(nof_bytes, is_first);
     return (int) nof_bytes; 
diff --git a/srsapps/ue/mac/src/ul_harq.cc b/srsapps/ue/mac/src/ul_harq.cc
index 81754ce30..07d03cef6 100644
--- a/srsapps/ue/mac/src/ul_harq.cc
+++ b/srsapps/ue/mac/src/ul_harq.cc
@@ -285,7 +285,7 @@ void ul_harq_entity::ul_harq_process::generate_new_tx(uint32_t tti_tx, bool is_m
                                                       mac_interface_phy::tb_action_ul_t *action)
 {
   if (grant) {
-    srslte_softbuffer_tx_reset(&softbuffer);
+    srslte_softbuffer_tx_reset_tbs(&softbuffer, cur_grant.n_bytes*8);
     memcpy(&cur_grant, grant, sizeof(mac_interface_phy::mac_grant_t));
     harq_feedback = false; 
     is_grant_configured = true; 
diff --git a/srsapps/ue/mac/test/mac_test.cc b/srsapps/ue/mac/test/mac_test.cc
index 29d3754bc..b012e1c87 100644
--- a/srsapps/ue/mac/test/mac_test.cc
+++ b/srsapps/ue/mac/test/mac_test.cc
@@ -368,7 +368,7 @@ public:
         return 6;
       }
     } else if (lcid == 1) {
-      if (connsetup_decoded && nsegm_dcch < 2) {        
+      if (connsetup_decoded && nsegm_dcch < 2) {    
         return lengths[nsegm_dcch];
       } else if (send_ack == 1) {
         return 2; 
@@ -409,14 +409,15 @@ public:
           memcpy(payload, setupComplete, 80);
           return 80; 
         } else {
+          uint32_t r = 0; 
           if (nof_bytes >= lengths[nsegm_dcch]) {
             printf("Sending Connection Setup Complete %d/2 length %d\n", nsegm_dcch, lengths[nsegm_dcch]);
             memcpy(payload, setupComplete_segm[nsegm_dcch], lengths[nsegm_dcch]);            
+            r = lengths[nsegm_dcch];
+            nsegm_dcch++;
           } else {
-            bzero(payload, nof_bytes);
-          }          
-          uint32_t r = lengths[nsegm_dcch];
-          nsegm_dcch++;
+            r = 0;
+          }                    
           return r; 
         }
       } else if (send_ack == 1) {
@@ -550,7 +551,7 @@ int main(int argc, char *argv[])
   
   while(1) {
     uint32_t tti; 
-    if (my_rlc.mib_decoded) {
+    if (my_rlc.mib_decoded && mac.get_current_tti()) {
       if (!my_rlc.sib1_decoded) {
         usleep(10000);
         tti = mac.get_current_tti();           
diff --git a/srsapps/ue/phy/include/srsapps/ue/phy/phch_common.h b/srsapps/ue/phy/include/srsapps/ue/phy/phch_common.h
index e88169212..2fa2e99ed 100644
--- a/srsapps/ue/phy/include/srsapps/ue/phy/phch_common.h
+++ b/srsapps/ue/phy/include/srsapps/ue/phy/phch_common.h
@@ -28,6 +28,7 @@
 
 #include <pthread.h>
 #include <string.h>
+#include <vector>
 #include "srslte/srslte.h"
 #include "srsapps/common/mac_interface.h"
 #include "srsapps/radio/radio.h"
@@ -49,7 +50,7 @@ namespace ue {
     log               *log_h; 
     mac_interface_phy *mac;
 
-    phch_common();
+    phch_common(uint32_t nof_workers);
     void init(phy_params *_params, log *_log, radio *_radio, mac_interface_phy *_mac);
     
     /* For RNTI searches, -1 means now or forever */
@@ -76,9 +77,7 @@ namespace ue {
     int  sr_last_tx_tti; 
     
   private: 
-    pthread_mutex_t    tx_mutex; 
-    pthread_cond_t     tx_cvar; 
-    uint32_t           tx_tti_cnt; 
+    std::vector<pthread_mutex_t>    tx_mutex; 
     
     bool               is_first_of_burst;
     radio             *radio_h;
@@ -105,6 +104,7 @@ namespace ue {
     pending_ack_t pending_ack[10];
     
     bool is_first_tx;
+    uint32_t nof_workers;
     
   };
   
diff --git a/srsapps/ue/phy/include/srsapps/ue/phy/phch_worker.h b/srsapps/ue/phy/include/srsapps/ue/phy/phch_worker.h
index 52be6f04b..1c4e6d66c 100644
--- a/srsapps/ue/phy/include/srsapps/ue/phy/phch_worker.h
+++ b/srsapps/ue/phy/include/srsapps/ue/phy/phch_worker.h
@@ -31,6 +31,7 @@
 #include "srslte/srslte.h"
 #include "srsapps/common/thread_pool.h"
 #include "srsapps/common/phy_interface.h"
+#include "srsapps/common/trace.h"
 #include "srsapps/ue/phy/phch_common.h"
 
 
@@ -60,6 +61,9 @@ public:
   void  set_crnti(uint16_t rnti);
   void  enable_pregen_signals(bool enabled);
   
+  void start_trace();
+  void write_trace(std::string filename);
+  
 private: 
   /* Inherited from thread_pool::worker. Function called every subframe to run the DL/UL processing */
   void work_imp();
@@ -85,6 +89,12 @@ private:
   bool srs_is_ready_to_send();
   void normalize();
   
+  void tr_log_start();
+  void tr_log_end();
+  struct timeval tr_time[3];
+  trace<uint32_t> tr_exec;
+  bool trace_enabled; 
+  
   /* Common objects */  
   phch_common    *phy;
   srslte_cell_t  cell; 
diff --git a/srsapps/ue/phy/include/srsapps/ue/phy/phy.h b/srsapps/ue/phy/include/srsapps/ue/phy/phy.h
index dc2379a34..43a2e5906 100644
--- a/srsapps/ue/phy/include/srsapps/ue/phy/phy.h
+++ b/srsapps/ue/phy/include/srsapps/ue/phy/phy.h
@@ -111,7 +111,7 @@ public:
   
 private:
     
-  const static int NOF_WORKERS         = 1; 
+  const static int NOF_WORKERS         = 2; 
   const static int SF_RECV_THREAD_PRIO = 1;
   const static int WORKERS_THREAD_PRIO = 0; 
   
@@ -131,11 +131,6 @@ private:
   
   bool         init_(radio *radio_handler, mac_interface_phy *mac, log *log_h, bool do_agc);
 
-  trace<uint32_t> tr_start_time;
-  trace<uint32_t> tr_end_time;
-  bool tr_enabled; 
-  void tr_log_start();
-  void tr_log_end();    
 };
 
 } 
diff --git a/srsapps/ue/phy/src/phch_common.cc b/srsapps/ue/phy/src/phch_common.cc
index 8b7d8a716..680939895 100644
--- a/srsapps/ue/phy/src/phch_common.cc
+++ b/srsapps/ue/phy/src/phch_common.cc
@@ -34,12 +34,13 @@
 namespace srslte {                                                                                                                                                                                                                                                                                                                                                                                                                                                     
 namespace ue {
 
-phch_common::phch_common()
+phch_common::phch_common(uint32_t nof_workers_) : tx_mutex(nof_workers_)
 {
   params_db = NULL; 
   log_h     = NULL; 
   radio_h   = NULL; 
   mac       = NULL; 
+  nof_workers = nof_workers_;
   sr_enabled        = false; 
   is_first_of_burst = true; 
   is_first_tx       = true; 
@@ -56,8 +57,9 @@ void phch_common::init(phy_params *_params, log *_log, radio *_radio, mac_interf
   is_first_tx = true; 
   sr_last_tx_tti = -1;
   
-  pthread_mutex_init(&tx_mutex, NULL);
-  pthread_cond_init(&tx_cvar, NULL);
+  for (int i=0;i<nof_workers;i++) {
+    pthread_mutex_init(&tx_mutex[i], NULL);
+  }
 }
 
 bool phch_common::ul_rnti_active(uint32_t tti) {
@@ -176,30 +178,24 @@ void phch_common::worker_end(uint32_t tti, bool tx_enable,
                                    srslte_timestamp_t tx_time) 
 {
 
-/*  pthread_mutex_lock(&tx_mutex);
   
   // Wait previous TTIs to be transmitted 
   if (is_first_tx) {
-    tx_tti_cnt = tti; 
     is_first_tx = false; 
   } else {
-    while(tti != tx_tti_cnt) {
-      pthread_cond_wait(&tx_cvar, &tx_mutex);
-    }
+    pthread_mutex_lock(&tx_mutex[tti%nof_workers]);
   }
-  */
+  radio_h->set_tti(tti); 
   if (tx_enable) {
     radio_h->tx(buffer, nof_samples, tx_time);
     is_first_of_burst = false; 
   } else if (!is_first_of_burst) {
     radio_h->tx_end();
     is_first_of_burst = true;   
-  } /* else do nothing, just update tti counter */
+  } 
   
-  tx_tti_cnt = (tx_tti_cnt + 1) % 10240;
-
-  //pthread_cond_signal(&tx_cvar);
-  //pthread_mutex_unlock(&tx_mutex);
+  // Trigger next transmission 
+  pthread_mutex_unlock(&tx_mutex[(tti+1)%nof_workers]);
 }    
 
 }
diff --git a/srsapps/ue/phy/src/phch_recv.cc b/srsapps/ue/phy/src/phch_recv.cc
index 2cc11bcb6..e410d8684 100644
--- a/srsapps/ue/phy/src/phch_recv.cc
+++ b/srsapps/ue/phy/src/phch_recv.cc
@@ -286,12 +286,14 @@ void phch_recv::run_thread()
         } 
        break;
       case SYNC_DONE:
-        worker = (phch_worker*) workers_pool->wait_worker();
+        tti = (tti + 1) % 10240;
+        worker = (phch_worker*) workers_pool->wait_worker(tti);
         if (worker) {          
           buffer = worker->get_buffer();
           if (srslte_ue_sync_zerocopy(&ue_sync, buffer) == 1) {
-            tti = (tti + 1) % 10240;
             log_h->step(tti);
+
+            Debug("Worker %d synchronized\n", worker->get_id());
             
             float cfo = srslte_ue_sync_get_cfo(&ue_sync)/15000; 
             worker->set_cfo(cfo);
@@ -304,6 +306,8 @@ void phch_recv::run_thread()
             srslte_timestamp_add(&tx_time, 0, 4e-3 - time_adv_sec);
             srslte_timestamp_add(&tx_time_prach, 0, 4e-3);
             worker->set_tx_time(tx_time);
+            
+            Debug("Settting TTI=%d to worker %d\n", tti, worker->get_id());
             worker->set_tti(tti);
 
             // Check if we need to TX a PRACH 
@@ -316,8 +320,12 @@ void phch_recv::run_thread()
               prach_buffer->send(radio_h, cfo, tx_time_prach);
               radio_h->tx_end();              
             }            
+            Debug("Starting worker id %d\n", worker->get_id());
             workers_pool->start_worker(worker);             
+            Debug("Done worker id %d\n", worker->get_id());
             mac->tti_clock(tti);
+          } else {
+            worker->release();
           }
         } else {
           // wait_worker() only returns NULL if it's being closed. Quit now to avoid unnecessary loops here
diff --git a/srsapps/ue/phy/src/phch_worker.cc b/srsapps/ue/phy/src/phch_worker.cc
index 420a2b204..a5afc9630 100644
--- a/srsapps/ue/phy/src/phch_worker.cc
+++ b/srsapps/ue/phy/src/phch_worker.cc
@@ -35,7 +35,7 @@ namespace srslte {
 
 #define log_h phy->log_h
 
-phch_worker::phch_worker()
+phch_worker::phch_worker() : tr_exec(10240)
 {
   phy = NULL; 
   signal_buffer = NULL; 
@@ -43,6 +43,7 @@ phch_worker::phch_worker()
   cell_initiated  = false; 
   pregen_enabled  = false; 
   rar_cqi_request = false; 
+  trace_enabled   = false; 
   cfi = 0;
   
   reset_ul_params();
@@ -120,7 +121,11 @@ void phch_worker::work_imp()
   if (!cell_initiated) {
     return; 
   }
+  
+  Debug("TTI %d running\n", tti);
 
+  tr_log_start();
+  
   reset_uci();
 
   bool ul_grant_available = false; 
@@ -205,9 +210,11 @@ void phch_worker::work_imp()
     encode_srs();
     tx_signal = true; 
   } 
+
+  tr_log_end();
   
   phy->worker_end(tti, tx_signal, signal_buffer, SRSLTE_SF_LEN_PRB(cell.nof_prb), tx_time);
-
+  
   if (dl_action.decode_enabled && !dl_action.generate_ack_callback) {
     phy->mac->tb_decoded(dl_ack, dl_mac_grant.rnti_type, dl_mac_grant.pid);
   }
@@ -447,7 +454,7 @@ void phch_worker::set_tx_time(srslte_timestamp_t _tx_time)
 }
 
 void phch_worker::normalize() {
-  srslte_vec_norm_cfc(signal_buffer, 0.6, signal_buffer, SRSLTE_SF_LEN_PRB(cell.nof_prb));  
+  srslte_vec_norm_cfc(signal_buffer, 0.8, signal_buffer, SRSLTE_SF_LEN_PRB(cell.nof_prb));  
 }
 
 void phch_worker::encode_pusch(srslte_ra_ul_grant_t *grant, uint8_t *payload, uint32_t current_tx_nb, 
@@ -477,6 +484,12 @@ void phch_worker::encode_pusch(srslte_ra_ul_grant_t *grant, uint8_t *payload, ui
         rnti, ue_ul.pusch.shortened?"yes":"no");
 
   normalize();
+  
+  /*
+  char filename[128];
+  sprintf(filename, "pusch%d",tti+4);
+  srslte_vec_save_file(filename, signal_buffer, SRSLTE_SF_LEN_PRB(cell.nof_prb)*sizeof(cf_t));
+  */
 }
 
 void phch_worker::encode_pucch()
@@ -600,7 +613,31 @@ void phch_worker::set_ul_params()
   
 }
 
+/********** Execution time trace function ************/
 
+void phch_worker::start_trace() {
+  trace_enabled = true; 
+}
+
+void phch_worker::write_trace(std::string filename) {
+  tr_exec.writeToBinary(filename + ".exec");
+}
+
+void phch_worker::tr_log_start()
+{
+  if (trace_enabled) {
+    gettimeofday(&tr_time[1], NULL);
+  }
+}
+
+void phch_worker::tr_log_end()
+{
+  if (trace_enabled) {
+    gettimeofday(&tr_time[2], NULL);
+    get_time_interval(tr_time);
+    tr_exec.push(tti, tr_time[0].tv_usec);
+  }
+}
 
 
   }
diff --git a/srsapps/ue/phy/src/phy.cc b/srsapps/ue/phy/src/phy.cc
index dcd325a34..56bb20558 100644
--- a/srsapps/ue/phy/src/phy.cc
+++ b/srsapps/ue/phy/src/phy.cc
@@ -25,6 +25,8 @@
  *
  */
 
+#include <string>
+#include <sstream>
 #include <string.h>
 #include <strings.h>
 #include <pthread.h>
@@ -38,10 +40,14 @@
 #include "srsapps/ue/phy/phy.h"
 #include "srsapps/ue/phy/phch_worker.h"
 
+using namespace std; 
+
 namespace srslte {
 namespace ue {
 
-phy::phy() : tr_end_time(1024*10), tr_start_time(1024*10), workers_pool(NOF_WORKERS), workers(NOF_WORKERS)
+phy::phy() : workers_pool(NOF_WORKERS), 
+             workers(NOF_WORKERS), 
+             workers_common(NOF_WORKERS)
 {
 }
 
@@ -83,40 +89,24 @@ bool phy::init_(radio* radio_handler_, mac_interface_phy *mac, log *log_h_, bool
 }
 void phy::start_trace()
 {
-  tr_enabled = true; 
+  for (int i=0;i<NOF_WORKERS;i++) {
+    workers[i].start_trace();
+  }
+  printf("trace started\n");
 }
 
 void phy::write_trace(std::string filename)
 {
-  tr_start_time.writeToBinary(filename + ".start");
-  tr_end_time.writeToBinary(filename + ".end");
-}
-
-void phy::tr_log_start()
-{
-  if (tr_enabled) {
-    tr_start_time.push_cur_time_us(sf_recv.get_current_tti());
-  }
-}
-
-void phy::tr_log_end()
-{
-  if (tr_enabled) {
-    tr_end_time.push_cur_time_us(sf_recv.get_current_tti());
+  for (int i=0;i<NOF_WORKERS;i++) {
+    string i_str = static_cast<ostringstream*>( &(ostringstream() << i) )->str();
+    workers[i].write_trace(filename + "_" + i_str);
   }
 }
 
 void phy::stop()
 {  
-  workers_pool.stop();
   sf_recv.stop();
-  
-  for (int i=0;i<NOF_WORKERS;i++) {
-    workers[i].free_cell();
-    workers[i].stop();
-  }
-    
-  prach_buffer.free_cell(); 
+  workers_pool.stop();
 }
 
 void phy::set_timeadv_rar(uint32_t ta_cmd) {
diff --git a/srslte/examples/usrp_txrx.c b/srslte/examples/usrp_txrx.c
index f86ce4e26..5784febd1 100644
--- a/srslte/examples/usrp_txrx.c
+++ b/srslte/examples/usrp_txrx.c
@@ -37,7 +37,7 @@
 #include "srslte/cuhd/cuhd.h"
 #include "srslte/srslte.h"
 
-uint32_t nof_prb          = 25;
+uint32_t nof_prb    = 25;
 uint32_t nof_frames = 20; 
 
 float uhd_rx_gain=40, uhd_tx_gain=40, uhd_freq=2.4e9; 
diff --git a/srslte/include/srslte/fec/softbuffer.h b/srslte/include/srslte/fec/softbuffer.h
index 2dedfb5d0..22210be5f 100644
--- a/srslte/include/srslte/fec/softbuffer.h
+++ b/srslte/include/srslte/fec/softbuffer.h
@@ -55,7 +55,13 @@ typedef struct SRSLTE_API {
 SRSLTE_API int  srslte_softbuffer_rx_init(srslte_softbuffer_rx_t * q,
                                           uint32_t nof_prb);
 
-SRSLTE_API void srslte_softbuffer_rx_reset(srslte_softbuffer_rx_t *p); 
+SRSLTE_API void srslte_softbuffer_rx_reset(srslte_softbuffer_rx_t *p);
+
+SRSLTE_API void srslte_softbuffer_rx_reset_tbs(srslte_softbuffer_rx_t *q, 
+                                               uint32_t tbs); 
+
+SRSLTE_API void srslte_softbuffer_rx_reset_cb(srslte_softbuffer_rx_t *q, 
+                                              uint32_t nof_cb); 
 
 SRSLTE_API void srslte_softbuffer_rx_free(srslte_softbuffer_rx_t *p);
 
@@ -64,6 +70,12 @@ SRSLTE_API int  srslte_softbuffer_tx_init(srslte_softbuffer_tx_t * q,
 
 SRSLTE_API void srslte_softbuffer_tx_reset(srslte_softbuffer_tx_t *p); 
 
+SRSLTE_API void srslte_softbuffer_tx_reset_tbs(srslte_softbuffer_tx_t *q, 
+                                               uint32_t tbs); 
+
+SRSLTE_API void srslte_softbuffer_tx_reset_cb(srslte_softbuffer_tx_t *q, 
+                                              uint32_t nof_cb);
+
 SRSLTE_API void srslte_softbuffer_tx_free(srslte_softbuffer_tx_t *p);
 
 
diff --git a/srslte/lib/cuhd/src/cuhd_imp.cpp b/srslte/lib/cuhd/src/cuhd_imp.cpp
index c8f3743ca..8a4ccbb9f 100644
--- a/srslte/lib/cuhd/src/cuhd_imp.cpp
+++ b/srslte/lib/cuhd/src/cuhd_imp.cpp
@@ -123,7 +123,7 @@ double cuhd_set_rx_gain_th(void *h, double gain)
 {
   cuhd_handler *handler = static_cast < cuhd_handler * >(h);
   gain = handler->rx_gain_range.clip(gain);     
-  if (gain != handler->new_rx_gain) {
+  if (gain > handler->new_rx_gain + 0.5 || gain < handler->new_rx_gain - 0.5) {
     pthread_mutex_lock(&handler->mutex);
     handler->new_rx_gain = gain; 
     pthread_cond_signal(&handler->cond);
@@ -151,7 +151,6 @@ static void* thread_gain_fcn(void *h) {
     if (handler->tx_gain_same_rx) {
       cuhd_set_tx_gain(h, handler->cur_rx_gain+handler->tx_rx_gain_offset);
     }
-    //printf("Set gain %.2f\n", handler->cur_rx_gain);
   }
 }
 
diff --git a/srslte/lib/fec/src/softbuffer.c b/srslte/lib/fec/src/softbuffer.c
index 32a654341..0c3057405 100644
--- a/srslte/lib/fec/src/softbuffer.c
+++ b/srslte/lib/fec/src/softbuffer.c
@@ -92,9 +92,21 @@ void srslte_softbuffer_rx_free(srslte_softbuffer_rx_t *q) {
   }
 }
 
+void srslte_softbuffer_rx_reset_tbs(srslte_softbuffer_rx_t *q, uint32_t tbs) {
+  uint32_t nof_cb = (tbs + 24)/(SRSLTE_TCOD_MAX_LEN_CB - 24) + 1; 
+  srslte_softbuffer_rx_reset_cb(q, nof_cb);
+}
+
 void srslte_softbuffer_rx_reset(srslte_softbuffer_rx_t *q) {
+  srslte_softbuffer_rx_reset_cb(q, q->max_cb);
+}
+
+void srslte_softbuffer_rx_reset_cb(srslte_softbuffer_rx_t *q, uint32_t nof_cb) {
   if (q->buffer_f) {
-    for (uint32_t i=0;i<q->max_cb;i++) {
+    if (nof_cb > q->max_cb) {
+      nof_cb = q->max_cb; 
+    }
+    for (uint32_t i=0;i<nof_cb;i++) {
       if (q->buffer_f[i]) {
         for (uint32_t j=0;j<q->buff_size;j++) {
           q->buffer_f[i][j] = SRSLTE_RX_NULL;
@@ -154,15 +166,25 @@ void srslte_softbuffer_tx_free(srslte_softbuffer_tx_t *q) {
   }
 }
 
+
+void srslte_softbuffer_tx_reset_tbs(srslte_softbuffer_tx_t *q, uint32_t tbs) {
+  uint32_t nof_cb = (tbs + 24)/(SRSLTE_TCOD_MAX_LEN_CB - 24) + 1; 
+  srslte_softbuffer_tx_reset_cb(q, nof_cb);
+}
+
 void srslte_softbuffer_tx_reset(srslte_softbuffer_tx_t *q) {
+  srslte_softbuffer_tx_reset_cb(q, q->max_cb);
+}
+
+void srslte_softbuffer_tx_reset_cb(srslte_softbuffer_tx_t *q, uint32_t nof_cb) {
   int i; 
   if (q->buffer_b) {
-    for (i=0;i<q->max_cb;i++) {
+    if (nof_cb > q->max_cb) {
+      nof_cb = q->max_cb; 
+    }
+    for (i=0;i<nof_cb;i++) {
       if (q->buffer_b[i]) {
         bzero(q->buffer_b[i], sizeof(uint8_t) * q->buff_size);
-        /*for (uint32_t j=0;j<q->buff_size;j++) {
-          q->buffer_b[i][j] = SRSLTE_TX_NULL;
-        }*/
       }
     }
   }