diff --git a/mex/include/srslte/mex/mexutils.h b/mex/include/srslte/mex/mexutils.h
index b5635bdd5..a1aad15ca 100644
--- a/mex/include/srslte/mex/mexutils.h
+++ b/mex/include/srslte/mex/mexutils.h
@@ -29,16 +29,16 @@
 #ifndef MEXUTILS_
 #define MEXUTILS_
 
-
-#include <stdbool.h>
-
 #ifdef UNDEF_BOOL
 #undef bool
 #endif
 
 #include "mex.h"
 
+#include <stdbool.h>
+
 #include "srslte/config.h"
+#include "srslte/common/phy_common.h"
 
  
 
diff --git a/mex/lib/mexutils.c b/mex/lib/mexutils.c
index 7b2803190..fc9071019 100644
--- a/mex/lib/mexutils.c
+++ b/mex/lib/mexutils.c
@@ -27,9 +27,9 @@
 
 #include <string.h>
 #include <complex.h>
-#include "srslte/common/phy_common.h"
 #include "srslte/mex/mexutils.h"
 #include "srslte/utils/vector.h"
+#include "srslte/common/phy_common.h"
 
 
 bool mexutils_isScalar(const mxArray *ptr) {
diff --git a/srslte/examples/pdsch_ue.c b/srslte/examples/pdsch_ue.c
index d081e5efe..eb24e0759 100644
--- a/srslte/examples/pdsch_ue.c
+++ b/srslte/examples/pdsch_ue.c
@@ -277,7 +277,8 @@ int main(int argc, char **argv) {
     srslte_netsink_set_nonblocking(&net_sink);
   }
   if (prog_args.net_port_signal > 0) {
-    if (srslte_netsink_init(&net_sink_signal, prog_args.net_address_signal, prog_args.net_port_signal, SRSLTE_NETSINK_UDP)) {
+    if (srslte_netsink_init(&net_sink_signal, prog_args.net_address_signal, 
+      prog_args.net_port_signal, SRSLTE_NETSINK_UDP)) {
       fprintf(stderr, "Error initiating UDP socket to %s:%d\n", prog_args.net_address_signal, prog_args.net_port_signal);
       exit(-1);
     }
diff --git a/srslte/examples/prach_ue.c b/srslte/examples/prach_ue.c
index db37631b4..031cb623f 100644
--- a/srslte/examples/prach_ue.c
+++ b/srslte/examples/prach_ue.c
@@ -394,7 +394,7 @@ cell.nof_ports = 1;
   dmrs_cfg.sequence_hopping_en = false; 
   dmrs_cfg.delta_ss = 0;
   dmrs_cfg.cyclic_shift = 0; 
-  srslte_ue_ul_set_cfg(&ue_ul, &dmrs_cfg, NULL, NULL, NULL, NULL, NULL);
+  srslte_ue_ul_set_cfg(&ue_ul, &dmrs_cfg, NULL, NULL, NULL, NULL, NULL, NULL);
 
   cf_t *ul_signal = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN_PRB(cell.nof_prb));
   if (!ul_signal) {
diff --git a/srslte/include/srslte/common/phy_common.h b/srslte/include/srslte/common/phy_common.h
index 39c673669..b1fceea43 100644
--- a/srslte/include/srslte/common/phy_common.h
+++ b/srslte/include/srslte/common/phy_common.h
@@ -48,6 +48,8 @@
 
 #define SRSLTE_NSOFT_BITS  250368 // Soft buffer size for Category 1 UE
 
+#define SRSLTE_PC_MAX 23         // Maximum TX power for Category 1 UE (in dBm)
+
 #define SRSLTE_MAX_PORTS     4
 #define SRSLTE_MAX_LAYERS    8
 #define SRSLTE_MAX_CODEWORDS 2
diff --git a/srslte/include/srslte/cuhd/cuhd.h b/srslte/include/srslte/cuhd/cuhd.h
index 9ab0cb51e..82fc158fb 100644
--- a/srslte/include/srslte/cuhd/cuhd.h
+++ b/srslte/include/srslte/cuhd/cuhd.h
@@ -74,6 +74,9 @@ SRSLTE_API void cuhd_set_tx_rx_gain_offset(void *h,
 SRSLTE_API double cuhd_set_rx_gain_th(void *h, 
                                       double gain);
 
+SRSLTE_API double cuhd_set_tx_gain_th(void *h, 
+                                      double gain);
+
 SRSLTE_API float cuhd_get_rx_gain_offset(void *h); 
 
 SRSLTE_API double cuhd_get_rx_gain(void *h);
diff --git a/srslte/include/srslte/phch/pusch_cfg.h b/srslte/include/srslte/phch/pusch_cfg.h
index 3b1fa1855..4732e2c20 100644
--- a/srslte/include/srslte/phch/pusch_cfg.h
+++ b/srslte/include/srslte/phch/pusch_cfg.h
@@ -55,6 +55,7 @@ typedef struct SRSLTE_API {
   uint32_t sf_idx;
   uint32_t tti; 
   srslte_cp_t cp; 
+  uint32_t last_O_cqi;
 } srslte_pusch_cfg_t;
 
 #endif
diff --git a/srslte/include/srslte/phch/sch.h b/srslte/include/srslte/phch/sch.h
index 9fe0cb033..8e396fad1 100644
--- a/srslte/include/srslte/phch/sch.h
+++ b/srslte/include/srslte/phch/sch.h
@@ -123,6 +123,8 @@ SRSLTE_API int srslte_ulsch_decode(srslte_sch_t *q,
                                    float *e_bits, 
                                    uint8_t *data);
 
+SRSLTE_API float srslte_sch_beta_cqi(uint32_t I_cqi); 
+
 SRSLTE_API uint32_t srslte_sch_find_Ioffset_ack(float beta); 
 
 SRSLTE_API uint32_t srslte_sch_find_Ioffset_cqi(float beta); 
diff --git a/srslte/include/srslte/ue/ue_ul.h b/srslte/include/srslte/ue/ue_ul.h
index 2062081e3..1bc24de5b 100644
--- a/srslte/include/srslte/ue/ue_ul.h
+++ b/srslte/include/srslte/ue/ue_ul.h
@@ -54,8 +54,22 @@
 
 #define SRSLTE_UE_UL_NOF_HARQ_PROCESSES 8
 
-
-
+/* UE UL power control */
+typedef struct {
+  // Common configuration
+  float p0_nominal_pusch;
+  float alpha;
+  float p0_nominal_pucch;
+  float delta_f_pucch[5];
+  float delta_preamble_msg3;
+  
+  // Dedicated configuration
+  float p0_ue_pusch;
+  bool delta_mcs_based;
+  bool acc_enabled;
+  float p0_ue_pucch;
+  float p_srs_offset;  
+} srslte_ue_ul_powerctrl_t;
 
 typedef struct SRSLTE_API {
   srslte_ofdm_t fft;
@@ -81,7 +95,8 @@ typedef struct SRSLTE_API {
   srslte_refsignal_srs_cfg_t        srs_cfg;
   srslte_uci_cfg_t                  uci_cfg;
   srslte_pusch_hopping_cfg_t        hopping_cfg;
-   
+  srslte_ue_ul_powerctrl_t          power_ctrl;
+  
   cf_t *refsignal; 
   cf_t *srs_signal; 
   cf_t *sf_symbols; 
@@ -90,6 +105,8 @@ typedef struct SRSLTE_API {
   bool signals_pregenerated;
 }srslte_ue_ul_t;
 
+
+
 /* This function shall be called just after the initial synchronization */
 SRSLTE_API int srslte_ue_ul_init(srslte_ue_ul_t *q, 
                                  srslte_cell_t cell);
@@ -111,7 +128,8 @@ SRSLTE_API void srslte_ue_ul_set_cfg(srslte_ue_ul_t *q,
                                      srslte_pucch_cfg_t                *pucch_cfg, 
                                      srslte_pucch_sched_t              *pucch_sched, 
                                      srslte_uci_cfg_t                  *uci_cfg,
-                                     srslte_pusch_hopping_cfg_t        *hopping_cfg); 
+                                     srslte_pusch_hopping_cfg_t        *hopping_cfg, 
+                                     srslte_ue_ul_powerctrl_t          *power_ctrl); 
 
 SRSLTE_API int srslte_ue_ul_cfg_grant(srslte_ue_ul_t *q, 
                                       srslte_ra_ul_grant_t *grant,
@@ -163,6 +181,11 @@ SRSLTE_API int srslte_ue_ul_pregen_signals(srslte_ue_ul_t *q);
 SRSLTE_API void srslte_ue_ul_set_rnti(srslte_ue_ul_t *q, 
                                       uint16_t rnti);
 
+/* Power control procedure */
+SRSLTE_API float srslte_ue_ul_pusch_power(srslte_ue_ul_t *q, 
+                                          float PL, 
+                                          float p0_preamble);
+
 /* Other static functions for UL PHY procedures defined in 36.213 */
 
 SRSLTE_API int srslte_ue_ul_sr_send_tti(uint32_t I_sr, 
@@ -171,4 +194,5 @@ SRSLTE_API int srslte_ue_ul_sr_send_tti(uint32_t I_sr,
 SRSLTE_API bool srslte_ue_ul_srs_tx_enabled(srslte_refsignal_srs_cfg_t *srs_cfg, 
                                             uint32_t tti); 
 
+
 #endif
diff --git a/srslte/lib/cuhd/src/cuhd_handler.hpp b/srslte/lib/cuhd/src/cuhd_handler.hpp
index 82910edeb..9f0bc5a74 100644
--- a/srslte/lib/cuhd/src/cuhd_handler.hpp
+++ b/srslte/lib/cuhd/src/cuhd_handler.hpp
@@ -42,9 +42,12 @@ public:
   pthread_mutex_t mutex; 
   double cur_rx_gain; 
   double new_rx_gain;   
+  double cur_tx_gain; 
+  double new_tx_gain;   
   bool   tx_gain_same_rx; 
   float  tx_rx_gain_offset; 
-  uhd::gain_range_t rx_gain_range; 
+  uhd::gain_range_t rx_gain_range;
+  uhd::gain_range_t tx_gain_range; 
   size_t rx_nof_samples;
   size_t tx_nof_samples;
   double tx_rate;
diff --git a/srslte/lib/cuhd/src/cuhd_imp.cpp b/srslte/lib/cuhd/src/cuhd_imp.cpp
index 9b30f0580..6dc3d81c8 100644
--- a/srslte/lib/cuhd/src/cuhd_imp.cpp
+++ b/srslte/lib/cuhd/src/cuhd_imp.cpp
@@ -153,6 +153,19 @@ double cuhd_set_rx_gain_th(void *h, double gain)
   return gain; 
 }
 
+double cuhd_set_tx_gain_th(void *h, double gain)
+{
+  cuhd_handler *handler = static_cast < cuhd_handler * >(h);
+  gain = handler->tx_gain_range.clip(gain);     
+  if (gain > handler->new_tx_gain + 0.5 || gain < handler->new_tx_gain - 0.5) {
+    pthread_mutex_lock(&handler->mutex);
+    handler->new_tx_gain = gain; 
+    pthread_cond_signal(&handler->cond);
+    pthread_mutex_unlock(&handler->mutex);
+  }
+  return gain; 
+}
+
 void cuhd_set_tx_rx_gain_offset(void *h, double offset) {
   cuhd_handler *handler = static_cast < cuhd_handler * >(h);
   handler->tx_rx_gain_offset = offset; 
@@ -163,15 +176,22 @@ static void* thread_gain_fcn(void *h) {
   cuhd_handler *handler = static_cast < cuhd_handler * >(h);
   while(1) {
     pthread_mutex_lock(&handler->mutex);
-    while(handler->cur_rx_gain == handler->new_rx_gain) {
+    while(handler->cur_rx_gain == handler->new_rx_gain && 
+          handler->cur_tx_gain == handler->new_tx_gain) 
+    {
       pthread_cond_wait(&handler->cond, &handler->mutex);
     }
-    handler->cur_rx_gain = handler->new_rx_gain; 
-    pthread_mutex_unlock(&handler->mutex);
-    cuhd_set_rx_gain(h, handler->cur_rx_gain);
+    if (handler->new_rx_gain != handler->cur_rx_gain) {
+      handler->cur_rx_gain = handler->new_rx_gain; 
+      cuhd_set_rx_gain(h, handler->cur_rx_gain);
+    }
     if (handler->tx_gain_same_rx) {
       cuhd_set_tx_gain(h, handler->cur_rx_gain+handler->tx_rx_gain_offset);
+    } else if (handler->new_tx_gain != handler->cur_tx_gain) {
+      handler->cur_tx_gain = handler->new_tx_gain; 
+      cuhd_set_tx_gain(h, handler->cur_tx_gain);
     }
+    pthread_mutex_unlock(&handler->mutex);
   }
 }
 
@@ -204,6 +224,7 @@ int cuhd_open_(char *args, void **h, bool create_thread_gain, bool tx_gain_same_
   handler->tx_gain_same_rx = tx_gain_same_rx; 
   handler->tx_rx_gain_offset = 0.0; 
   handler->rx_gain_range = handler->usrp->get_rx_gain_range();
+  handler->tx_gain_range = handler->usrp->get_tx_gain_range();
 
   
   *h = handler;
diff --git a/srslte/lib/phch/src/cqi.c b/srslte/lib/phch/src/cqi.c
index 61d0060f3..d8fde2102 100644
--- a/srslte/lib/phch/src/cqi.c
+++ b/srslte/lib/phch/src/cqi.c
@@ -146,9 +146,9 @@ static float cqi_to_snr_table[15] = { 1.95, 4, 6, 8, 10, 11.95, 14.05, 16, 17.9,
 
 uint8_t srslte_cqi_from_snr(float snr)
 {
- for (uint8_t cqi=14;cqi>=0;cqi--) {
+ for (int cqi=14;cqi>=0;cqi--) {
    if (snr >= cqi_to_snr_table[cqi]) {
-     return cqi+1;
+     return (uint8_t) cqi+1;
    }
  }
  return 0;
diff --git a/srslte/lib/phch/src/sch.c b/srslte/lib/phch/src/sch.c
index 731d06337..5d9bda3a7 100644
--- a/srslte/lib/phch/src/sch.c
+++ b/srslte/lib/phch/src/sch.c
@@ -56,6 +56,14 @@ float beta_cqi_offset[16] = {-1.0, -1.0, 1.125, 1.25, 1.375, 1.625, 1.750, 2.0,
                              3.125, 3.5, 4.0, 5.0, 6.25};
 
 
+float srslte_sch_beta_cqi(uint32_t I_cqi) {
+  if (I_cqi <= 16) {
+    return beta_cqi_offset[I_cqi];
+  } else {
+    return 0;
+  }
+}
+                             
 uint32_t srslte_sch_find_Ioffset_ack(float beta) {
   for (int i=0;i<16;i++) {
     if (beta_harq_offset[i] >= beta) {
@@ -610,7 +618,7 @@ int srslte_ulsch_uci_encode(srslte_sch_t *q,
     }
     Q_prime_ri = (uint32_t) ret; 
   }
-
+  cfg->last_O_cqi = uci_data.uci_cqi_len;
   // Encode CQI
   if (uci_data.uci_cqi_len > 0) {
     ret = srslte_uci_encode_cqi_pusch(&q->uci_cqi, cfg, 
diff --git a/srslte/lib/ue/src/ue_ul.c b/srslte/lib/ue/src/ue_ul.c
index 955fdc6a3..10ffebc99 100644
--- a/srslte/lib/ue/src/ue_ul.c
+++ b/srslte/lib/ue/src/ue_ul.c
@@ -189,7 +189,8 @@ void srslte_ue_ul_set_cfg(srslte_ue_ul_t *q,
                           srslte_pucch_cfg_t                *pucch_cfg, 
                           srslte_pucch_sched_t              *pucch_sched, 
                           srslte_uci_cfg_t                  *uci_cfg,
-                          srslte_pusch_hopping_cfg_t        *hopping_cfg)
+                          srslte_pusch_hopping_cfg_t        *hopping_cfg, 
+                          srslte_ue_ul_powerctrl_t          *power_ctrl)
 {
   srslte_refsignal_ul_set_cfg(&q->signals, dmrs_cfg, pucch_cfg, srs_cfg);
   if (pucch_cfg && dmrs_cfg) {
@@ -207,6 +208,9 @@ void srslte_ue_ul_set_cfg(srslte_ue_ul_t *q,
   if (hopping_cfg) {
     memcpy(&q->hopping_cfg, hopping_cfg, sizeof(srslte_pusch_hopping_cfg_t));
   }
+  if (power_ctrl) {
+    memcpy(&q->power_ctrl, power_ctrl, sizeof(srslte_ue_ul_powerctrl_t));
+  }
 }
 
 int srslte_ue_ul_cfg_grant(srslte_ue_ul_t *q, srslte_ra_ul_grant_t *grant,
@@ -487,6 +491,41 @@ int srslte_ue_ul_pusch_encode_rnti_softbuffer(srslte_ue_ul_t *q,
   return ret;   
 }
 
+
+
+
+/* Returns the transmission power for PUSCH for this subframe */
+float srslte_ue_ul_pusch_power(srslte_ue_ul_t *q, float PL, float p0_preamble) 
+{
+  float p0_pusch, alpha;
+  if (p0_preamble) {
+    p0_pusch = p0_preamble + q->power_ctrl.delta_preamble_msg3;
+    alpha = 1;
+  } else {
+    alpha = q->power_ctrl.alpha;
+    p0_pusch = q->power_ctrl.p0_nominal_pusch+q->power_ctrl.p0_ue_pusch;
+  }
+  float delta=0;
+  if (q->power_ctrl.delta_mcs_based) {
+    float beta_offset_pusch = 1;
+    float MPR = q->pusch_cfg.cb_segm.K1*q->pusch_cfg.cb_segm.C1+q->pusch_cfg.cb_segm.K2*q->pusch_cfg.cb_segm.C2;
+    if (q->pusch_cfg.cb_segm.tbs == 0) {
+      beta_offset_pusch = srslte_sch_beta_cqi(q->pusch_cfg.uci_cfg.I_offset_cqi);
+      MPR = q->pusch_cfg.last_O_cqi;
+    }
+    MPR /= q->pusch_cfg.nbits.nof_re;
+    delta = 10*log10((pow(2,MPR*1.25)-1)*beta_offset_pusch);  
+  }
+  // This implements closed-loop power control
+  float f=0;  
+  
+  float pusch_power = 10*log10(q->pusch_cfg.grant.L_prb)+p0_pusch+alpha*PL+delta+f;
+  DEBUG("P=%f -- 10M=%f, p0=%f,alpha=%f,PL=%f,delta=%f,f=%f\n", pusch_power, 10*log10(q->pusch_cfg.grant.L_prb), p0_pusch, alpha, PL, delta, f);
+  return SRSLTE_MIN(SRSLTE_PC_MAX, pusch_power);
+}
+
+
+
 /* Returns 1 if a SR needs to be sent at current_tti given I_sr, as defined in Section 10.1 of 36.213 */
 int srslte_ue_ul_sr_send_tti(uint32_t I_sr, uint32_t current_tti) {
   uint32_t sr_periodicity; 
diff --git a/srslte/lib/utils/src/vector.c b/srslte/lib/utils/src/vector.c
index 57bfd0fb7..834d1ca40 100644
--- a/srslte/lib/utils/src/vector.c
+++ b/srslte/lib/utils/src/vector.c
@@ -162,15 +162,11 @@ void srslte_vec_sc_prod_fff(float *x, float h, float *z, uint32_t len) {
 
 // TODO: Improve this implementation
 void srslte_vec_norm_cfc(cf_t *x, float amplitude, cf_t *y, uint32_t len) {
-  // Compute peak
-  float max = 0; 
-  float *t = (float*) x;
-  for (int i=0;i<2*len;i++) {
-    if (fabsf(t[i]) > max) {
-      max = fabsf(t[i]);
-    }
-  }
-    
+  // We should use fabs() here but is statistically should be similar
+  float *xp = (float*) x; 
+  uint32_t idx = srslte_vec_max_fi(xp, 2*len);
+  float max = xp[idx]; 
+
   // Normalize before TX 
   srslte_vec_sc_prod_cfc(x, amplitude/max, y, len);
 }