diff --git a/lte/phy/include/liblte/phy/modem/demod_soft.h b/lte/phy/include/liblte/phy/modem/demod_soft.h
index 169af0312..4b182f239 100644
--- a/lte/phy/include/liblte/phy/modem/demod_soft.h
+++ b/lte/phy/include/liblte/phy/modem/demod_soft.h
@@ -46,7 +46,8 @@ typedef struct LIBLTE_API {
   uint32_t max_symbols;
 }demod_soft_t;
 
-LIBLTE_API void demod_soft_init(demod_soft_t *q, uint32_t max_symbols);
+LIBLTE_API int demod_soft_init(demod_soft_t *q, uint32_t max_symbols);
+LIBLTE_API void demod_soft_free(demod_soft_t *q); 
 LIBLTE_API void demod_soft_table_set(demod_soft_t *q, modem_table_t *table);
 LIBLTE_API void demod_soft_alg_set(demod_soft_t *q, enum alg alg_type);
 LIBLTE_API void demod_soft_sigma_set(demod_soft_t *q, float sigma);
diff --git a/lte/phy/lib/modem/src/demod_soft.c b/lte/phy/lib/modem/src/demod_soft.c
index 7c770379a..4b3f6cdce 100644
--- a/lte/phy/lib/modem/src/demod_soft.c
+++ b/lte/phy/lib/modem/src/demod_soft.c
@@ -35,12 +35,40 @@
 #include "soft_algs.h"
 
 
