diff --git a/srslte/include/srslte/ch_estimation/chest_ul.h b/srslte/include/srslte/ch_estimation/chest_ul.h
index 90a706cc7..8f21cb6b3 100644
--- a/srslte/include/srslte/ch_estimation/chest_ul.h
+++ b/srslte/include/srslte/ch_estimation/chest_ul.h
@@ -56,6 +56,7 @@ typedef struct {
   bool dmrs_signal_configured; 
   
   cf_t *pilot_estimates;
+  cf_t *pilot_estimates_tmp[4];
   cf_t *pilot_recv_signal; 
   cf_t *pilot_known_signal; 
   cf_t *tmp_noise; 
@@ -105,7 +106,8 @@ SRSLTE_API int srslte_chest_ul_estimate_pucch(srslte_chest_ul_t *q,
                                               cf_t *ce, 
                                               srslte_pucch_format_t format, 
                                               uint32_t n_pucch, 
-                                              uint32_t sf_idx); 
+                                              uint32_t sf_idx, 
+                                              uint8_t *pucch2_ack_bits); 
 
 SRSLTE_API float srslte_chest_ul_get_noise_estimate(srslte_chest_ul_t *q); 
 
diff --git a/srslte/include/srslte/mimo/precoding.h b/srslte/include/srslte/mimo/precoding.h
index bf842faeb..0002daebb 100644
--- a/srslte/include/srslte/mimo/precoding.h
+++ b/srslte/include/srslte/mimo/precoding.h
@@ -106,4 +106,14 @@ SRSLTE_API int srslte_predecoding_type(cf_t *y,
                                        srslte_mimo_type_t type, 
                                        float noise_estimate);
 
+SRSLTE_API int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], 
+                                             cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], 
+                                             cf_t *x[SRSLTE_MAX_LAYERS],
+                                             int nof_rxant,
+                                             int nof_ports, 
+                                             int nof_layers, 
+                                             int nof_symbols, 
+                                             srslte_mimo_type_t type, 
+                                             float noise_estimate);
+
 #endif /* PRECODING_H_ */
diff --git a/srslte/include/srslte/phch/pucch.h b/srslte/include/srslte/phch/pucch.h
index 2ed0743b4..a91274441 100644
--- a/srslte/include/srslte/phch/pucch.h
+++ b/srslte/include/srslte/phch/pucch.h
@@ -43,6 +43,7 @@
 #include "srslte/phch/uci.h"
 
 #define SRSLTE_PUCCH_N_SEQ       12 
+#define SRSLTE_PUCCH2_NOF_BITS   SRSLTE_UCI_CQI_CODED_PUCCH_B
 #define SRSLTE_PUCCH_MAX_BITS    SRSLTE_CQI_MAX_BITS
 #define SRSLTE_PUCCH_MAX_SYMBOLS 120
 
@@ -78,13 +79,20 @@ typedef struct SRSLTE_API {
   bool srs_simul_ack; 
 } srslte_pucch_cfg_t;
 
