diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index bf4414f08..681b75566 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -326,7 +326,8 @@ srslte_netsink_t net_sink, net_sink_signal;
 #define PRINT_LINE_ADVANCE_CURSOR() printf("\033[%dB", prev_nof_lines + 1)
 
 int main(int argc, char **argv) {
-  int ret; 
+  struct timeval t[3];
+  int ret;
   int decimate = 1;
   srslte_cell_t cell;  
   int64_t sf_cnt;
@@ -559,7 +560,7 @@ int main(int argc, char **argv) {
   // Variables for measurements 
   uint32_t nframes=0;
   uint8_t ri = 0, pmi = 0;
-  float rsrp0=0.0, rsrp1=0.0, rsrq=0.0, noise=0.0, enodebrate = 0.0, uerate = 0.0,
+  float rsrp0=0.0, rsrp1=0.0, rsrq=0.0, noise=0.0, enodebrate = 0.0, uerate = 0.0, procrate = 0.0,
       sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS], cn = 0.0;
   bool decode_pdsch = false; 
 
@@ -655,6 +656,7 @@ int main(int argc, char **argv) {
               decode_pdsch = false; 
             }
           }
+          gettimeofday(&t[1], NULL);
           if (decode_pdsch) {
             if(sfidx != 1 || prog_args.mbsfn_area_id < 0){ // Not an MBSFN subframe
               if (cell.nof_ports == 1) {
@@ -690,6 +692,8 @@ int main(int argc, char **argv) {
                 INFO("mbsfn PDU size is %d\n", n);
               }
             }
+            gettimeofday(&t[2], NULL);
+            get_time_interval(t);
             if (n < 0) {
              // fprintf(stderr, "Error decoding UE DL\n");fflush(stdout);
             } else if (n > 0) {
@@ -723,16 +727,19 @@ int main(int argc, char **argv) {
 
             } 
                                     
-            nof_trials++; 
-            
- 
+            nof_trials++;
+
+            uint32_t nof_bits = ((acks[0]?ue_dl.pdsch_cfg.grant.mcs[0].tbs:0) + (acks[1]?ue_dl.pdsch_cfg.grant.mcs[1].tbs:0));
             rsrq = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrq(&ue_dl.chest), rsrq, 0.1f);
             rsrp0 = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp_port(&ue_dl.chest, 0), rsrp0, 0.05f);
             rsrp1 = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp_port(&ue_dl.chest, 1), rsrp1, 0.05f);
             noise = SRSLTE_VEC_EMA(srslte_chest_dl_get_noise_estimate(&ue_dl.chest), noise, 0.05f);