-void demod_soft_init(demod_soft_t *q, uint32_t max_symbols) {
+int demod_soft_init(demod_soft_t *q, uint32_t max_symbols) {
+  int ret = LIBLTE_ERROR; 
+  
   bzero((void*)q,sizeof(demod_soft_t));
   q->sigma = 1.0; 
   q->zones = vec_malloc(sizeof(uint32_t) * max_symbols);
+  if (!q->zones) {
+    perror("malloc");
+    goto clean_exit;
+  }
   q->dd = vec_malloc(sizeof(float*) * max_symbols * 7);
+  if (!q->dd) {
+    perror("malloc");
+    goto clean_exit;
+  }
   q->max_symbols = max_symbols;
+  
+  ret = LIBLTE_SUCCESS;
+  
+clean_exit:
+  if (ret != LIBLTE_SUCCESS) {
+    demod_soft_free(q);
+  }
+  return ret; 
+}
+
+void demod_soft_free(demod_soft_t *q) {
+  if (q->zones) {
+    free(q->zones);
+  }
+  if (q->dd) {
+    free(q->dd);
+  }
+  bzero((void*)q,sizeof(demod_soft_t));
 }
 
 void demod_soft_table_set(demod_soft_t *q, modem_table_t *table) {
diff --git a/lte/phy/lib/modem/test/modem_test.c b/lte/phy/lib/modem/test/modem_test.c
index 3a66b8f6a..45378f107 100644
--- a/lte/phy/lib/modem/test/modem_test.c
+++ b/lte/phy/lib/modem/test/modem_test.c
@@ -151,7 +151,6 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-
   /* generate random data */
   srand(time(NULL));
   for (i=0;i<num_bits;i++) {
@@ -190,6 +189,9 @@ int main(int argc, char **argv) {
   free(input);
 
   modem_table_free(&mod);
+  if (soft_output) {
+    demod_soft_free(&demod_soft);    
+  }
 
   printf("Ok\n");
   exit(0);
diff --git a/lte/phy/lib/modem/test/soft_demod_test.c b/lte/phy/lib/modem/test/soft_demod_test.c
index d500cee44..065ff6ccd 100644
--- a/lte/phy/lib/modem/test/soft_demod_test.c
+++ b/lte/phy/lib/modem/test/soft_demod_test.c
@@ -222,6 +222,7 @@ clean_exit:
   free(input);
 
   modem_table_free(&mod);
+  demod_soft_free(&demod_soft);
 
   if (ret == 0) {
     printf("Ok Mean Throughput: %.2f. Mbps ExTime: %.2f us\n", num_bits/mean_texec, mean_texec);    
diff --git a/lte/phy/lib/phch/src/pbch.c b/lte/phy/lib/phch/src/pbch.c
index ecde15ab4..4e0965e61 100644
--- a/lte/phy/lib/phch/src/pbch.c
+++ b/lte/phy/lib/phch/src/pbch.c
@@ -235,6 +235,7 @@ void pbch_free(pbch_t *q) {
   sequence_free(&q->seq_pbch);
   modem_table_free(&q->mod);
   viterbi_free(&q->decoder);
+  demod_soft_free(&q->demod);
 
   bzero(q, sizeof(pbch_t));
 
diff --git a/lte/phy/lib/phch/src/pcfich.c b/lte/phy/lib/phch/src/pcfich.c
index 2ad301194..1d9a37954 100644
--- a/lte/phy/lib/phch/src/pcfich.c
+++ b/lte/phy/lib/phch/src/pcfich.c
@@ -116,6 +116,7 @@ void pcfich_free(pcfich_t *q) {
   }
   modem_table_free(&q->mod);
   precoding_free(&q->precoding); 
+  demod_soft_free(&q->demod);
 
   bzero(q, sizeof(pcfich_t));
 }
diff --git a/lte/phy/lib/phch/src/pdcch.c b/lte/phy/lib/phch/src/pdcch.c
index 4d18828c7..7ad32ebf6 100644
--- a/lte/phy/lib/phch/src/pdcch.c
+++ b/lte/phy/lib/phch/src/pdcch.c
@@ -171,6 +171,8 @@ void pdcch_free(pdcch_t *q) {
     sequence_free(&q->seq_pdcch[i]);
   }
 
+  demod_soft_free(&q->demod);
+
   precoding_free(&q->precoding);
   modem_table_free(&q->mod);
   viterbi_free(&q->decoder);
diff --git a/lte/phy/lib/phch/src/pdsch.c b/lte/phy/lib/phch/src/pdsch.c
index f0138edb7..226d3ae3a 100644
--- a/lte/phy/lib/phch/src/pdsch.c
+++ b/lte/phy/lib/phch/src/pdsch.c
@@ -53,7 +53,7 @@ const lte_mod_t modulations[4] =
 
 #ifdef DEBUG_IDX    
 cf_t *offset_original=NULL;
-extern int indices[2048];
+extern int indices[100000];
 extern int indices_ptr; 
 #endif
 
@@ -326,6 +326,7 @@ void pdsch_free(pdsch_t *q) {
   for (i = 0; i < 4; i++) {
     modem_table_free(&q->mod[i]);
   }
+  demod_soft_free(&q->demod);
   tdec_free(&q->decoder);
   tcod_free(&q->encoder);
   precoding_free(&q->precoding);
@@ -380,7 +381,7 @@ static int codeblock_segmentation(struct cb_segm *s, uint32_t tbs) {
         }
         s->F = s->C1 * s->K1 + s->C2 * s->K2 - Bp;
         INFO("CB Segmentation: TBS: %d, C=%d, C+=%d K+=%d, C-=%d, K-=%d, F=%d, Bp=%d\n",
-            tbs, s->C, s->C1, s->K1, s->C2, s->K2, s->F, Bp); 
+            tbs, s->C, s->C1, s->K1, s->C2, s->K2, s->F, Bp);         
       }
     }
   }  
@@ -536,16 +537,21 @@ int pdsch_decode_tb(pdsch_t *q, uint8_t *data, uint32_t tbs, uint32_t nb_e,
   uint32_t i;
   uint32_t cb_len, rp, wp, rlen, F, n_e;
   float *e_bits = q->pdsch_e;
+  uint32_t Qm = lte_mod_bits_x_symbol(harq_process->mcs.mod);
   
-  if (q         != NULL   && 
-      data      != NULL   &&       
-      nb_e      < q->max_symbols * lte_mod_bits_x_symbol(LTE_QAM64))
+  if (q            != NULL   && 
+      data         != NULL   &&       
+      harq_process != NULL   &&
+      nb_e      < q->max_symbols * Qm)
   {
 
     rp = 0;
     rp = 0;
     wp = 0;
-    for (i = 0; i < harq_process->cb_segm.C; i++) {
+    uint32_t Gp = nb_e / Qm;
+    uint32_t gamma = Gp%harq_process->cb_segm.C;
+    bool early_stop = true;
+    for (i = 0; i < harq_process->cb_segm.C && early_stop; i++) {
 
       /* Get read/write lengths */
       if (i < harq_process->cb_segm.C - harq_process->cb_segm.C2) {
@@ -564,13 +570,13 @@ int pdsch_decode_tb(pdsch_t *q, uint8_t *data, uint32_t tbs, uint32_t nb_e,
         F = 0;
       }
 
-      if (i < harq_process->cb_segm.C - 1) {
-        n_e = nb_e / harq_process->cb_segm.C;
+      if (i <= harq_process->cb_segm.C - gamma - 1) {
+        n_e = Qm * (Gp/harq_process->cb_segm.C);
       } else {
-        n_e = nb_e - rp;
+        n_e = Qm * ((uint32_t) ceilf((float) Gp/harq_process->cb_segm.C));
       }
 
-      DEBUG("CB#%d: cb_len: %d, rlen: %d, wp: %d, rp: %d, F: %d, E: %d\n", i,
+      INFO("CB#%d: cb_len: %d, rlen: %d, wp: %d, rp: %d, F: %d, E: %d\n", i,
           cb_len, rlen - F, wp, rp, F, n_e);
 
       
@@ -584,10 +590,11 @@ int pdsch_decode_tb(pdsch_t *q, uint8_t *data, uint32_t tbs, uint32_t nb_e,
       
       /* Turbo Decoding with CRC-based early stopping */
       q->nof_iterations = 0; 
-      bool early_stop = false;
       uint32_t len_crc; 
       uint8_t *cb_in_ptr; 
       crc_t *crc_ptr; 
+      early_stop = false; 
+      
       tdec_reset(&q->decoder, cb_len);
             
       do {
@@ -615,6 +622,7 @@ int pdsch_decode_tb(pdsch_t *q, uint8_t *data, uint32_t tbs, uint32_t nb_e,
       } while (q->nof_iterations < TDEC_MAX_ITERATIONS && !early_stop);
       q->average_nof_iterations = VEC_EMA((float) q->nof_iterations, q->average_nof_iterations, 0.2);
       
+      // If CB CRC is not correct, early_stop will be false and wont continue with rest of CBs
 
       /* Copy data to another buffer, removing the Codeblock CRC */
       if (i < harq_process->cb_segm.C - 1) {
@@ -633,24 +641,30 @@ int pdsch_decode_tb(pdsch_t *q, uint8_t *data, uint32_t tbs, uint32_t nb_e,
       rp += n_e;
     }
 
-    DEBUG("END CB#%d: wp: %d, rp: %d\n", i, wp, rp);
-
-    // Compute transport block CRC
-    par_rx = crc_checksum(&q->crc_tb, data, tbs);
-
-    // check parity bits
-    par_tx = bit_unpack(&p_parity, 24);
-
-    if (!par_rx) {
-      INFO("\n\tCAUTION!! Received all-zero transport block\n\n", 0);
-    }
-
-    if (par_rx == par_tx) {
-      INFO("TB decoded OK\n",i);
-      return LIBLTE_SUCCESS;
+    if (!early_stop) {
+      INFO("CB %d failed. TB is erroneous.\n",i-1);
+      return LIBLTE_ERROR; 
     } else {
-      INFO("Error in TB parity\n",i);
-      return LIBLTE_ERROR;
+      INFO("END CB#%d: wp: %d, rp: %d\n", i, wp, rp);
+
+      // Compute transport block CRC
+      par_rx = crc_checksum(&q->crc_tb, data, tbs);
+
+      // check parity bits
+      par_tx = bit_unpack(&p_parity, 24);
+
+      if (!par_rx) {
+        INFO("\n\tCAUTION!! Received all-zero transport block\n\n", 0);
+      }
+
+      if (par_rx == par_tx) {
+        INFO("TB decoded OK\n",i);
+        return LIBLTE_SUCCESS;
+      } else {
+        INFO("Error in TB parity\n",i);
+        return LIBLTE_ERROR;
+      }
+      
     }
   } else {
     return LIBLTE_ERROR_INVALID_INPUTS;
@@ -705,6 +719,7 @@ int pdsch_decode(pdsch_t *q, cf_t *sf_symbols, cf_t *ce[MAX_PORTS], float noise_
           return LIBLTE_ERROR;
         }
       }
+      
 
       /* TODO: only diversity is supported */
       if (q->cell.nof_ports == 1) {
@@ -753,12 +768,17 @@ int pdsch_encode_tb(pdsch_t *q, uint8_t *data, uint32_t tbs, uint32_t nb_e,
   uint32_t cb_len, rp, wp, rlen, F, n_e;
   uint8_t *e_bits = q->pdsch_e;
   int ret = LIBLTE_ERROR_INVALID_INPUTS; 
+  uint32_t Qm = lte_mod_bits_x_symbol(harq_process->mcs.mod);
   
   if (q             != NULL &&
       data          != NULL &&
-      nb_e          <  q->max_symbols * lte_mod_bits_x_symbol(LTE_QAM64))
+      harq_process  != NULL && 
+      nb_e          <  q->max_symbols * Qm)
   {
   
+    uint32_t Gp = nb_e / Qm;
+    uint32_t gamma = Gp%harq_process->cb_segm.C;
+
     if (q->rnti_is_set) {
       if (rv_idx == 0) {
         /* Compute transport block CRC */
@@ -800,11 +820,10 @@ int pdsch_encode_tb(pdsch_t *q, uint8_t *data, uint32_t tbs, uint32_t nb_e,
         } else {
           F = 0;
         }
-
-        if (i < harq_process->cb_segm.C - 1) {
-          n_e = nb_e / harq_process->cb_segm.C;
+        if (i <= harq_process->cb_segm.C - gamma - 1) {
+          n_e = Qm * (Gp/harq_process->cb_segm.C);
         } else {
-          n_e = nb_e - wp;
+          n_e = Qm * ((uint32_t) ceilf((float) Gp/harq_process->cb_segm.C));
         }
 
         INFO("CB#%d: cb_len: %d, rlen: %d, wp: %d, rp: %d, F: %d, E: %d\n", i,
diff --git a/lte/phy/lib/phch/src/prb.c b/lte/phy/lib/phch/src/prb.c
index b395fe973..85414d2e9 100644
--- a/lte/phy/lib/phch/src/prb.c
+++ b/lte/phy/lib/phch/src/prb.c
@@ -36,14 +36,14 @@
 
 #ifdef DEBUG_IDX
 extern cf_t *offset_original;
-LIBLTE_API int indices[2048];
+LIBLTE_API int indices[100000];
 LIBLTE_API int indices_ptr=0;
 #endif
 
 void print_indexes(cf_t *offset, int len) {
 #ifdef DEBUG_IDX
   for (int i=0;i<len;i++) {
-    indices[(i+indices_ptr)%2048]=offset-offset_original+i;
+    indices[(i+indices_ptr)%100000]=offset-offset_original+i;
   }
   indices_ptr+=len;
 #endif
diff --git a/lte/phy/lib/phch/test/CMakeLists.txt b/lte/phy/lib/phch/test/CMakeLists.txt
index 7a5c2de0c..82d729998 100644
--- a/lte/phy/lib/phch/test/CMakeLists.txt
+++ b/lte/phy/lib/phch/test/CMakeLists.txt
@@ -95,8 +95,8 @@ TARGET_LINK_LIBRARIES(pdsch_test lte_phy)
 
 ADD_TEST(pdsch_test_bpsk pdsch_test -l 500 -m 1 -n 50 -r 2)
 ADD_TEST(pdsch_test_qpsk pdsch_test -l 1000 -m 2 -n 50 -r 1)
-ADD_TEST(pdsch_test_qam16 pdsch_test -l 50000 -m 4 -n 110)
-ADD_TEST(pdsch_test_qam64 pdsch_test -l 5000 -m 6 -n 50 -r 0)
+ADD_TEST(pdsch_test_qam16 pdsch_test -l 50000 -m 4 -n 100)
+ADD_TEST(pdsch_test_qam64 pdsch_test -l 61664 -m 6 -n 100 -r 0)
 
 BuildMex(MEXNAME pdsch SOURCES pdsch_test_mex.c LIBRARIES lte_phy liblte_mex)
 
diff --git a/lte/phy/lib/phch/test/pdsch_test.c b/lte/phy/lib/phch/test/pdsch_test.c
index 68b33134e..3b82c2a95 100644
--- a/lte/phy/lib/phch/test/pdsch_test.c
+++ b/lte/phy/lib/phch/test/pdsch_test.c
@@ -37,13 +37,13 @@
 lte_cell_t cell = {
   6,            // nof_prb
   1,            // nof_ports
-  1,            // cell_id
+  0,            // cell_id
   CPNORM,       // cyclic prefix
-  R_1,          // PHICH resources      
+  R_1_6,          // PHICH resources      
   PHICH_NORM    // PHICH length
 };
 
-uint32_t cfi = 1;
+uint32_t cfi = 2;
 uint32_t tbs = 0;
 uint32_t subframe = 1;
 lte_mod_t modulation = LTE_BPSK;
@@ -140,12 +140,12 @@ int main(int argc, char **argv) {
   
   prb_alloc.slot[0].nof_prb = cell.nof_prb;
   for (i=0;i<prb_alloc.slot[0].nof_prb;i++) {
-    prb_alloc.slot[0].prb_idx[i] = i;
+    prb_alloc.slot[0].prb_idx[i] = true;
   }
   memcpy(&prb_alloc.slot[1], &prb_alloc.slot[0], sizeof(ra_prb_slot_t));
 
-  ra_prb_get_re_dl(&prb_alloc, cell.nof_prb, cell.nof_ports, cell.nof_prb<10?(cfi+1):cfi, CPNORM);
-
+  ra_prb_get_re_dl(&prb_alloc, cell.nof_prb, cell.nof_ports, cell.nof_prb<10?(cfi+1):cfi, cell.cp);
+  
   /* init memory */
   for (i=0;i<cell.nof_ports;i++) {
     ce[i] = malloc(sizeof(cf_t) * nof_re);
@@ -223,7 +223,8 @@ int main(int argc, char **argv) {
   ret = 0;
 quit:
   pdsch_free(&pdsch);
-
+  pdsch_harq_free(&harq_process);
+  
   for (i=0;i<cell.nof_ports;i++) {
     if (ce[i]) {
       free(ce[i]);
diff --git a/lte/phy/lib/phch/test/pdsch_test_mex.c b/lte/phy/lib/phch/test/pdsch_test_mex.c
index fb906cc64..6def9c865 100644
--- a/lte/phy/lib/phch/test/pdsch_test_mex.c
+++ b/lte/phy/lib/phch/test/pdsch_test_mex.c
@@ -44,7 +44,7 @@ void help()
     ("[decoded_ok, llr, rm, bits, symbols] = liblte_pdsch(enbConfig, pdschConfig, trblklen, rxWaveform)\n\n");
 }
 
-//extern int indices[2048];
+extern int indices[2048];
 
 /* the gateway function */
 void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
@@ -161,14 +161,12 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   free(prbset);
   
   ra_prb_get_re_dl(&prb_alloc, cell.nof_prb, cell.nof_ports, cell.nof_prb<10?(cfi+1):cfi, cell.cp);
-
+  
   if (pdsch_harq_setup(&harq_process, mcs, &prb_alloc)) {
     mexErrMsgTxt("Error configuring HARQ process\n");
     return;
   }
 
-  
-  //mexPrintf("C: %d, K1: %d, K2: %d\n", harq_process.cb_segm.C, harq_process.cb_segm.K1, harq_process.cb_segm.K2);
   /** Allocate input buffers */
   if (mexutils_read_cf(INPUT, &input_signal) < 0) {
     mexErrMsgTxt("Error reading input signal\n");
@@ -227,7 +225,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     mexutils_write_cf(pdsch.pdsch_d, &plhs[3], harq_process.prb_alloc.re_sf[sf_idx], 1);  
   }
   if (nlhs >= 5) {
-    mexutils_write_f(pdsch.pdsch_e, &plhs[4], harq_process.prb_alloc.re_sf[sf_idx]*lte_mod_bits_x_symbol(mcs.mod), 1);  
+    mexutils_write_f(pdsch.pdsch_e, &plhs[4], harq_process.prb_alloc.re_sf[sf_idx] * lte_mod_bits_x_symbol(mcs.mod), 1);  
   }
   
   chest_dl_free(&chest);
diff --git a/matlab/tests/pdsch_bler.m b/matlab/tests/pdsch_bler.m
index 3e295a8cd..2f9c5c62d 100644
--- a/matlab/tests/pdsch_bler.m
+++ b/matlab/tests/pdsch_bler.m
@@ -6,11 +6,11 @@
 
 recordedSignal=[];
 
-Npackets = 4;
-SNR_values = linspace(10,20,4);
+Npackets = 1;
+SNR_values = linspace(15,20,4);
 
 %% Choose RMC 
-[waveform,rgrid,rmccFgOut] = lteRMCDLTool('R.7',[1;0;0;1]);
+[waveform,rgrid,rmccFgOut] = lteRMCDLTool('R.9',[1;0;0;1]);
 waveform = sum(waveform,2);
 
 if ~isempty(recordedSignal)
@@ -78,8 +78,7 @@ for snr_idx=1:length(SNR_values)
         %% Demodulate 
         frame_rx = lteOFDMDemodulate(rmccFgOut, rxWaveform);
 
-        %for sf_idx=0:Nsf
-        sf_idx=1;
+        for sf_idx=0:Nsf
             subframe_waveform = rxWaveform(sf_idx*flen+1:(sf_idx+1)*flen);
             subframe_rx=frame_rx(:,sf_idx*14+1:(sf_idx+1)*14);
             rmccFgOut.NSubframe=sf_idx;
@@ -88,7 +87,7 @@ for snr_idx=1:length(SNR_values)
             % Perform channel estimation
             [hest, nest] = lteDLChannelEstimate(rmccFgOut, cec, subframe_rx);
 
-            [cws,symbols] = ltePDSCHDecode(rmccFgOut,rmccFgOut.PDSCH,subframe_rx,hest,nest);
+            [cws,symbols,pdschSymbols,hestCH,indices] = ltePDSCHDecode2(rmccFgOut,rmccFgOut.PDSCH,subframe_rx,hest,nest);
             [trblkout,blkcrc,dstate] = lteDLSCHDecode(rmccFgOut,rmccFgOut.PDSCH, ... 
                                                     rmccFgOut.PDSCH.TrBlkSizes(sf_idx+1),cws);
 
@@ -104,7 +103,7 @@ for snr_idx=1:length(SNR_values)
                 dec2 = 1;
             end
             decoded_liblte(snr_idx) = decoded_liblte(snr_idx)+dec2;
-        %end
+        end
 
         if ~isempty(recordedSignal)
             recordedSignal = recordedSignal(flen*10+1:end);