+typedef struct  {
+  srslte_sequence_t seq_f2[SRSLTE_NSUBFRAMES_X_FRAME];   
+} srslte_pucch_user_t; 
+
 /* PUCCH object */
 typedef struct SRSLTE_API {
   srslte_cell_t cell;
   srslte_pucch_cfg_t pucch_cfg;
-  srslte_sequence_t seq_f2[SRSLTE_NSUBFRAMES_X_FRAME]; 
   srslte_modem_table_t mod; 
   
+  srslte_uci_cqi_pucch_t cqi; 
+  
+  srslte_pucch_user_t **users;
+  
   uint8_t bits_scram[SRSLTE_PUCCH_MAX_BITS];
   cf_t d[SRSLTE_PUCCH_MAX_BITS/2];
   uint32_t n_cs_cell[SRSLTE_NSLOTS_X_FRAME][SRSLTE_CP_NORM_NSYMB]; 
@@ -95,7 +103,6 @@ typedef struct SRSLTE_API {
   cf_t *z_tmp;
   cf_t *ce;
   
-  bool rnti_is_set;
   bool shortened; 
   bool group_hopping_en;
 
@@ -124,6 +131,9 @@ SRSLTE_API void srslte_pucch_set_threshold(srslte_pucch_t *q,
 SRSLTE_API int srslte_pucch_set_crnti(srslte_pucch_t *q, 
                                       uint16_t c_rnti); 
 
+SRSLTE_API void srslte_pucch_clear_rnti(srslte_pucch_t *q, 
+                                        uint16_t rnti); 
+
 SRSLTE_API uint32_t srslte_pucch_nof_symbols(srslte_pucch_cfg_t *cfg, 
                                              srslte_pucch_format_t format, 
                                              bool shortened); 
@@ -134,6 +144,7 @@ SRSLTE_API int srslte_pucch_encode(srslte_pucch_t *q,
                                    srslte_pucch_format_t format,
                                    uint32_t n_pucch, // n_pucch_1 or n_pucch_2 depending on format
                                    uint32_t sf_idx, 
+                                   uint16_t rnti,
                                    uint8_t bits[SRSLTE_PUCCH_MAX_BITS], 
                                    cf_t *sf_symbols); 
 
@@ -141,6 +152,7 @@ SRSLTE_API int srslte_pucch_decode(srslte_pucch_t *q,
                                    srslte_pucch_format_t format,
                                    uint32_t n_pucch, // n_pucch_1 or n_pucch_2 depending on format
                                    uint32_t sf_idx, 
+                                   uint16_t rnti,
                                    cf_t *sf_symbols,
                                    cf_t *ce, 
                                    float noise_estimate,
diff --git a/srslte/include/srslte/phch/uci.h b/srslte/include/srslte/phch/uci.h
index 4b0d24195..fa916c283 100644
--- a/srslte/include/srslte/phch/uci.h
+++ b/srslte/include/srslte/phch/uci.h
@@ -56,6 +56,11 @@ typedef struct SRSLTE_API {
   int16_t *cqi_table_s[11];
 } srslte_uci_cqi_pusch_t;
 
+typedef struct SRSLTE_API {
+  uint8_t cqi_table[16][32];
+  int16_t cqi_table_s[16][32]; // aligned for simd
+} srslte_uci_cqi_pucch_t;
+
 typedef struct SRSLTE_API {
   uint8_t  uci_cqi[SRSLTE_CQI_MAX_BITS];
   uint32_t uci_cqi_len;
@@ -78,6 +83,18 @@ typedef struct {
   srslte_uci_bit_type_t type;
 } srslte_uci_bit_t;
 
+SRSLTE_API void srslte_uci_cqi_pucch_init(srslte_uci_cqi_pucch_t *q);
+
+SRSLTE_API int srslte_uci_encode_cqi_pucch(uint8_t *cqi_data, 
+                                           uint32_t cqi_len, 
+                                           uint8_t b_bits[SRSLTE_UCI_CQI_CODED_PUCCH_B]);
+
+SRSLTE_API int16_t srslte_uci_decode_cqi_pucch(srslte_uci_cqi_pucch_t *q, 
+                                           int16_t b_bits[32], // aligned for simd 
+                                           uint8_t *cqi_data, 
+                                           uint32_t cqi_len);
+
+
 SRSLTE_API int srslte_uci_cqi_init(srslte_uci_cqi_pusch_t *q);
 
 SRSLTE_API void srslte_uci_cqi_free(srslte_uci_cqi_pusch_t *q);
@@ -99,10 +116,6 @@ SRSLTE_API int srslte_uci_decode_cqi_pusch(srslte_uci_cqi_pusch_t *q,
                                            uint8_t *cqi_data, 
                                            bool *cqi_ack); 
 
-SRSLTE_API int srslte_uci_encode_cqi_pucch(uint8_t *cqi_data, 
-                                           uint32_t cqi_len, 
-                                           uint8_t b_bits[SRSLTE_UCI_CQI_CODED_PUCCH_B]);
-
 SRSLTE_API int srslte_uci_encode_ack(srslte_pusch_cfg_t *cfg,
                                      uint8_t data, 
                                      uint32_t O_cqi, 
diff --git a/srslte/lib/ch_estimation/chest_ul.c b/srslte/lib/ch_estimation/chest_ul.c
index 7e558c2d0..9f86b69ee 100644
--- a/srslte/lib/ch_estimation/chest_ul.c
+++ b/srslte/lib/ch_estimation/chest_ul.c
@@ -78,6 +78,13 @@ int srslte_chest_ul_init(srslte_chest_ul_t *q, srslte_cell_t cell)
       perror("malloc");
       goto clean_exit;
     }      
+    for (int i=0;i<4;i++) {
+      q->pilot_estimates_tmp[i] = srslte_vec_malloc(sizeof(cf_t) * NOF_REFS_SF);
+      if (!q->pilot_estimates_tmp[i]) {
+        perror("malloc");
+        goto clean_exit;
+      }      
+    }
     q->pilot_recv_signal = srslte_vec_malloc(sizeof(cf_t) * (NOF_REFS_SF+1));
     if (!q->pilot_recv_signal) {
       perror("malloc");
@@ -125,6 +132,11 @@ void srslte_chest_ul_free(srslte_chest_ul_t *q)
   if (q->pilot_estimates) {
     free(q->pilot_estimates);
   }      
+  for (int i=0;i<4;i++) {
+    if (q->pilot_estimates_tmp[i]) {
+      free(q->pilot_estimates_tmp[i]);
+    }
+  }
   if (q->pilot_recv_signal) {
     free(q->pilot_recv_signal);
   }
@@ -266,7 +278,8 @@ int srslte_chest_ul_estimate(srslte_chest_ul_t *q, cf_t *input, cf_t *ce,
 }
 
 int srslte_chest_ul_estimate_pucch(srslte_chest_ul_t *q, cf_t *input, cf_t *ce, 
-                                   srslte_pucch_format_t format, uint32_t n_pucch, uint32_t sf_idx) 
+                                   srslte_pucch_format_t format, uint32_t n_pucch, uint32_t sf_idx, 
+                                   uint8_t *pucch2_ack_bits) 
 {
   if (!q->dmrs_signal_configured) {
     fprintf(stderr, "Error must call srslte_chest_ul_set_cfg() before using the UL estimator\n");
@@ -285,11 +298,37 @@ int srslte_chest_ul_estimate_pucch(srslte_chest_ul_t *q, cf_t *input, cf_t *ce,
   
   /* Generate known pilots */
   uint8_t pucch2_bits[2] = {0, 0};
-  srslte_refsignal_dmrs_pucch_gen(&q->dmrs_signal, format, n_pucch, sf_idx, pucch2_bits, q->pilot_known_signal),
+  if (format == SRSLTE_PUCCH_FORMAT_2A || format == SRSLTE_PUCCH_FORMAT_2B) {
+    float max = -1e9;
+    int i_max = 0; 
+    
+    int m = 0; 
+    if (format == SRSLTE_PUCCH_FORMAT_2A) {
+      m = 2; 
+    } else {
+      m = 4; 
+    }
+    
+    for (int i=0;i<m;i++) {
+      pucch2_bits[0] = i%2;
+      pucch2_bits[1] = i/2;
+      srslte_refsignal_dmrs_pucch_gen(&q->dmrs_signal, format, n_pucch, sf_idx, pucch2_bits, q->pilot_known_signal);
+      srslte_vec_prod_conj_ccc(q->pilot_recv_signal, q->pilot_known_signal, q->pilot_estimates_tmp[i], nrefs_sf);
+      float x = cabsf(srslte_vec_acc_cc(q->pilot_estimates_tmp[i], nrefs_sf));
+      if (x >= max) {
+        max = x; 
+        i_max = i; 
+      }      
+    }
+    memcpy(q->pilot_estimates, q->pilot_estimates_tmp[i_max], nrefs_sf*sizeof(cf_t));
+    pucch2_ack_bits[0] = i_max%2;
+    pucch2_ack_bits[1] = i_max/2;
+  } else {
+    srslte_refsignal_dmrs_pucch_gen(&q->dmrs_signal, format, n_pucch, sf_idx, pucch2_bits, q->pilot_known_signal);
+    /* Use the known DMRS signal to compute Least-squares estimates */
+    srslte_vec_prod_conj_ccc(q->pilot_recv_signal, q->pilot_known_signal, q->pilot_estimates, nrefs_sf);
+  }
   
-  /* Use the known DMRS signal to compute Least-squares estimates */
-  srslte_vec_prod_conj_ccc(q->pilot_recv_signal, q->pilot_known_signal, q->pilot_estimates, nrefs_sf);
- 
   if (ce != NULL) {
     /* FIXME: Currently averaging entire slot, performance good enough? */
     for (int ns=0;ns<2;ns++) {
diff --git a/srslte/lib/enb/enb_ul.c b/srslte/lib/enb/enb_ul.c
index d41bfc245..f536e33f8 100644
--- a/srslte/lib/enb/enb_ul.c
+++ b/srslte/lib/enb/enb_ul.c
@@ -162,7 +162,16 @@ int srslte_enb_ul_add_rnti(srslte_enb_ul_t *q, uint16_t rnti)
 {
   if (!q->users[rnti]) {
     q->users[rnti] = malloc(sizeof(srslte_enb_ul_user_t));
-    return srslte_pusch_set_rnti(&q->pusch, rnti);
+    
+    if (srslte_pucch_set_crnti(&q->pucch, rnti)) {
+      fprintf(stderr, "Error setting PUCCH rnti\n");
+      return -1;
+    }
+    if (srslte_pusch_set_rnti(&q->pusch, rnti)) {
+      fprintf(stderr, "Error setting PUSCH rnti\n");
+      return -1; 
+    }
+    return 0; 
   } else {
     fprintf(stderr, "Error adding rnti=0x%x, already exists\n", rnti);
     return -1; 
@@ -221,13 +230,12 @@ int get_pucch(srslte_enb_ul_t *q, uint16_t rnti,
     
   uint32_t n_pucch = srslte_pucch_get_npucch(pdcch_n_cce, format, uci_data->scheduling_request, &q->users[rnti]->pucch_sched);
   
-  if (srslte_chest_ul_estimate_pucch(&q->chest, q->sf_symbols, q->ce, format, n_pucch, sf_rx)) {
+  if (srslte_chest_ul_estimate_pucch(&q->chest, q->sf_symbols, q->ce, format, n_pucch, sf_rx, &bits[20])) {
     fprintf(stderr,"Error estimating PUCCH DMRS\n");
     return SRSLTE_ERROR;
   }
   
-  
-  int ret_val = srslte_pucch_decode(&q->pucch, format, n_pucch, sf_rx, q->sf_symbols, q->ce, noise_power, bits); 
+  int ret_val = srslte_pucch_decode(&q->pucch, format, n_pucch, sf_rx, rnti, q->sf_symbols, q->ce, noise_power, bits); 
   if (ret_val < 0) {
     fprintf(stderr,"Error decoding PUCCH\n");
     return SRSLTE_ERROR; 
@@ -239,17 +247,17 @@ int srslte_enb_ul_get_pucch(srslte_enb_ul_t *q, uint16_t rnti,
                             uint32_t pdcch_n_cce, uint32_t sf_rx, 
                             srslte_uci_data_t *uci_data)
 {
-  uint8_t bits[SRSLTE_PUCCH_MAX_BITS];
+  uint8_t pucch_bits[SRSLTE_PUCCH_MAX_BITS];
   
   if (q->users[rnti]) {
 
-    int ret_val = get_pucch(q, rnti, pdcch_n_cce, sf_rx, uci_data, bits);
+    int ret_val = get_pucch(q, rnti, pdcch_n_cce, sf_rx, uci_data, pucch_bits);
 
     // If we are looking for SR and ACK at the same time and ret=0, means there is no SR. 
     // try again to decode ACK only 
     if (uci_data->scheduling_request && uci_data->uci_ack_len && ret_val != 1) {
       uci_data->scheduling_request = false; 
-      ret_val = get_pucch(q, rnti, pdcch_n_cce, sf_rx, uci_data, bits);
+      ret_val = get_pucch(q, rnti, pdcch_n_cce, sf_rx, uci_data, pucch_bits);
     }
 
     // update schedulign request 
@@ -259,8 +267,20 @@ int srslte_enb_ul_get_pucch(srslte_enb_ul_t *q, uint16_t rnti,
     
     // Save ACK bits 
     if (uci_data->uci_ack_len > 0) {
-      uci_data->uci_ack = bits[0];            
+      uci_data->uci_ack = pucch_bits[0];            
     }
+    
+    // PUCCH2 CQI bits are decoded inside srslte_pucch_decode() 
+    if (uci_data->uci_cqi_len) {
+      memcpy(uci_data->uci_cqi, pucch_bits, uci_data->uci_cqi_len*sizeof(uint8_t));
+      if (uci_data->uci_ack_len >= 1) {
+        uci_data->uci_ack = pucch_bits[20];
+      }
+      if (uci_data->uci_ack_len == 2) {
+        uci_data->uci_ack_2 = pucch_bits[21];
+      }
+    }
+
     return SRSLTE_SUCCESS;
   } else {
     fprintf(stderr, "Error getting PUCCH: rnti=0x%x not found\n", rnti);
diff --git a/srslte/lib/mimo/layermap.c b/srslte/lib/mimo/layermap.c
index 58cfc1121..982e00f5b 100644
--- a/srslte/lib/mimo/layermap.c
+++ b/srslte/lib/mimo/layermap.c
@@ -207,11 +207,9 @@ int srslte_layerdemap_type(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *d[SRSLTE_MAX_CODEWO
     }
     break;
   case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+  case SRSLTE_MIMO_TYPE_CDD:
     return srslte_layerdemap_multiplex(x, d, nof_layers, nof_cw, nof_layer_symbols, nof_symbols);
     break;
-  case SRSLTE_MIMO_TYPE_CDD:
-    fprintf(stderr, "CDD Not implemented\n");
-    return -1; 
   }
   return 0;
 }
diff --git a/srslte/lib/mimo/precoding.c b/srslte/lib/mimo/precoding.c
index b5af62711..289c6a981 100644
--- a/srslte/lib/mimo/precoding.c
+++ b/srslte/lib/mimo/precoding.c
@@ -516,9 +516,23 @@ int srslte_predecoding_diversity_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE
 }
 
 
+int srslte_predecoding_type(cf_t *y_, cf_t *h_[SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
+    int nof_ports, int nof_layers, int nof_symbols, srslte_mimo_type_t type, float noise_estimate) 
+{
+  cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS]; 
+  cf_t *y[SRSLTE_MAX_PORTS]; 
+  uint32_t nof_rxant = 1; 
+  
+  for (int i=0;i<nof_ports;i++) {
+    h[i][0] = h_[i];
+  }
+  y[0] = y_; 
+  return srslte_predecoding_type_multi(y, h, x, nof_rxant, nof_ports, nof_layers, nof_symbols, type, noise_estimate);  
+}
+
 /* 36.211 v10.3.0 Section 6.3.4 */
-int srslte_predecoding_type(cf_t *y, cf_t *h[SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
-    int nof_ports, int nof_layers, int nof_symbols, srslte_mimo_type_t type, float noise_estimate) {
+int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
+    int nof_rxant, int nof_ports, int nof_layers, int nof_symbols, srslte_mimo_type_t type, float noise_estimate) {
 
   if (nof_ports > SRSLTE_MAX_PORTS) {
     fprintf(stderr, "Maximum number of ports is %d (nof_ports=%d)\n", SRSLTE_MAX_PORTS,
@@ -537,7 +551,7 @@ int srslte_predecoding_type(cf_t *y, cf_t *h[SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_M
     return -1; 
   case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
     if (nof_ports == 1 && nof_layers == 1) {
-      return srslte_predecoding_single(y, h[0], x[0], nof_symbols, noise_estimate);              
+      return srslte_predecoding_single_multi(y, h[0], x[0], nof_rxant, nof_symbols, noise_estimate);              
     } else {
       fprintf(stderr,
           "Number of ports and layers must be 1 for transmission on single antenna ports\n");
@@ -546,7 +560,7 @@ int srslte_predecoding_type(cf_t *y, cf_t *h[SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_M
     break;
   case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
     if (nof_ports == nof_layers) {
-      return srslte_predecoding_diversity(y, h, x, nof_ports, nof_symbols);
+      return srslte_predecoding_diversity_multi(y, h, x, nof_rxant, nof_ports, nof_symbols);
     } else {
       fprintf(stderr,
           "Error number of layers must equal number of ports in transmit diversity\n");
diff --git a/srslte/lib/mimo/test/CMakeLists.txt b/srslte/lib/mimo/test/CMakeLists.txt
index d8d4f2359..9fe369779 100644
--- a/srslte/lib/mimo/test/CMakeLists.txt
+++ b/srslte/lib/mimo/test/CMakeLists.txt
@@ -52,5 +52,6 @@ add_test(precoding_single precoding_test -n 1000 -m single)
 add_test(precoding_diversity2 precoding_test -n 1000 -m diversity -l 2 -p 2) 
 add_test(precoding_diversity4 precoding_test -n 1024 -m diversity -l 4 -p 4) 
 
+ 
 
 
diff --git a/srslte/lib/mimo/test/predecoder_mex.c b/srslte/lib/mimo/test/predecoder_mex.c
index 5a2fa3611..629ac0dc6 100644
--- a/srslte/lib/mimo/test/predecoder_mex.c
+++ b/srslte/lib/mimo/test/predecoder_mex.c
@@ -31,16 +31,18 @@
 /** MEX function to be called from MATLAB to test the predecoder
  */
 
-#define INPUT prhs[0]
-#define HEST  prhs[1]
-#define NEST  prhs[2]
-#define NOF_INPUTS 2
+#define INPUT    prhs[0]
+#define HEST     prhs[1]
+#define NEST     prhs[2]
+#define NLAYERS  prhs[3]
+#define TXSCHEME prhs[4]
+#define NOF_INPUTS 5
 
 
 void help()
 {
   mexErrMsgTxt
-    ("[output] = srslte_predecoder(input, hest, nest)\n\n");
+    ("[output] = srslte_predecoder(input, hest, nest, Nl, TxScheme)\n\n");
 }
 
 /* the gateway function */
@@ -61,7 +63,10 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     mexErrMsgTxt("Error reading input\n");
     return; 
   }
+  uint32_t nof_layers   = mxGetScalar(NLAYERS); 
   uint32_t nof_tx_ports = 1; 
+  uint32_t nof_codewords = 1; 
+
   uint32_t nof_rx_ants  = 1; 
   const mwSize *dims = mxGetDimensions(INPUT);
   mwSize ndims = mxGetNumberOfDimensions(INPUT);
@@ -83,7 +88,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     nof_tx_ports = dims[2];        
   }
   
-  mexPrintf("nof_tx_ports=%d, nof_rx_ants=%d, nof_symbols=%d\n", nof_tx_ports, nof_rx_ants, nof_symbols);
+  mexPrintf("nof_tx_ports=%d, nof_rx_ants=%d, nof_layers=%d, nof_symbols=%d\n", nof_tx_ports, nof_rx_ants, nof_layers, nof_symbols);
 
   // Read noise estimate
   float noise_estimate = 0; 
@@ -117,12 +122,31 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     y[j] = &input[j*nof_symbols];
   }
     
-  if (nof_tx_ports > 1) {
-    srslte_predecoding_diversity_multi(y, h, x, nof_rx_ants, nof_tx_ports, nof_symbols); 
-    srslte_layerdemap_diversity(x, output, nof_tx_ports, nof_symbols / nof_tx_ports);
+  char *txscheme = "Port0";
+  if (nrhs >= NOF_INPUTS) {
+    txscheme = mxArrayToString(TXSCHEME);
+  }  
+  srslte_mimo_type_t type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA; 
+  if (!strcmp(txscheme, "Port0")) {
+    type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+  } else if (!strcmp(txscheme, "TxDiversity")) {
+    type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+  } else if (!strcmp(txscheme, "CDD")) {    
+    type = SRSLTE_MIMO_TYPE_CDD;
+  } else if (!strcmp(txscheme, "SpatialMux")) {    
+    type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
   } else {
-    srslte_predecoding_single_multi(y, h[0], output, nof_rx_ants, nof_symbols, noise_estimate);
+    mexPrintf("Unsupported TxScheme=%s\n", txscheme);
+    return; 
   }
+  int symbols_layers[SRSLTE_MAX_LAYERS];
+  for (int i=0;i<nof_layers;i++) {
+    symbols_layers[i] = nof_symbols; 
+  }
+  cf_t *d[SRSLTE_MAX_LAYERS]; 
+  d[0] = output; 
+  srslte_predecoding_type_multi(y, h, x, nof_rx_ants, nof_tx_ports, nof_layers, nof_symbols/nof_layers, type, noise_estimate);
+  srslte_layerdemap_type(x, d, nof_layers, nof_codewords, nof_symbols, symbols_layers, type);
   
 
   if (nlhs >= 1) { 
diff --git a/srslte/lib/phch/pdcch.c b/srslte/lib/phch/pdcch.c
index bd3bd2a9a..8b1d3ab52 100644
--- a/srslte/lib/phch/pdcch.c
+++ b/srslte/lib/phch/pdcch.c
@@ -219,7 +219,7 @@ uint32_t srslte_pdcch_ue_locations_ncce(uint32_t nof_cce, srslte_dci_location_t
     L = (1 << l);
     // For all possible ncce offset
     for (i = 0; i < SRSLTE_MIN(nof_cce / L, S[l]/PDCCH_FORMAT_NOF_CCE(l)); i++) {
-      if (nof_cce > L) {
+      if (nof_cce >= L) {
         ncce = L * ((Yk + i) % (nof_cce / L));      
         if (k < max_candidates  && ncce + L <= nof_cce) 
         {            
diff --git a/srslte/lib/phch/pucch.c b/srslte/lib/phch/pucch.c
index 9472711bb..443dd57bd 100644
--- a/srslte/lib/phch/pucch.c
+++ b/srslte/lib/phch/pucch.c
@@ -42,6 +42,7 @@
 #include "srslte/scrambling/scrambling.h"
 #include "srslte/utils/debug.h"
 #include "srslte/utils/vector.h"
+#include "srslte/modem/demod_soft.h"
 
 #define MAX_PUSCH_RE(cp) (2 * SRSLTE_CP_NSYMB(cp) * 12)
 
@@ -422,14 +423,13 @@ int srslte_pucch_init(srslte_pucch_t *q, srslte_cell_t cell) {
     bzero(q, sizeof(srslte_pucch_t));
     
     q->cell = cell; 
-    q->rnti_is_set = false; 
     
     srslte_pucch_cfg_default(&q->pucch_cfg);
     
     if (srslte_modem_table_lte(&q->mod, SRSLTE_MOD_QPSK)) {
       return SRSLTE_ERROR;
     }
-
+    
     // Precompute group hopping values u. 
     if (srslte_group_hopping_f_gh(q->f_gh, q->cell.id)) {
       return SRSLTE_ERROR;
@@ -439,6 +439,14 @@ int srslte_pucch_init(srslte_pucch_t *q, srslte_cell_t cell) {
       return SRSLTE_ERROR;
     }
     
+    q->users = calloc(sizeof(srslte_pucch_user_t*), 1+SRSLTE_SIRNTI);
+    if (!q->users) {
+      perror("malloc");
+      return SRSLTE_ERROR;
+    }
+    
+    srslte_uci_cqi_pucch_init(&q->cqi);
+    
     q->z = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_PUCCH_MAX_SYMBOLS);
     q->z_tmp = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_PUCCH_MAX_SYMBOLS);
     q->ce = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_PUCCH_MAX_SYMBOLS);
@@ -449,10 +457,11 @@ int srslte_pucch_init(srslte_pucch_t *q, srslte_cell_t cell) {
 }
 
 void srslte_pucch_free(srslte_pucch_t *q) {
-  if (q->rnti_is_set) {
-    for (uint32_t sf_idx=0;sf_idx<SRSLTE_NSUBFRAMES_X_FRAME;sf_idx++) {
-      srslte_sequence_free(&q->seq_f2[sf_idx]);
+  if (q->users) {
+    for (int rnti=0;rnti<SRSLTE_SIRNTI;rnti++) {      
+      srslte_pucch_clear_rnti(q, rnti);
     }
+    free(q->users);
   }
   if (q->z) {
     free(q->z);
@@ -468,15 +477,29 @@ void srslte_pucch_free(srslte_pucch_t *q) {
   bzero(q, sizeof(srslte_pucch_t));
 }
 
-int srslte_pucch_set_crnti(srslte_pucch_t *q, uint16_t c_rnti) {
-  for (uint32_t sf_idx=0;sf_idx<SRSLTE_NSUBFRAMES_X_FRAME;sf_idx++) {
-    // Precompute scrambling sequence for pucch format 2    
-    if (srslte_sequence_pucch(&q->seq_f2[sf_idx], c_rnti, 2*sf_idx, q->cell.id)) {
-      fprintf(stderr, "Error computing PUCCH Format 2 scrambling sequence\n");
-      return SRSLTE_ERROR; 
-    }        
+void srslte_pucch_clear_rnti(srslte_pucch_t *q, uint16_t rnti) {
+  if (q->users[rnti]) {
+    for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
+      srslte_sequence_free(&q->users[rnti]->seq_f2[i]);
+    }    
+    free(q->users[rnti]);
+    q->users[rnti] = NULL; 
+  }
+}
+
+int srslte_pucch_set_crnti(srslte_pucch_t *q, uint16_t rnti) {
+  if (!q->users[rnti]) {
+    q->users[rnti] = malloc(sizeof(srslte_pucch_user_t));
+    if (q->users[rnti]) {
+      for (uint32_t sf_idx=0;sf_idx<SRSLTE_NSUBFRAMES_X_FRAME;sf_idx++) {
+        // Precompute scrambling sequence for pucch format 2    
+        if (srslte_sequence_pucch(&q->users[rnti]->seq_f2[sf_idx], rnti, 2*sf_idx, q->cell.id)) {
+          fprintf(stderr, "Error computing PUCCH Format 2 scrambling sequence\n");
+          return SRSLTE_ERROR; 
+        }        
+      }
+    }
   }
-  q->rnti_is_set = true; 
   return SRSLTE_SUCCESS; 
 }
 
@@ -549,7 +572,7 @@ int srslte_pucch_format2ab_mod_bits(srslte_pucch_format_t format, uint8_t bits[2
 }
 
 /* Encode PUCCH bits according to Table 5.4.1-1 in Section 5.4.1 of 36.211 */
-static int uci_mod_bits(srslte_pucch_t *q, srslte_pucch_format_t format, uint8_t bits[SRSLTE_PUCCH_MAX_BITS], uint32_t sf_idx)
+static int uci_mod_bits(srslte_pucch_t *q, srslte_pucch_format_t format, uint8_t bits[SRSLTE_PUCCH_MAX_BITS], uint32_t sf_idx, uint16_t rnti)
 {  
   uint8_t tmp[2];
   
@@ -568,9 +591,14 @@ static int uci_mod_bits(srslte_pucch_t *q, srslte_pucch_format_t format, uint8_t
     case SRSLTE_PUCCH_FORMAT_2:
     case SRSLTE_PUCCH_FORMAT_2A:
     case SRSLTE_PUCCH_FORMAT_2B:
-      memcpy(q->bits_scram, bits, SRSLTE_PUCCH_MAX_BITS*sizeof(uint8_t));
-      srslte_scrambling_b(&q->seq_f2[sf_idx], q->bits_scram);
-      srslte_mod_modulate(&q->mod, q->bits_scram, q->d, SRSLTE_PUCCH_MAX_BITS);
+      if (q->users[rnti]) {
+        memcpy(q->bits_scram, bits, SRSLTE_PUCCH2_NOF_BITS*sizeof(uint8_t));
+        srslte_scrambling_b(&q->users[rnti]->seq_f2[sf_idx], q->bits_scram);
+        srslte_mod_modulate(&q->mod, q->bits_scram, q->d, SRSLTE_PUCCH2_NOF_BITS);
+      } else {
+        fprintf(stderr, "Error modulating PUCCH2 bits: rnti not set\n");
+        return -1; 
+      }
       break;
     default:
       fprintf(stderr, "PUCCH format 2 not supported\n");
@@ -582,13 +610,19 @@ static int uci_mod_bits(srslte_pucch_t *q, srslte_pucch_format_t format, uint8_t
 // Declare this here, since we can not include refsignal_ul.h
 void srslte_refsignal_r_uv_arg_1prb(float *arg, uint32_t u);
 
-static int pucch_encode(srslte_pucch_t* q, srslte_pucch_format_t format, 
-                    uint32_t n_pucch, uint32_t sf_idx, 
-                    uint8_t bits[SRSLTE_PUCCH_MAX_BITS], cf_t z[SRSLTE_PUCCH_MAX_SYMBOLS]) 
+static int pucch_encode_(srslte_pucch_t* q, srslte_pucch_format_t format, 
+                          uint32_t n_pucch, uint32_t sf_idx, uint16_t rnti,
+                          uint8_t bits[SRSLTE_PUCCH_MAX_BITS], cf_t z[SRSLTE_PUCCH_MAX_SYMBOLS], bool signal_only) 
 {
-  if (uci_mod_bits(q, format, bits, sf_idx)) {
-    fprintf(stderr, "Error encoding PUCCH bits\n");
-    return SRSLTE_ERROR; 
+  if (!signal_only) {
+    if (uci_mod_bits(q, format, bits, sf_idx, rnti)) {
+      fprintf(stderr, "Error encoding PUCCH bits\n");
+      return SRSLTE_ERROR; 
+    }
+  } else {
+    for (int i=0;i<SRSLTE_PUCCH_MAX_BITS/2;i++) {
+      q->d[i] = 1.0; 
+    }
   }
   uint32_t N_sf_0 = get_N_sf(format, 0, q->shortened);
   for (uint32_t ns=2*sf_idx;ns<2*(sf_idx+1);ns++) {
@@ -631,9 +665,17 @@ static int pucch_encode(srslte_pucch_t* q, srslte_pucch_format_t format,
   return SRSLTE_SUCCESS;
 }
 
+static int pucch_encode(srslte_pucch_t* q, srslte_pucch_format_t format, 
+                    uint32_t n_pucch, uint32_t sf_idx, uint16_t rnti,
+                    uint8_t bits[SRSLTE_PUCCH_MAX_BITS], cf_t z[SRSLTE_PUCCH_MAX_SYMBOLS]) 
+{
+  return pucch_encode_(q, format, n_pucch, sf_idx, rnti, bits, z, false); 
+}
+
+
 /* Encode, modulate and resource mapping of PUCCH bits according to Section 5.4.1 of 36.211 */
 int srslte_pucch_encode(srslte_pucch_t* q, srslte_pucch_format_t format, 
-                        uint32_t n_pucch, uint32_t sf_idx, uint8_t bits[SRSLTE_PUCCH_MAX_BITS], 
+                        uint32_t n_pucch, uint32_t sf_idx, uint16_t rnti, uint8_t bits[SRSLTE_PUCCH_MAX_BITS], 
                         cf_t *sf_symbols) 
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
@@ -656,11 +698,7 @@ int srslte_pucch_encode(srslte_pucch_t* q, srslte_pucch_format_t format,
     
     q->last_n_pucch = n_pucch; 
     
-    if (format >= SRSLTE_PUCCH_FORMAT_2 && !q->rnti_is_set) {
-      fprintf(stderr, "Error encoding PUCCH: C-RNTI must be set before encoding PUCCH Format 2/2a/2b\n");
-      return SRSLTE_ERROR; 
-    }
-    if (pucch_encode(q, format, n_pucch, sf_idx, bits, q->z)) {
+    if (pucch_encode(q, format, n_pucch, sf_idx, rnti, bits, q->z)) {
       return SRSLTE_ERROR; 
     }
     if (pucch_put(q, format, n_pucch, q->z, sf_symbols) < 0) {
@@ -680,7 +718,7 @@ float srslte_pucch_get_last_corr(srslte_pucch_t* q)
   
 /* Equalize, demodulate and decode PUCCH bits according to Section 5.4.1 of 36.211 */
 int srslte_pucch_decode(srslte_pucch_t* q, srslte_pucch_format_t format, 
-                        uint32_t n_pucch, uint32_t sf_idx, cf_t *sf_symbols, cf_t *ce, float noise_estimate, 
+                        uint32_t n_pucch, uint32_t sf_idx, uint16_t rnti, cf_t *sf_symbols, cf_t *ce, float noise_estimate, 
                         uint8_t bits[SRSLTE_PUCCH_MAX_BITS]) 
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
@@ -689,6 +727,8 @@ int srslte_pucch_decode(srslte_pucch_t* q, srslte_pucch_format_t format,
       sf_symbols != NULL)
   {
     ret = SRSLTE_ERROR; 
+    cf_t ref[SRSLTE_PUCCH_MAX_SYMBOLS]; 
+    int16_t llr_pucch2[32];
     
     // Shortened PUCCH happen in every cell-specific SRS subframes for Format 1/1a/1b
     if (q->pucch_cfg.srs_configured && format < SRSLTE_PUCCH_FORMAT_2) {
@@ -704,10 +744,6 @@ int srslte_pucch_decode(srslte_pucch_t* q, srslte_pucch_format_t format,
     
     q->last_n_pucch = n_pucch; 
     
-    if (format >= SRSLTE_PUCCH_FORMAT_2 && !q->rnti_is_set) {
-      fprintf(stderr, "Error decoding PUCCH: C-RNTI must be set before encoding PUCCH Format 2/2a/2b\n");
-      return SRSLTE_ERROR; 
-    }
     int nof_re = pucch_get(q, format, n_pucch, sf_symbols, q->z_tmp); 
     if (nof_re < 0) {
       fprintf(stderr, "Error getting PUCCH symbols\n");
@@ -728,7 +764,7 @@ int srslte_pucch_decode(srslte_pucch_t* q, srslte_pucch_format_t format,
     switch(format) {
       case SRSLTE_PUCCH_FORMAT_1:
         bzero(bits, SRSLTE_PUCCH_MAX_BITS*sizeof(uint8_t));
-        pucch_encode(q, format, n_pucch, sf_idx, bits, q->z_tmp);
+        pucch_encode(q, format, n_pucch, sf_idx, rnti, bits, q->z_tmp);
         corr = crealf(srslte_vec_dot_prod_conj_ccc(q->z, q->z_tmp, nof_re))/nof_re;
         if (corr >= q->threshold_format1) {
           ret = 1; 
@@ -743,7 +779,7 @@ int srslte_pucch_decode(srslte_pucch_t* q, srslte_pucch_format_t format,
         ret = 0; 
         for (int b=0;b<2;b++) {
           bits[0] = b; 
-          pucch_encode(q, format, n_pucch, sf_idx, bits, q->z_tmp);
+          pucch_encode(q, format, n_pucch, sf_idx, rnti, bits, q->z_tmp);
           corr = crealf(srslte_vec_dot_prod_conj_ccc(q->z, q->z_tmp, nof_re))/nof_re;          
           if (corr > corr_max) {
             corr_max = corr; 
@@ -755,28 +791,32 @@ int srslte_pucch_decode(srslte_pucch_t* q, srslte_pucch_format_t format,
           DEBUG("format1a b=%d, corr=%f, nof_re=%d, th=%f\n", b, corr, nof_re, q->threshold_format1a);
         }
         q->last_corr = corr_max; 
-
-/*
-        if (corr_max < 0.01) {
-          srslte_vec_save_file("sf_symbols", sf_symbols, sizeof(cf_t)*SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp));
-          srslte_vec_save_file("sf_ce", ce, sizeof(cf_t)*SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp));
-          srslte_vec_save_file("ce", q->ce, sizeof(cf_t)*nof_re);
-          srslte_vec_save_file("z_before", zz, sizeof(cf_t)*nof_re);
-          srslte_vec_save_file("z_eq", q->z, sizeof(cf_t)*nof_re);
-          srslte_vec_save_file("z_1", q->z_tmp, sizeof(cf_t)*nof_re);
-          bits[0] = 0; 
-          pucch_encode(q, format, n_pucch, sf_idx, bits, q->z_tmp);
-          srslte_vec_save_file("z_0", q->z_tmp, sizeof(cf_t)*nof_re);
-          printf("corr_max=%f, b_max=%d, n_pucch=%d, n_prb=%d, sf_idx=%d, nof_re=%d, noise_estimate=%f\n", corr_max, b_max, n_pucch, q->last_n_prb, sf_idx, nof_re, noise_estimate);
-          exit(-1);
-        }
-*/
         bits[0] = b_max; 
         break;
-      default:
-        fprintf(stderr, "Error decoding PUCCH: Format %d not supported\n", format);
-        ret = SRSLTE_ERROR; 
+      case SRSLTE_PUCCH_FORMAT_2:
+      case SRSLTE_PUCCH_FORMAT_2A:
+      case SRSLTE_PUCCH_FORMAT_2B:
+        if (q->users[rnti]) {
+          pucch_encode_(q, format, n_pucch, sf_idx, rnti, NULL, ref, true);
+          srslte_vec_prod_conj_ccc(q->z, ref, q->z_tmp, SRSLTE_PUCCH_MAX_SYMBOLS);
+          for (int i=0;i<SRSLTE_PUCCH2_NOF_BITS/2;i++) {
+            q->z[i] = 0; 
+            for (int j=0;j<SRSLTE_NRE;j++) {
+              q->z[i] += q->z_tmp[i*SRSLTE_NRE+j]/SRSLTE_NRE;
+            }
+          }
+          srslte_demod_soft_demodulate_s(SRSLTE_MOD_QPSK, q->z, llr_pucch2, SRSLTE_PUCCH2_NOF_BITS/2);
+          srslte_scrambling_s(&q->users[rnti]->seq_f2[sf_idx], llr_pucch2);  
+          q->last_corr = (float) srslte_uci_decode_cqi_pucch(&q->cqi, llr_pucch2, bits, 4)/2000;
+          ret = 1; 
+        } else {
+          fprintf(stderr, "Decoding PUCCH2: rnti not set\n");
+          return -1; 
+        }
         break;
+      default:
+        fprintf(stderr, "PUCCH format %d not implemented\n", format);
+        return SRSLTE_ERROR; 
     }
   }
 
diff --git a/srslte/lib/phch/test/pucch_test.c b/srslte/lib/phch/test/pucch_test.c
index 20b173e4b..316a5efc4 100644
--- a/srslte/lib/phch/test/pucch_test.c
+++ b/srslte/lib/phch/test/pucch_test.c
@@ -135,7 +135,7 @@ int main(int argc, char **argv) {
             goto quit; 
           }
           
-          if (srslte_pucch_encode(&pucch, format, n_pucch, subframe, bits, sf_symbols)) {
+          if (srslte_pucch_encode(&pucch, format, n_pucch, subframe, 11, bits, sf_symbols)) {
             fprintf(stderr, "Error encoding PUCCH\n");
             goto quit; 
           }
diff --git a/srslte/lib/phch/test/pucch_test_mex.c b/srslte/lib/phch/test/pucch_test_mex.c
index c65097b11..baf6c84db 100644
--- a/srslte/lib/phch/test/pucch_test_mex.c
+++ b/srslte/lib/phch/test/pucch_test_mex.c
@@ -192,7 +192,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     return;
   }
   
-  if (srslte_pucch_decode(&pucch, format, n_pucch, sf_idx, sf_symbols, ce, 0, bits)) {
+  if (srslte_pucch_decode(&pucch, format, n_pucch, sf_idx, sf_symbols, ce, 0, bits)<0) {
     mexErrMsgTxt("Error decoding PUCCH\n");
     return; 
   }
@@ -210,11 +210,11 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   }
 
   if (nlhs >= 2) {
-    mexutils_write_cf(pucch.z, &plhs[1], 2*srslte_refsignal_dmrs_N_rs(format, cell.cp)*SRSLTE_NRE*2, 1);  
+    mexutils_write_cf(pucch.z, &plhs[1], 10, 1);  
   }
 
   if (nlhs >= 3) {
-    mexutils_write_cf(ce, &plhs[2], nof_re, 1);
+    mexutils_write_cf(pucch.z_tmp, &plhs[2], 120, 1);
   }
 
   srslte_pucch_free(&pucch);  
diff --git a/srslte/lib/phch/uci.c b/srslte/lib/phch/uci.c
index 03234d26e..60ce417f5 100644
--- a/srslte/lib/phch/uci.c
+++ b/srslte/lib/phch/uci.c
@@ -104,7 +104,75 @@ static uint8_t M_basis_seq_pucch[20][13]={
                                   {1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0},
                                   };                                    
 
+void srslte_uci_cqi_pucch_init(srslte_uci_cqi_pucch_t *q) {
+  uint8_t word[16]; 
+    
+  uint32_t nwords      = 16;
+  for (uint32_t w=0;w<nwords;w++) {
+    uint8_t *ptr = word; 
+    srslte_bit_unpack(w, &ptr, 4);
+    srslte_uci_encode_cqi_pucch(word, 4, q->cqi_table[w]);    
+    for (int j=0;j<SRSLTE_UCI_CQI_CODED_PUCCH_B;j++) {
+      q->cqi_table_s[w][j] = 2*q->cqi_table[w][j]-1;
+    }
+  }
+}
 
+/* Encode UCI CQI/PMI as described in 5.2.3.3 of 36.212 
+ */
+int srslte_uci_encode_cqi_pucch(uint8_t *cqi_data, uint32_t cqi_len, uint8_t b_bits[SRSLTE_UCI_CQI_CODED_PUCCH_B])
+{
+  if (cqi_len <= SRSLTE_UCI_MAX_CQI_LEN_PUCCH) {
+    for (uint32_t i=0;i<SRSLTE_UCI_CQI_CODED_PUCCH_B;i++) {
+      uint64_t x=0;
+      for (uint32_t n=0;n<cqi_len;n++) {
+        x += cqi_data[n]*M_basis_seq_pucch[i][n];
+      }
+      b_bits[i] = (uint8_t) (x%2);
+    }
+    return SRSLTE_SUCCESS;
+  } else {
+    return SRSLTE_ERROR_INVALID_INPUTS;
+  }
+}
+
+/* Decode UCI CQI/PMI over PUCCH 
+ */
+int16_t srslte_uci_decode_cqi_pucch(srslte_uci_cqi_pucch_t *q, int16_t b_bits[32], uint8_t *cqi_data, uint32_t cqi_len)
+{
+  if (cqi_len           == 4     &&
+      b_bits            != NULL  &&
+      cqi_data          != NULL) 
+  {
+    uint32_t max_w = 0;
+    int32_t max_corr = INT32_MIN;   
+    for (uint32_t w=0;w<16;w++) {
+          
+      // Calculate correlation with pregenerated word and select maximum
+      int32_t corr = srslte_vec_dot_prod_sss(q->cqi_table_s[w], b_bits, SRSLTE_UCI_CQI_CODED_PUCCH_B);
+      if (corr > max_corr) {
+        max_corr = corr; 
+        max_w = w; 
+      }
+    }
+    // Convert word to bits again
+    uint8_t *ptr = cqi_data; 
+    srslte_bit_unpack(max_w, &ptr, cqi_len);
+    
+    INFO("Decoded CQI: w=%d, corr=%d\n", max_w, max_corr);
+    return max_corr;
+  } else {
+    return SRSLTE_ERROR_INVALID_INPUTS;
+  }  
+}
+
+                                  
+                                  
+                                  
+                                  
+                                  
+                                  
+                                  
 void encode_cqi_pusch_block(srslte_uci_cqi_pusch_t *q, uint8_t *data, uint32_t nof_bits, uint8_t output[32]) {
   for (int i=0;i<32;i++) {
     output[i] = 0;
@@ -320,24 +388,6 @@ int decode_cqi_long(srslte_uci_cqi_pusch_t *q, int16_t *q_bits, uint32_t Q,
   return ret;   
 }
 
-/* Encode UCI CQI/PMI as described in 5.2.3.3 of 36.212 
- */
-int srslte_uci_encode_cqi_pucch(uint8_t *cqi_data, uint32_t cqi_len, uint8_t b_bits[SRSLTE_UCI_CQI_CODED_PUCCH_B])
-{
-  if (cqi_len <= SRSLTE_UCI_MAX_CQI_LEN_PUCCH) {
-    for (uint32_t i=0;i<SRSLTE_UCI_CQI_CODED_PUCCH_B;i++) {
-      uint64_t x=0;
-      for (uint32_t n=0;n<cqi_len;n++) {
-        x += cqi_data[n]*M_basis_seq_pucch[i][n];
-      }
-      b_bits[i] = (uint8_t) (x%2);
-    }
-    return SRSLTE_SUCCESS;
-  } else {
-    return SRSLTE_ERROR_INVALID_INPUTS;
-  }
-}
-
 /* Encode UCI CQI/PMI 
  */
 int srslte_uci_decode_cqi_pusch(srslte_uci_cqi_pusch_t *q, srslte_pusch_cfg_t *cfg,
diff --git a/srslte/lib/rf/rf_uhd_imp.c b/srslte/lib/rf/rf_uhd_imp.c
index 2c6584abc..dbe293bf8 100644
--- a/srslte/lib/rf/rf_uhd_imp.c
+++ b/srslte/lib/rf/rf_uhd_imp.c
@@ -108,12 +108,16 @@ static bool find_string(uhd_string_vector_handle h, char *str)
   return false; 
 }
 
-static bool isLocked(rf_uhd_handler_t *handler, char *sensor_name, uhd_sensor_value_handle *value_h)
+static bool isLocked(rf_uhd_handler_t *handler, char *sensor_name, bool is_rx, uhd_sensor_value_handle *value_h)
 {
   bool val_out = false; 
   
   if (sensor_name) {
-    uhd_usrp_get_rx_sensor(handler->usrp, sensor_name, 0, value_h);
+    if (is_rx) {
+      uhd_usrp_get_rx_sensor(handler->usrp, sensor_name, 0, value_h);
+    } else {
+      uhd_usrp_get_mboard_sensor(handler->usrp, sensor_name, 0, value_h);
+    }
     uhd_sensor_value_to_bool(*value_h, &val_out);
   } else {
     usleep(500);
@@ -143,26 +147,28 @@ bool rf_uhd_rx_wait_lo_locked(void *h)
   uhd_usrp_get_mboard_sensor_names(handler->usrp, 0, &mb_sensors);
   uhd_usrp_get_rx_sensor_names(handler->usrp, 0, &rx_sensors);
 
-  if (find_string(rx_sensors, "lo_locked")) {
+  /*if (find_string(rx_sensors, "lo_locked")) {
     sensor_name = "lo_locked";
-  } else if (find_string(mb_sensors, "ref_locked")) {
+  } else */if (find_string(mb_sensors, "ref_locked")) {
     sensor_name = "ref_locked";
   } else {
     sensor_name = NULL;
   }
   
   double report = 0.0;
-  while (!isLocked(handler, sensor_name, &value_h) && report < 30.0) {
+  while (!isLocked(handler, sensor_name, false, &value_h) && report < 30.0) {
     report += 0.1;
     usleep(1000);
   }
 
-  bool val = isLocked(handler, sensor_name, &value_h);
+  bool val = isLocked(handler, sensor_name, false, &value_h);
   
   uhd_string_vector_free(&mb_sensors);
   uhd_string_vector_free(&rx_sensors);
   uhd_sensor_value_free(&value_h);
 
+  printf("Locked=%d\n", val);
+  
   return val;
 }
 
@@ -335,7 +341,10 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
     // Set external clock reference   
     if (strstr(args, "clock=external")) {
       uhd_usrp_set_clock_source(handler->usrp, "external", 0);       
+    } else if (strstr(args, "clock=gpsdo")) {
+      uhd_usrp_set_clock_source(handler->usrp, "gpsdo", 0);       
     }
+
       
     handler->has_rssi = get_has_rssi(handler);  
     if (handler->has_rssi) {        
diff --git a/srslte/lib/rf/uhd_c_api.cpp b/srslte/lib/rf/uhd_c_api.cpp
index c7684aa23..92af48f69 100644
--- a/srslte/lib/rf/uhd_c_api.cpp
+++ b/srslte/lib/rf/uhd_c_api.cpp
@@ -9,6 +9,7 @@ extern "C" {
 #include "uhd_c_api.h"
 }
 
+#if UHD_VERSION < 31100
 static void (*handler)(const char*);
 
 void translate_handler(uhd::msg::type_t type, const std::string & msg)
@@ -16,11 +17,14 @@ void translate_handler(uhd::msg::type_t type, const std::string & msg)
   if(handler)
     handler(msg.c_str());
 }
+#endif
 
 void rf_uhd_register_msg_handler_c(void (*new_handler)(const char*)) 
 {
+#if UHD_VERSION < 31100
   handler = new_handler;
   uhd::msg::register_handler(translate_handler);
+#endif
 }
 
 void uhd_tx_metadata_set_time_spec(uhd_tx_metadata_handle *md, time_t secs, double frac_secs)
diff --git a/srslte/lib/ue/ue_ul.c b/srslte/lib/ue/ue_ul.c
index 2b05e2df1..b14b45673 100644
--- a/srslte/lib/ue/ue_ul.c
+++ b/srslte/lib/ue/ue_ul.c
@@ -269,7 +269,7 @@ int srslte_ue_ul_pucch_encode(srslte_ue_ul_t *q, srslte_uci_data_t uci_data,
     // Choose n_pucch 
     uint32_t n_pucch = srslte_pucch_get_npucch(pdcch_n_cce, format, uci_data.scheduling_request, &q->pucch_sched);
     
-    if (srslte_pucch_encode(&q->pucch, format, n_pucch, sf_idx, pucch_bits, q->sf_symbols)) {
+    if (srslte_pucch_encode(&q->pucch, format, n_pucch, sf_idx, q->current_rnti, pucch_bits, q->sf_symbols)) {
       fprintf(stderr, "Error encoding TB\n");
       return ret; 
     }