-            enodebrate = SRSLTE_VEC_EMA((ue_dl.pdsch_cfg.grant.mcs[0].tbs + ue_dl.pdsch_cfg.grant.mcs[1].tbs)/1000.0f, enodebrate, 0.05f);
-            uerate = SRSLTE_VEC_EMA(((acks[0]?ue_dl.pdsch_cfg.grant.mcs[0].tbs:0) + (acks[1]?ue_dl.pdsch_cfg.grant.mcs[1].tbs:0))/1000.0f, uerate, 0.01f);
-
+            enodebrate = SRSLTE_VEC_EMA(nof_bits/1000.0f, enodebrate, 0.05f);
+            uerate = SRSLTE_VEC_EMA(nof_bits/1000.0f, uerate, 0.001f);
+            float elapsed = (float) t[0].tv_usec + t[0].tv_sec*1.0e+6f;
+            if (elapsed != 0.0f) {
+              procrate = SRSLTE_VEC_EMA(nof_bits/elapsed, procrate, 0.01f);
+            }
    
             nframes++;
             if (isnan(rsrq)) {
@@ -769,7 +776,7 @@ int main(int argc, char **argv) {
             PRINT_LINE("nof codewords: %d", SRSLTE_RA_DL_GRANT_NOF_TB(&ue_dl.pdsch_cfg.grant));
             PRINT_LINE("          CFO: %+5.2f kHz", srslte_ue_sync_get_cfo(&ue_sync) / 1000);
             PRINT_LINE("          SNR: %+5.1f dB | %+5.1f dB", 10 * log10(rsrp0 / noise), 10 * log10(rsrp1 / noise));
-            PRINT_LINE("           Rb: %6.2f / %6.2f Mbps (net/maximum)", uerate, enodebrate);
+            PRINT_LINE("           Rb: %6.2f / %6.2f / %6.2f Mbps (net/maximum/processing)", uerate, enodebrate, procrate);
             PRINT_LINE("   PDCCH-Miss: %5.2f%%", 100 * (1 - (float) ue_dl.nof_detected / nof_trials));
             PRINT_LINE("   PDSCH-BLER: %5.2f%%", (float) 100 * ue_dl.pdsch_pkt_errors / ue_dl.pdsch_pkts_total);
             if(prog_args.mbsfn_area_id > -1){
diff --git a/lib/include/srslte/common/buffer_pool.h b/lib/include/srslte/common/buffer_pool.h
index 1c6dfc53a..567203c75 100644
--- a/lib/include/srslte/common/buffer_pool.h
+++ b/lib/include/srslte/common/buffer_pool.h
@@ -123,8 +123,6 @@ public:
       used.erase(elem); 
       available.push(b);
       ret = true; 
-    } else {
-      printf("Error deallocating from buffer pool: buffer not created in this pool.\n");
     }
     pthread_mutex_unlock(&mutex);
     return ret; 
diff --git a/lib/include/srslte/common/common.h b/lib/include/srslte/common/common.h
index 6372af73e..c450fc1d0 100644
--- a/lib/include/srslte/common/common.h
+++ b/lib/include/srslte/common/common.h
@@ -46,6 +46,7 @@
 
 #define HARQ_DELAY_MS   4
 #define MSG3_DELAY_MS   2 // Delay added to HARQ_DELAY_MS
+#define TTI_RX(tti)     (tti>HARQ_DELAY_MS?((tti-HARQ_DELAY_MS)%10240):(10240+tti-HARQ_DELAY_MS))
 #define TTI_TX(tti)     ((tti+HARQ_DELAY_MS)%10240)
 #define TTI_RX_ACK(tti) ((tti+(2*HARQ_DELAY_MS))%10240)
 
diff --git a/lib/include/srslte/phy/phch/cqi.h b/lib/include/srslte/phy/phch/cqi.h
index cfc9e92e7..0d0c17fec 100644
--- a/lib/include/srslte/phy/phch/cqi.h
+++ b/lib/include/srslte/phy/phch/cqi.h
@@ -55,13 +55,22 @@ typedef struct {
 } srslte_cqi_periodic_cfg_t; 
   
 /* Table 5.2.2.6.2-1: Fields for channel quality information feedback for higher layer configured subband
-CQI reports
-(transmission mode 1, transmission mode 2, transmission mode 3, transmission mode 7 and
-transmission mode 8 configured without PMI/RI reporting). */
+   CQI reports  (transmission mode 1, transmission mode 2, transmission mode 3, transmission mode 7 and
+   transmission mode 8 configured without PMI/RI reporting). */
+
+/* Table 5.2.2.6.2-2: Fields for channel quality information (CQI) feedback for higher layer configured subband CQI
+   reports (transmission mode 4, transmission mode 5 and transmission mode 6). */
+
 typedef struct SRSLTE_API {
-  uint8_t  wideband_cqi; // 4-bit width
-  uint32_t subband_diff_cqi; // 2N-bit width
-  uint32_t N; 
+  uint8_t  wideband_cqi_cw0;      // 4-bit width
+  uint32_t subband_diff_cqi_cw0;  // 2N-bit width
+  uint8_t  wideband_cqi_cw1;      // if RI > 1 then 4-bit width otherwise 0-bit width
+  uint32_t subband_diff_cqi_cw1;  // if RI > 1 then 2N-bit width otherwise 0-bit width
+  uint32_t pmi;                   // if RI > 1 then 2-bit width otherwise 1-bit width
+  uint32_t N;
+  bool pmi_present;
+  bool four_antenna_ports;        // If cell has 4 antenna ports then true otherwise false
+  bool rank_is_not_one;           // If rank > 1 then true otherwise false
 } srslte_cqi_hl_subband_t;
 
 /* Table 5.2.2.6.3-1: Fields for channel quality information feedback for UE selected subband CQI
diff --git a/lib/src/phy/phch/cqi.c b/lib/src/phy/phch/cqi.c
index 0041c3fcb..2c0940af5 100644
--- a/lib/src/phy/phch/cqi.c
+++ b/lib/src/phy/phch/cqi.c
@@ -44,11 +44,38 @@
  *******************************************************/
 int srslte_cqi_hl_subband_pack(srslte_cqi_hl_subband_t *msg, uint8_t buff[SRSLTE_CQI_MAX_BITS]) 
 {
-  uint8_t *body_ptr = buff; 
-  srslte_bit_unpack(msg->wideband_cqi, &body_ptr, 4);
-  srslte_bit_unpack(msg->subband_diff_cqi, &body_ptr, 2*msg->N);
-  
-  return 4+2*msg->N;
+  uint8_t *body_ptr = buff;
+  uint32_t bit_count = 0;
+
+  /* Unpack codeword 0, common for 3GPP 36.212 Tables 5.2.2.6.2-1 and 5.2.2.6.2-2 */
+  srslte_bit_unpack(msg->wideband_cqi_cw0, &body_ptr, 4);
+  srslte_bit_unpack(msg->subband_diff_cqi_cw0, &body_ptr, 2*msg->N);
+  bit_count += 4+2*msg->N;
+
+  /* Unpack codeword 1, 3GPP 36.212 Table 5.2.2.6.2-2 */
+  if (msg->rank_is_not_one) {
+    srslte_bit_unpack(msg->wideband_cqi_cw1, &body_ptr, 4);
+    srslte_bit_unpack(msg->subband_diff_cqi_cw1, &body_ptr, 2*msg->N);
+    bit_count += 4+2*msg->N;
+  }
+
+  /* If PMI is present, unpack it */
+  if (msg->pmi_present) {
+    if (msg->four_antenna_ports) {
+      srslte_bit_unpack(msg->pmi, &body_ptr, 4);
+      bit_count += 4;
+    } else {
+      if (msg->rank_is_not_one) {
+        srslte_bit_unpack(msg->pmi, &body_ptr, 1);
+        bit_count += 1;
+      } else {
+        srslte_bit_unpack(msg->pmi, &body_ptr, 2);
+        bit_count += 2;
+      }
+    }
+  }
+
+  return bit_count;
 }
 
 int srslte_cqi_ue_subband_pack(srslte_cqi_ue_subband_t *msg, uint8_t buff[SRSLTE_CQI_MAX_BITS])
@@ -98,11 +125,37 @@ int srslte_cqi_value_pack(srslte_cqi_value_t *value, uint8_t buff[SRSLTE_CQI_MAX
 
 int srslte_cqi_hl_subband_unpack(uint8_t buff[SRSLTE_CQI_MAX_BITS], srslte_cqi_hl_subband_t *msg) 
 {
-  uint8_t *body_ptr     = buff; 
-  msg->wideband_cqi     = srslte_bit_pack(&body_ptr, 4);
-  msg->subband_diff_cqi = srslte_bit_pack(&body_ptr, 2*msg->N);
-  
-  return 4+2*msg->N;
+  uint8_t *body_ptr     = buff;
+  uint32_t bit_count = 0;
+
+  msg->wideband_cqi_cw0     = (uint8_t) srslte_bit_pack(&body_ptr, 4);
+  msg->subband_diff_cqi_cw0 = srslte_bit_pack(&body_ptr, 2*msg->N);
+  bit_count += 4+2*msg->N;
+
+  /* Unpack codeword 1, 3GPP 36.212 Table 5.2.2.6.2-2 */
+  if (msg->rank_is_not_one) {
+    msg->wideband_cqi_cw1     = (uint8_t) srslte_bit_pack(&body_ptr, 4);
+    msg->subband_diff_cqi_cw1 = srslte_bit_pack(&body_ptr, 2*msg->N);
+    bit_count += 4+2*msg->N;
+  }
+
+  /* If PMI is present, unpack it */
+  if (msg->pmi_present) {
+    if (msg->four_antenna_ports) {
+      msg->pmi = (uint8_t) srslte_bit_pack(&body_ptr, 4);
+      bit_count += 4;
+    } else {
+      if (msg->rank_is_not_one) {
+        msg->pmi = (uint8_t) srslte_bit_pack(&body_ptr, 1);
+        bit_count += 1;
+      } else {
+        msg->pmi = (uint8_t) srslte_bit_pack(&body_ptr, 2);
+        bit_count += 2;
+      }
+    }
+  }
+
+  return bit_count;
 }
 
 int srslte_cqi_ue_subband_unpack(uint8_t buff[SRSLTE_CQI_MAX_BITS], srslte_cqi_ue_subband_t *msg)
diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index 29aec5b44..729d96e11 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -291,6 +291,13 @@ int rf_uhd_open(char *args, void **h)
   return rf_uhd_open_multi(args, h, 1);
 }
 
+#define REMOVE_SUBSTRING_WITHCOMAS(S, TOREMOVE) \
+  remove_substring(S, TOREMOVE ",");\
+  remove_substring(S, TOREMOVE ", ");\
+  remove_substring(S, "," TOREMOVE);\
+  remove_substring(S, ", " TOREMOVE);\
+  remove_substring(S, TOREMOVE)
+
 static void remove_substring(char *s,const char *toremove)
 {
   while((s=strstr(s,toremove))) {
@@ -298,6 +305,17 @@ static void remove_substring(char *s,const char *toremove)
   }
 }
 
+static void copy_subdev_string(char *dst, char *src) {
+  int n = 0;
+  size_t len = strlen(src);
+  /* Copy until end of string or comma */
+  while (n < len && src != '\0' && src[n] != ',') {
+    dst[n] = src[n];
+    n++;
+  }
+  dst[n] = '\0';
+}
+
 int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
 {
   if (h) {
@@ -336,11 +354,11 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
     // Check external clock argument
     enum {DEFAULT, EXTERNAL, GPSDO} clock_src;
     if (strstr(args, "clock=external")) {
-      remove_substring(args, "clock=external");
+      REMOVE_SUBSTRING_WITHCOMAS(args, "clock=external");
       clock_src = EXTERNAL;
     } else if (strstr(args, "clock=gpsdo")) {
       printf("Using GPSDO clock\n");
-      remove_substring(args, "clock=gpsdo");
+      REMOVE_SUBSTRING_WITHCOMAS(args, "clock=gpsdo");
       clock_src = GPSDO;
     } else {
       clock_src = DEFAULT;
@@ -349,14 +367,36 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
     // Set over the wire format
     char *otw_format = "sc16";
     if (strstr(args, "otw_format=sc12")) {
+      REMOVE_SUBSTRING_WITHCOMAS(args, "otw_format=sc12");
       otw_format = "sc12";
     } else if (strstr(args, "otw_format=sc16")) {
+      REMOVE_SUBSTRING_WITHCOMAS(args, "otw_format=sc16");
       /* Do nothing */
     } else if (strstr(args, "otw_format=")) {
       fprintf(stderr, "Wrong over the wire format. Valid formats: sc12, sc16\n");
       return -1;
     }
 
+    // Set transmitter subdevice spec string
+    const char tx_subdev_arg[] = "tx_subdev_spec=";
+    char tx_subdev_str[64] = {0};
+    char *tx_subdev_ptr = strstr(args, tx_subdev_arg);
+    if (tx_subdev_ptr) {
+      copy_subdev_string(tx_subdev_str, tx_subdev_ptr + strlen(tx_subdev_arg));
+      remove_substring(args, tx_subdev_arg);
+      remove_substring(args, tx_subdev_str);
+    }
+
+    // Set receiver subdevice spec string
+    const char rx_subdev_arg[] = "rx_subdev_spec=";
+    char rx_subdev_str[64] = {0};
+    char *rx_subdev_ptr = strstr(args, rx_subdev_arg);
+    if (rx_subdev_ptr) {
+      copy_subdev_string(rx_subdev_str, rx_subdev_ptr + strlen(rx_subdev_arg));
+      remove_substring(args, rx_subdev_arg);
+      remove_substring(args, rx_subdev_str);
+    }
+
     /* If device type or name not given in args, choose a B200 */
     if (args[0]=='\0') {
       if (find_string(devices_str, "type=b200") && !strstr(args, "recv_frame_size")) {
@@ -396,7 +436,29 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
       fprintf(stderr, "Error opening UHD: code %d\n", error);
       return -1; 
     }
-    
+
+    /* Set transmitter subdev spec if specified */
+    if (strlen(tx_subdev_str)) {
+      uhd_subdev_spec_handle subdev_spec_handle = {0};
+
+      printf("Setting tx_subdev_spec to '%s'\n", tx_subdev_str);
+
+      uhd_subdev_spec_make(&subdev_spec_handle, tx_subdev_str);
+      uhd_usrp_set_tx_subdev_spec(handler->usrp, subdev_spec_handle, 0);
+      uhd_subdev_spec_free(&subdev_spec_handle);
+    }
+
+    /* Set receiver subdev spec if specified */
+    if (strlen(rx_subdev_str)) {
+      uhd_subdev_spec_handle subdev_spec_handle = {0};
+
+      printf("Setting rx_subdev_spec to '%s'\n", rx_subdev_str);
+
+      uhd_subdev_spec_make(&subdev_spec_handle, rx_subdev_str);
+      uhd_usrp_set_rx_subdev_spec(handler->usrp, subdev_spec_handle, 0);
+      uhd_subdev_spec_free(&subdev_spec_handle);
+    }
+
     if (!handler->devname) {
       char dev_str[1024];
       uhd_usrp_get_mboard_name(handler->usrp, 0, dev_str, 1024);
diff --git a/lib/src/upper/rlc_am.cc b/lib/src/upper/rlc_am.cc
index 05f35ad76..f8abc0aef 100644
--- a/lib/src/upper/rlc_am.cc
+++ b/lib/src/upper/rlc_am.cc
@@ -201,8 +201,14 @@ uint32_t rlc_am::get_total_buffer_state()
     rlc_amd_retx_t retx = retx_queue.front();
     log->debug("Buffer state - retx - SN: %d, Segment: %s, %d:%d\n", retx.sn, retx.is_segment ? "true" : "false", retx.so_start, retx.so_end);
     if(tx_window.end() != tx_window.find(retx.sn)) {
-        n_bytes += required_buffer_size(retx);
+      int req_bytes = required_buffer_size(retx);
+      if (req_bytes < 0) {
+        log->error("In get_total_buffer_state(): Removing retx.sn=%d from queue\n", retx.sn);
+        retx_queue.pop_front();
+      } else {
+        n_bytes += req_bytes;
         log->debug("Buffer state - retx: %d bytes\n", n_bytes);
+      }
     }
   }
 
@@ -250,7 +256,13 @@ uint32_t rlc_am::get_buffer_state()
     rlc_amd_retx_t retx = retx_queue.front();
     log->debug("Buffer state - retx - SN: %d, Segment: %s, %d:%d\n", retx.sn, retx.is_segment ? "true" : "false", retx.so_start, retx.so_end);
     if(tx_window.end() != tx_window.find(retx.sn)) {
-      n_bytes = required_buffer_size(retx);
+      int req_bytes = required_buffer_size(retx);
+      if (req_bytes < 0) {
+        log->error("In get_buffer_state(): Removing retx.sn=%d from queue\n", retx.sn);
+        retx_queue.pop_front();
+        goto unlock_and_return;
+      }
+      n_bytes = (uint32_t) req_bytes;
       log->debug("Buffer state - retx: %d bytes\n", n_bytes);
       goto unlock_and_return;
     }
@@ -296,8 +308,9 @@ 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 build_retx_pdu(payload, nof_bytes);
+    return ret; 
   }
 
   // Build a PDU from SDUs
@@ -438,16 +451,33 @@ int  rlc_am::build_status_pdu(uint8_t *payload, uint32_t nof_bytes)
 
 int  rlc_am::build_retx_pdu(uint8_t *payload, uint32_t nof_bytes)
 {
+  // Check there is at least 1 element before calling front()
+  if (retx_queue.empty()) {
+    log->error("In build_retx_pdu(): retx_queue is empty\n");
+    return -1;
+  }
+
   rlc_amd_retx_t retx = retx_queue.front();
 
   // Sanity check - drop any retx SNs not present in tx_window
   while(tx_window.end() == tx_window.find(retx.sn)) {
     retx_queue.pop_front();
-    retx = retx_queue.front();
+    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;
+    }
   }
 
   // Is resegmentation needed?
-  if(retx.is_segment || required_buffer_size(retx) > (int)nof_bytes) {
+  int req_size = required_buffer_size(retx);
+  if (req_size < 0) {
+    log->error("In build_retx_pdu(): Removing retx.sn=%d from queue\n", retx.sn);
+    retx_queue.pop_front();
+    return -1;
+  }
+  if(retx.is_segment || req_size > (int)nof_bytes) {
     log->debug("%s build_retx_pdu - resegmentation required\n", rrc->get_rb_name(lcid).c_str());
     return build_segment(payload, nof_bytes, retx);
   }
@@ -481,6 +511,10 @@ int  rlc_am::build_retx_pdu(uint8_t *payload, uint32_t nof_bytes)
 
 int rlc_am::build_segment(uint8_t *payload, uint32_t nof_bytes, rlc_amd_retx_t retx)
 {
+  if (!tx_window[retx.sn].buf) {
+    log->error("In build_segment: retx.sn=%d has null buffer\n", retx.sn);
+    return 0;
+  }
   if(!retx.is_segment){
     retx.so_start = 0;
     retx.so_end   = tx_window[retx.sn].buf->N_bytes;
@@ -1147,7 +1181,17 @@ bool rlc_am::add_segment_and_check(rlc_amd_rx_pdu_segments_t *pdu, rlc_amd_rx_pd
 int rlc_am::required_buffer_size(rlc_amd_retx_t retx)
 {
   if(!retx.is_segment){
-    return rlc_am_packed_length(&tx_window[retx.sn].header) + tx_window[retx.sn].buf->N_bytes;
+    if (tx_window.count(retx.sn)) {
+      if (tx_window[retx.sn].buf) {
+        return rlc_am_packed_length(&tx_window[retx.sn].header) + tx_window[retx.sn].buf->N_bytes;
+      } else {
+        log->warning("retx.sn=%d has null ptr in required_buffer_size()\n", retx.sn);
+        return -1;
+      }
+    } else {
+      log->warning("retx.sn=%d does not exist in required_buffer_size()\n", retx.sn);
+      return -1;
+    }
   }
 
   // Construct new header
diff --git a/srsenb/src/phy/phch_worker.cc b/srsenb/src/phy/phch_worker.cc
index abd7961ce..8ed6428ef 100644
--- a/srsenb/src/phy/phch_worker.cc
+++ b/srsenb/src/phy/phch_worker.cc
@@ -498,7 +498,7 @@ int phch_worker::decode_pusch(srslte_enb_ul_pusch_t *grants, uint32_t nof_pusch)
         if (ue_db[rnti].cqi_en) {
           wideband_cqi_value = cqi_value.wideband.wideband_cqi;
         } else if (grants[i].grant.cqi_request) {
-          wideband_cqi_value = cqi_value.subband_hl.wideband_cqi;
+          wideband_cqi_value = cqi_value.subband_hl.wideband_cqi_cw0;
         }
         snprintf(cqi_str, 64, ", cqi=%d", wideband_cqi_value);
       }
diff --git a/srsue/hdr/mac/ul_harq.h b/srsue/hdr/mac/ul_harq.h
index 4749971b6..50143a571 100644
--- a/srsue/hdr/mac/ul_harq.h
+++ b/srsue/hdr/mac/ul_harq.h
@@ -219,7 +219,7 @@ private:
 
       // Receive and route HARQ feedbacks
       if (grant) {
-        if ((!(grant->rnti_type == SRSLTE_RNTI_TEMP) && grant->ndi[0] != get_ndi()) ||
+        if ((!(grant->rnti_type == SRSLTE_RNTI_TEMP) && grant->ndi[0] != get_ndi() && grant->phy_grant.ul.mcs.idx < 29) ||
           (grant->rnti_type == SRSLTE_RNTI_USER && !has_grant())             ||
           grant->is_from_rar)
         {
@@ -245,7 +245,7 @@ private:
               Warning("Uplink grant but no MAC PDU in Multiplex Unit buffer\n");
             }
           }
-        } else {
+        } else if (has_grant()) {
           // Adaptive Re-TX
           if (current_tx_nb >= max_retx) {
             Info("UL %d:  Maximum number of ReTX reached (%d). Discarting TB.\n", pid, max_retx);
@@ -254,6 +254,8 @@ private:
           } else {
             generate_retx(tti_tx, grant, action);
           }
+        } else {
+          Warning("UL %d: Received mcs=%d but no previous grant available for this PID.\n", pid, grant->phy_grant.ul.mcs.idx);
         }
       } else if (has_grant()) {
         // Non-Adaptive Re-Tx
diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index 3e2fb70dd..0e123e988 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -127,6 +127,8 @@ private:
 
   // RRC constants and timers
   srslte::mac_interface_timers *mac_timers;
+  uint32_t sync_reset_cnt;
+  const static uint32_t SYNC_RESET_TIMEOUT = 10;
   uint32_t n310_cnt, N310;
   uint32_t n311_cnt, N311;
   uint32_t t301, t310, t311;
diff --git a/srsue/src/mac/mux.cc b/srsue/src/mac/mux.cc
index e38300b87..957c257a1 100644
--- a/srsue/src/mac/mux.cc
+++ b/srsue/src/mac/mux.cc
@@ -286,7 +286,7 @@ bool mux::sched_sdu(lchid_t *ch, int *sdu_space, int max_sdu_sz)
       }        
 
       log_h->info("SDU:   scheduled lcid=%d, rlc_buffer=%d, allocated=%d/%d\n", 
-                   ch->id, ch->buffer_len, sched_len, *sdu_space);
+                   ch->id, ch->buffer_len, sched_len, sdu_space?*sdu_space:0);
       
       *sdu_space     -= sched_len; 
       ch->buffer_len -= sched_len; 
@@ -309,12 +309,11 @@ bool mux::allocate_sdu(uint32_t lcid, srslte::sch_pdu* pdu_msg, int max_sdu_sz)
       sdu_len = max_sdu_sz;
     }
     int sdu_space = pdu_msg->get_sdu_space();
-    if (sdu_len > sdu_space) {
+    if (sdu_len > sdu_space || max_sdu_sz < 0) {
       sdu_len = sdu_space;
-    }        
+    }
     if (sdu_len > MIN_RLC_SDU_LEN) {
       if (pdu_msg->new_subh()) { // there is space for a new subheader
-        int sdu_len2 = sdu_len; 
         sdu_len = pdu_msg->get()->set_sdu(lcid, sdu_len, rlc);
         if (sdu_len > 0) { // new SDU could be added
           
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 548d5167a..088c4a22c 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -27,6 +27,7 @@
 #include <unistd.h>
 #include <string.h>
 #include "phy/phch_worker.h"
+#include "srslte/srslte.h"
 #include "srslte/interfaces/ue_interfaces.h"
 #include "srslte/asn1/liblte_rrc.h"
 
@@ -40,8 +41,6 @@
 #ifdef ENABLE_GUI
 #include "srsgui/srsgui.h"
 #include <semaphore.h>
-#include "srslte/srslte.h"
-#include "srslte/interfaces/ue_interfaces.h"
 
 void init_plots(srsue::phch_worker *worker);
 pthread_t plot_thread; 
@@ -290,9 +289,9 @@ void phch_worker::work_imp()
   ul_action.tti_offset = HARQ_DELAY_MS;
 
   /* Send UL grant or HARQ information (from PHICH) to MAC */
-  if (ul_grant_available         && ul_ack_available)  {    
+  if (ul_grant_available         && ul_ack_available && ul_mac_grant.phy_grant.ul.mcs.idx < 29)  {
     phy->mac->new_grant_ul_ack(ul_mac_grant, ul_ack, &ul_action);      
-  } else if (ul_grant_available  && !ul_ack_available) {
+  } else if (ul_grant_available  && (!ul_ack_available || ul_mac_grant.phy_grant.ul.mcs.idx < 29)) {
     phy->mac->new_grant_ul(ul_mac_grant, &ul_action);
   } else if (!ul_grant_available && ul_ack_available)  {    
     phy->mac->harq_recv(tti, ul_ack, &ul_action);        
@@ -485,11 +484,15 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
 
     grant->pid = ASYNC_DL_SCHED?dci_unpacked.harq_process:(tti%(2*HARQ_DELAY_MS));
 
-    // Set last TBS for this TB (pid) in case of mcs>29 (7.1.7.2 of 36.213)
+    // Set last TBS for this TB (pid) in case of mcs>28 (7.1.7.2 of 36.213)
     for (int i=0;i<SRSLTE_MAX_CODEWORDS;i++) {
-      if (grant->phy_grant.dl.mcs[i].tbs < 0) {
+      if (grant->phy_grant.dl.mcs[i].idx > 28) {
         grant->phy_grant.dl.mcs[i].tbs = last_dl_tbs[grant->pid%(2*HARQ_DELAY_MS)][i];
       }
+      if(grant->phy_grant.dl.mcs[i].tbs < 0) {
+        Info("Invalid TBS size for PDSCH grant\n");
+        grant->phy_grant.dl.mcs[i].tbs = 0;
+      }
       // save it
       last_dl_tbs[grant->pid%(2*HARQ_DELAY_MS)][i] = grant->phy_grant.dl.mcs[i].tbs;
     }
@@ -544,10 +547,15 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
     }
   }
 
+  uint32_t nof_tb = SRSLTE_RA_DL_GRANT_NOF_TB(grant);
   switch(phy->config->dedicated.antenna_info_explicit_value.tx_mode) {
     /* Implemented Tx Modes */
     case LIBLTE_RRC_TRANSMISSION_MODE_1:
       mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+      if (nof_tb != 1) {
+        Error("Wrong number of transport blocks (%d) for single antenna.", nof_tb);
+        valid_config = false;
+      }
       break;
     case LIBLTE_RRC_TRANSMISSION_MODE_2:
       if (cell.nof_ports > 1) {
@@ -555,26 +563,30 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
       } else {
         mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
       }
+      if (nof_tb != 1) {
+        Error("Wrong number of transport blocks (%d) for transmit diversity.", nof_tb);
+        valid_config = false; 
+      }
       break;
     case LIBLTE_RRC_TRANSMISSION_MODE_3:
-      if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 1) {
+      if (nof_tb == 1) {
         mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
-      } else if (ue_dl.nof_rx_antennas > 1 && SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
+      } else if (ue_dl.nof_rx_antennas > 1 && nof_tb == 2) {
         mimo_type = SRSLTE_MIMO_TYPE_CDD;
       } else {
         Error("Wrong combination of antennas (%d) or transport blocks (%d) for TM3\n", ue_dl.nof_rx_antennas,
-              SRSLTE_RA_DL_GRANT_NOF_TB(grant));
+              nof_tb);
         valid_config = false;
       }
       break;
     case LIBLTE_RRC_TRANSMISSION_MODE_4:
-      if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 1) {
+      if (nof_tb == 1) {
         mimo_type = (grant->pinfo == 0) ? SRSLTE_MIMO_TYPE_TX_DIVERSITY : SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
-      } else if (ue_dl.nof_rx_antennas > 1 && SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
+      } else if (ue_dl.nof_rx_antennas > 1 && nof_tb == 2) {
         mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
       } else {
         Error("Wrong combination of antennas (%d) or transport blocks (%d) for TM3\n", ue_dl.nof_rx_antennas,
-              SRSLTE_RA_DL_GRANT_NOF_TB(grant));
+              nof_tb);
         valid_config = false;
       }
     break;
@@ -745,16 +757,17 @@ bool phch_worker::decode_pdcch_ul(mac_interface_phy::mac_grant_t* grant)
   if (ret) {
 
     // Use last TBS for this TB in case of mcs>28
-    if (grant->phy_grant.ul.mcs.tbs < 0) {
-      grant->phy_grant.ul.mcs.tbs = last_ul_tbs[tti%(2*HARQ_DELAY_MS)];
+    if (grant->phy_grant.ul.mcs.idx > 28) {
+      grant->phy_grant.ul.mcs.tbs = last_ul_tbs[TTI_RX(tti)%(2*HARQ_DELAY_MS)];
+      Info("RETX: mcs=%d, old_tbs=%d pid=%d\n", grant->phy_grant.ul.mcs.idx, grant->phy_grant.ul.mcs.tbs, TTI_TX(tti)%(2*HARQ_DELAY_MS));
     }
-    last_ul_tbs[tti%(2*HARQ_DELAY_MS)] = grant->phy_grant.ul.mcs.tbs;
+    last_ul_tbs[TTI_RX(tti)%(2*HARQ_DELAY_MS)] = grant->phy_grant.ul.mcs.tbs;
 
     if (grant->phy_grant.ul.mcs.mod == SRSLTE_MOD_LAST) {
-      grant->phy_grant.ul.mcs.mod = last_ul_mod[tti%(2*HARQ_DELAY_MS)];
+      grant->phy_grant.ul.mcs.mod = last_ul_mod[TTI_RX(tti)%(2*HARQ_DELAY_MS)];
       grant->phy_grant.ul.Qm      = srslte_mod_bits_x_symbol(grant->phy_grant.ul.mcs.mod);
     }
-    last_ul_mod[tti%(2*HARQ_DELAY_MS)] = grant->phy_grant.ul.mcs.mod;
+    last_ul_mod[TTI_RX(tti)%(2*HARQ_DELAY_MS)] = grant->phy_grant.ul.mcs.mod;
   }
 
   /* Limit UL modulation if not supported by the UE or disabled by higher layers */
@@ -883,18 +896,71 @@ void phch_worker::set_uci_aperiodic_cqi()
             reported RI. For other transmission modes they are reported conditioned on rank 1.
         */
         if (rnti_is_set) {
-          srslte_cqi_value_t cqi_report;
+          srslte_cqi_value_t cqi_report = {0};
           cqi_report.type = SRSLTE_CQI_TYPE_SUBBAND_HL;
-          cqi_report.subband_hl.wideband_cqi = srslte_cqi_from_snr(phy->avg_snr_db);
+          cqi_report.subband_hl.wideband_cqi_cw0 = srslte_cqi_from_snr(phy->avg_snr_db);
 
           // TODO: implement subband CQI properly
-          cqi_report.subband_hl.subband_diff_cqi = 0; // Always report zero offset on all subbands
+          cqi_report.subband_hl.subband_diff_cqi_cw0 = 0; // Always report zero offset on all subbands
           cqi_report.subband_hl.N = (cell.nof_prb > 7) ? srslte_cqi_hl_get_no_subbands(cell.nof_prb) : 0;
 
           Info("PUSCH: Aperiodic CQI=%d, SNR=%.1f dB, for %d subbands\n", cqi_report.wideband.wideband_cqi, phy->avg_snr_db, cqi_report.subband_hl.N);
           uci_data.uci_cqi_len = srslte_cqi_value_pack(&cqi_report, uci_data.uci_cqi);
         }
         break;
+      case LIBLTE_RRC_CQI_REPORT_MODE_APERIODIC_RM31:
+        /* only Higher Layer-configured subband feedback support right now, according to TS36.213 section 7.2.1
+          - A single precoding matrix is selected from the codebook subset assuming transmission on set S subbands
+          - A UE shall report one subband CQI value per codeword for each set S subband which are calculated assuming
+            the use of the single precoding matrix in all subbands and assuming transmission in the corresponding
+            subband.
+          - A UE shall report a wideband CQI value per codeword which is calculated assuming the use of the single
+            precoding matrix in all subbands and transmission on set S subbands
+          - The UE shall report the single selected precoding matrix indicator.
+          - For transmission mode 4 the reported PMI and CQI values are calculated conditioned on the reported RI. For
+            other transmission modes they are reported conditioned on rank 1.
+        */
+        if (rnti_is_set) {
+          /* Compute RI, PMI and SINR */
+          compute_ri();
+
+          /* Select RI, PMI and SINR */
+          uint32_t ri = ue_dl.ri;       // Select RI (0: 1 layer, 1: 2 layer, otherwise: not implemented)
+          uint32_t pmi = ue_dl.pmi[ri]; // Select PMI
+          float sinr_db = 10 * log10(ue_dl.sinr[ri][pmi]);
+
+          /* Fill CQI Report */
+          srslte_cqi_value_t cqi_report = {0};
+          cqi_report.type = SRSLTE_CQI_TYPE_SUBBAND_HL;
+
+          cqi_report.subband_hl.wideband_cqi_cw0 = srslte_cqi_from_snr(sinr_db);
+          cqi_report.subband_hl.subband_diff_cqi_cw0 = 0; // Always report zero offset on all subbands
+
+          if (ri > 0) {
+            cqi_report.subband_hl.rank_is_not_one = true;
+            cqi_report.subband_hl.wideband_cqi_cw1 = srslte_cqi_from_snr(sinr_db);
+            cqi_report.subband_hl.subband_diff_cqi_cw1 = 0; // Always report zero offset on all subbands
+          }
+
+          cqi_report.subband_hl.pmi = pmi;
+          cqi_report.subband_hl.pmi_present = true;
+          cqi_report.subband_hl.four_antenna_ports = (cell.nof_ports == 4);
+
+          // TODO: implement subband CQI properly
+          cqi_report.subband_hl.N = (uint32_t) ((cell.nof_prb > 7) ? srslte_cqi_hl_get_no_subbands(cell.nof_prb) : 0);
+
+          if (cqi_report.subband_hl.rank_is_not_one) {
+            Info("PUSCH: Aperiodic ri~1, CQI=%02d/%02d, SINR=%2.1f/%2.1fdB, pmi=%d for %d subbands\n",
+                 cqi_report.subband_hl.wideband_cqi_cw0, cqi_report.subband_hl.wideband_cqi_cw1,
+                 sinr_db, sinr_db, pmi, cqi_report.subband_hl.N);
+          } else {
+            Info("PUSCH: Aperiodic ri=1, CQI=%d/%d, SINR=%2.1f dB, for %d subbands\n",
+                 cqi_report.wideband.wideband_cqi,
+                 phy->avg_snr_db, cqi_report.subband_hl.N);
+          }
+          uci_data.uci_cqi_len = srslte_cqi_value_pack(&cqi_report, uci_data.uci_cqi);
+        }
+        break;
       default:
         Warning("Received CQI request but mode %s is not supported\n", 
                 liblte_rrc_cqi_report_mode_aperiodic_text[phy->config->dedicated.cqi_report_cnfg.report_mode_aperiodic]);
diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc
index fe5528d79..6dfb2c6d7 100644
--- a/srsue/src/phy/phy.cc
+++ b/srsue/src/phy/phy.cc
@@ -199,9 +199,10 @@ void phy::set_timeadv_rar(uint32_t ta_cmd) {
 }
 
 void phy::set_timeadv(uint32_t ta_cmd) {
-  n_ta = srslte_N_ta_new(n_ta, ta_cmd);
-  sf_recv.set_time_adv_sec(((float) n_ta)*SRSLTE_LTE_TS);
-  Info("PHY:   Set TA: ta_cmd: %d, n_ta: %d, ta_usec: %.1f\n", ta_cmd, n_ta, ((float) n_ta)*SRSLTE_LTE_TS*1e6);
+  uint32_t new_nta = srslte_N_ta_new(n_ta, ta_cmd);
+  sf_recv.set_time_adv_sec(((float) (new_nta - n_ta))*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;
 }
 
 void phy::configure_prach_params()
diff --git a/srsue/src/upper/nas.cc b/srsue/src/upper/nas.cc
index 110c8c78e..d8bd40c6e 100644
--- a/srsue/src/upper/nas.cc
+++ b/srsue/src/upper/nas.cc
@@ -480,6 +480,7 @@ void nas::parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu) {
       sec_mode_cmd.nas_ksi.tsc_flag != LIBLTE_MME_TYPE_OF_SECURITY_CONTEXT_FLAG_NATIVE) {
     sec_mode_rej.emm_cause = LIBLTE_MME_EMM_CAUSE_UE_SECURITY_CAPABILITIES_MISMATCH;
     nas_log->warning("Sending Security Mode Reject due to security capabilities mismatch\n");
+    nas_log->console("Unsupported Security Mode Command settings: use ciphering algorithm EEA0 and integrity check EIA1 or EIA2.\n");
     success = false;
   } else {
     // Generate NAS encryption key and integrity protection key
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 5806d200a..c85e7e588 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -48,6 +48,9 @@ rrc::rrc()
   :state(RRC_STATE_IDLE)
   ,drb_up(false)
 {
+  sync_reset_cnt = 0;
+  n310_cnt       = 0;
+  n311_cnt       = 0;
 }
 
 static void liblte_rrc_handler(void *ctx, char *str) {
@@ -489,13 +492,17 @@ void rrc::earfcn_end() {
 
 // Detection of physical layer problems (5.3.11.1)
 void rrc::out_of_sync() {
+  // attempt resync
+  sync_reset_cnt++;
+  if (sync_reset_cnt >= SYNC_RESET_TIMEOUT) {
+    rrc_log->info("Detected %d out-of-sync from PHY. Resynchronizing PHY.\n", sync_reset_cnt);
+    phy->sync_reset();
+    sync_reset_cnt = 0;
+  }
   current_cell->in_sync = false;
   if (!mac_timers->timer_get(t311)->is_running() && !mac_timers->timer_get(t310)->is_running()) {
     n310_cnt++;
     if (n310_cnt == N310) {
-      // attempt resync
-      //phy->sync_reset();
-
       mac_timers->timer_get(t310)->reset();
       mac_timers->timer_get(t310)->run();
       n310_cnt = 0;