diff --git a/lib/include/srslte/phy/mimo/layermap.h b/lib/include/srslte/phy/mimo/layermap.h
index d73807124..3f8e9329c 100644
--- a/lib/include/srslte/phy/mimo/layermap.h
+++ b/lib/include/srslte/phy/mimo/layermap.h
@@ -77,6 +77,6 @@ SRSLTE_API int srslte_layerdemap_type(cf_t*              x[SRSLTE_MAX_LAYERS],
 
 SRSLTE_API int srslte_layermap_nr(cf_t** d, int nof_cw, cf_t** x, int nof_layers, uint32_t nof_re);
 
-SRSLTE_API int srslte_layerdemap_nr(cf_t** d, int nof_cw, const cf_t** x, int nof_layers, uint32_t nof_re);
+SRSLTE_API int srslte_layerdemap_nr(cf_t** d, int nof_cw, cf_t** x, int nof_layers, uint32_t nof_re);
 
 #endif // SRSLTE_LAYERMAP_H
diff --git a/lib/include/srslte/phy/phch/sch_cfg_nr.h b/lib/include/srslte/phy/phch/sch_cfg_nr.h
index f7b56dd71..089ac7b6d 100644
--- a/lib/include/srslte/phy/phch/sch_cfg_nr.h
+++ b/lib/include/srslte/phy/phch/sch_cfg_nr.h
@@ -41,6 +41,7 @@ typedef struct SRSLTE_API {
   int          tbs; ///< Payload size, TS 38.212 refers to it as A
   double       R;   ///< Target LDPC rate
   int          rv;
+  uint32_t     nof_re;   ///< Number of available resource elements to send, known as N_RE
   uint32_t     nof_bits; ///< Number of available bits to send, known as G
   uint32_t     cw_idx;
   bool         enabled;
diff --git a/lib/src/phy/mimo/layermap.c b/lib/src/phy/mimo/layermap.c
index b3f72102c..f41a5f3ac 100644
--- a/lib/src/phy/mimo/layermap.c
+++ b/lib/src/phy/mimo/layermap.c
@@ -262,7 +262,7 @@ int srslte_layermap_nr(cf_t** d, int nof_cw, cf_t** x, int nof_layers, uint32_t
   return SRSLTE_ERROR;
 }
 
-int srslte_layerdemap_nr(cf_t** d, int nof_cw, const cf_t** x, int nof_layers, uint32_t nof_re)
+int srslte_layerdemap_nr(cf_t** d, int nof_cw, cf_t** x, int nof_layers, uint32_t nof_re)
 {
   if (nof_cw == 1 && nof_layers > 0 && nof_layers < 5) {
     for (uint32_t i = 0; i < nof_re / nof_layers; i++) {
diff --git a/lib/src/phy/phch/pdsch_nr.c b/lib/src/phy/phch/pdsch_nr.c
index 29b1fafa4..889d61808 100644
--- a/lib/src/phy/phch/pdsch_nr.c
+++ b/lib/src/phy/phch/pdsch_nr.c
@@ -20,6 +20,7 @@
  */
 #include "srslte/phy/phch/pdsch_nr.h"
 #include "srslte/phy/common/phy_common_nr.h"
+#include "srslte/phy/phch/ra_nr.h"
 
 int pdsch_nr_init_common(srslte_pdsch_nr_t* q)
 {
@@ -64,6 +65,7 @@ int srslte_pdsch_nr_init_rx(srslte_pdsch_nr_t* q)
   }
 
   srslte_sch_nr_decoder_cfg_t decoder_cfg = {};
+  decoder_cfg.disable_simd                = true;
   if (srslte_sch_nr_init_rx(&q->sch, &decoder_cfg)) {
     ERROR("Initialising SCH\n");
     return SRSLTE_ERROR;
@@ -319,7 +321,12 @@ static int srslte_pdsch_nr_cp(const srslte_pdsch_nr_t*       q,
     return SRSLTE_ERROR;
   }
 
-  for (uint32_t l = grant->S; l < grant->L; l++) {
+  if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_INFO && !handler_registered) {
+    printf("dmrs_l_idx=");
+    srslte_vec_fprint_i(stdout, (int32_t*)dmrs_l_idx, nof_dmrs_symbols);
+  }
+
+  for (uint32_t l = grant->S; l < grant->S + grant->L; l++) {
     // Advance DMRS symbol counter until:
     // - the current DMRS symbol index is greater or equal than current symbol l
     // - no more DMRS symbols
@@ -386,6 +393,11 @@ static inline int pdsch_nr_encode_codeword(srslte_pdsch_nr_t*           q,
     return SRSLTE_ERROR;
   }
 
+  if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_INFO && !handler_registered) {
+    printf("b=");
+    srslte_vec_fprint_b(stdout, q->b[tb->cw_idx], tb->nof_bits);
+  }
+
   // 7.3.1.1 Scrambling
   uint32_t n_id = q->carrier.id;
   if (cfg->scrambling_id_present && SRSLTE_RNTI_ISUSER(rnti)) {
@@ -397,6 +409,11 @@ static inline int pdsch_nr_encode_codeword(srslte_pdsch_nr_t*           q,
   // 7.3.1.2 Modulation
   srslte_mod_modulate(&q->modem_tables[tb->mod], q->b[tb->cw_idx], q->d[tb->cw_idx], tb->nof_bits);
 
+  if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_INFO && !handler_registered) {
+    printf("d=");
+    srslte_vec_fprint_c(stdout, q->d[tb->cw_idx], tb->nof_re);
+  }
+
   return SRSLTE_SUCCESS;
 }
 
@@ -471,12 +488,20 @@ static inline int pdsch_nr_decode_codeword(srslte_pdsch_nr_t*           q,
     return SRSLTE_ERROR_OUT_OF_BOUNDS;
   }
 
+  if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_INFO && !handler_registered) {
+    printf("d=");
+    srslte_vec_fprint_c(stdout, q->d[tb->cw_idx], tb->nof_re);
+  }
+
   // Demodulation
-  cf_t**  d   = (tb->N_L > 1) ? q->d : q->x;
-  int8_t* llr = (int8_t*)q->b;
-  if (srslte_demod_soft_demodulate_b(tb->mod, d[tb->cw_idx], llr, tb->nof_bits)) {
+  int8_t* llr = (int8_t*)q->b[tb->cw_idx];
+  if (srslte_demod_soft_demodulate_b(tb->mod, q->d[tb->cw_idx], llr, tb->nof_re)) {
     return SRSLTE_ERROR;
   }
+  // Change LLR sign
+  for (uint32_t i = 0; i < tb->nof_bits; i++) {
+    llr[i] = -llr[i];
+  }
 
   // Descrambling
   uint32_t n_id = q->carrier.id;
@@ -486,6 +511,11 @@ static inline int pdsch_nr_decode_codeword(srslte_pdsch_nr_t*           q,
   uint32_t cinit = ((uint32_t)rnti << 15U) + (tb->cw_idx << 14U) + n_id;
   srslte_sequence_apply_c(llr, llr, tb->nof_bits, cinit);
 
+  if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_INFO && !handler_registered) {
+    printf("b=");
+    srslte_vec_fprint_b(stdout, q->b[tb->cw_idx], tb->nof_bits);
+  }
+
   // Decode SCH
   if (srslte_dlsch_nr_decode(&q->sch, &cfg->sch_cfg, tb, llr, res->payload, &res->crc) < SRSLTE_SUCCESS) {
     ERROR("Error in DL-SCH encoding\n");
@@ -507,8 +537,15 @@ int srslte_pdsch_nr_decode(srslte_pdsch_nr_t*             q,
     nof_cw += grant->tb[tb].enabled ? 1 : 0;
   }
 
+  uint32_t nof_re = srslte_ra_dl_nr_slot_nof_re(cfg, grant);
+
   // Demapping from virtual to physical resource blocks
-  srslte_pdsch_nr_get(q, cfg, grant, q->x[0], sf_symbols[0]);
+  cf_t**   x          = (grant->nof_layers > 1) ? q->x : q->d;
+  uint32_t nof_re_get = srslte_pdsch_nr_get(q, cfg, grant, x[0], sf_symbols[0]);
+  if (nof_re_get != nof_re) {
+    ERROR("Inconsistent number of RE (%d!=%d)\n", nof_re_get, nof_re);
+    return SRSLTE_ERROR;
+  }
 
   // Demapping to virtual resource blocks
   // ... Not implemented
@@ -518,7 +555,7 @@ int srslte_pdsch_nr_decode(srslte_pdsch_nr_t*             q,
 
   // Layer demapping
   if (grant->nof_layers > 1) {
-    srslte_layermap_nr(q->d, nof_cw, q->x, grant->nof_layers, grant->nof_layers);
+    srslte_layerdemap_nr(q->d, nof_cw, q->x, grant->nof_layers, nof_re);
   }
 
   // SCH decode
@@ -532,4 +569,4 @@ int srslte_pdsch_nr_decode(srslte_pdsch_nr_t*             q,
   }
 
   return SRSLTE_SUCCESS;
-}
\ No newline at end of file
+}
diff --git a/lib/src/phy/phch/ra_nr.c b/lib/src/phy/phch/ra_nr.c
index 99afc22d2..5f55caf3a 100644
--- a/lib/src/phy/phch/ra_nr.c
+++ b/lib/src/phy/phch/ra_nr.c
@@ -381,7 +381,8 @@ int srslte_ra_nr_fill_tb(const srslte_pdsch_cfg_nr_t*   pdsch_cfg,
   tb->tbs      = (int)srslte_ra_nr_tbs(N_re, S, R, Qm, tb->N_L);
   tb->R        = R;
   tb->mod      = m;
-  tb->nof_bits = N_re * Qm * grant->nof_layers;
+  tb->nof_re   = N_re * grant->nof_layers;
+  tb->nof_bits = tb->nof_re * Qm;
   tb->enabled  = true;
 
   return SRSLTE_SUCCESS;
diff --git a/lib/src/phy/phch/sch_nr.c b/lib/src/phy/phch/sch_nr.c
index 747dfc6fb..b3cbf69de 100644
--- a/lib/src/phy/phch/sch_nr.c
+++ b/lib/src/phy/phch/sch_nr.c
@@ -405,7 +405,10 @@ int srslte_dlsch_nr_encode(srslte_sch_nr_t*        q,
 
   // Calculate TB CRC
   uint32_t checksum_tb = srslte_crc_checksum_byte(cfg.crc_tb, data, tb->tbs);
-  // printf(" Encode: "); srslte_vec_fprint_byte(stdout, data, tb->tbs / 8);
+  if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_INFO && !handler_registered) {
+    printf("Encode: ");
+    srslte_vec_fprint_byte(stdout, data, tb->tbs / 8);
+  }
 
   // For each code block...
   uint32_t j = 0;
@@ -423,23 +426,30 @@ int srslte_dlsch_nr_encode(srslte_sch_nr_t*        q,
 
       // If it is the last segment...
       if (r == cfg.C - 1) {
+        cb_len -= cfg.L_tb;
+
         // Copy payload without TB CRC
-        srslte_bit_unpack_vector(input_ptr, q->temp_cb, (int)(cb_len - cfg.L_tb));
+        srslte_bit_unpack_vector(input_ptr, q->temp_cb, (int)cb_len);
 
         // Append TB CRC
-        uint8_t* ptr = &q->temp_cb[cfg.Kp - cfg.L_cb - cfg.L_tb];
+        uint8_t* ptr = &q->temp_cb[cb_len];
         srslte_bit_unpack(checksum_tb, &ptr, cfg.L_tb);
         INFO("CB %d: appending TB CRC=%06x\n", r, checksum_tb);
       } else {
         // Copy payload
         srslte_bit_unpack_vector(input_ptr, q->temp_cb, (int)cb_len);
       }
-      //      printf("CB %d:", r); srslte_vec_fprint_byte(stdout, input_ptr, cb_len / 8);
+
+      if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_INFO && !handler_registered) {
+        printf("CB %d:", r);
+        srslte_vec_fprint_byte(stdout, input_ptr, cb_len / 8);
+      }
+
       input_ptr += cb_len / 8;
 
       // Attach code block CRC if required
       if (cfg.L_cb) {
-        srslte_crc_attach(&q->crc_cb, q->temp_cb, (int)cb_len);
+        srslte_crc_attach(&q->crc_cb, q->temp_cb, (int)(cfg.Kp - cfg.L_cb));
         INFO("CB %d: CRC=%06x\n", r, (uint32_t)srslte_crc_checksum_get(&q->crc_cb));
       }
 
@@ -604,7 +614,11 @@ int srslte_dlsch_nr_decode(srslte_sch_nr_t*        q,
 
       srslte_vec_u8_copy(output_ptr, tb->softbuffer.rx->data[r], cb_len / 8);
       output_ptr += cb_len / 8;
-      //      printf("CB %d:", r); srslte_vec_fprint_byte(stdout, tb->softbuffer.rx->data[r], cb_len / 8);
+
+      if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_INFO && !handler_registered) {
+        printf("CB %d:", r);
+        srslte_vec_fprint_byte(stdout, tb->softbuffer.rx->data[r], cb_len / 8);
+      }
 
       if (r == cfg.C - 1) {
         uint8_t  tb_crc_unpacked[24] = {};
@@ -619,7 +633,10 @@ int srslte_dlsch_nr_decode(srslte_sch_nr_t*        q,
     *crc_ok            = (checksum1 == checksum2);
 
     INFO("TB: TBS=%d; CRC={%06x, %06x}\n", tb->tbs, checksum1, checksum2);
-    //    printf("Decoded: "); srslte_vec_fprint_byte(stdout, data, tb->tbs / 8);
+    if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_INFO && !handler_registered) {
+      printf("Decode: ");
+      srslte_vec_fprint_byte(stdout, data, tb->tbs / 8);
+    }
   } else {
     *crc_ok = false;
   }
diff --git a/lib/src/phy/phch/test/pdsch_nr_test.c b/lib/src/phy/phch/test/pdsch_nr_test.c
index 9d6c7fce4..f2b3ea29b 100644
--- a/lib/src/phy/phch/test/pdsch_nr_test.c
+++ b/lib/src/phy/phch/test/pdsch_nr_test.c
@@ -23,9 +23,9 @@
 #include "srslte/phy/phch/ra_nr.h"
 #include "srslte/phy/ue/ue_dl_nr_data.h"
 #include "srslte/phy/utils/debug.h"
+#include "srslte/phy/utils/random.h"
 #include "srslte/phy/utils/vector.h"
 #include <getopt.h>
-#include <srslte/phy/utils/random.h>
 
 static srslte_carrier_nr_t carrier = {
     0,                 // cell_id
@@ -217,12 +217,33 @@ int main(int argc, char** argv)
         goto clean_exit;
       }
 
+      float    mse    = 0.0f;
+      uint32_t nof_re = srslte_ra_dl_nr_slot_nof_re(&pdsch_cfg, &pdsch_grant);
+      for (uint32_t i = 0; i < pdsch_grant.nof_layers; i++) {
+        for (uint32_t j = 0; j < nof_re; j++) {
+          mse += cabsf(pdsch_tx.d[i][j] - pdsch_rx.d[i][j]);
+        }
+      }
+      if (nof_re * pdsch_grant.nof_layers > 0) {
+        mse = mse / (nof_re * pdsch_grant.nof_layers);
+      }
+      if (mse > 0.001) {
+        ERROR("MSE error (%f) is too high\n", mse);
+        for (uint32_t i = 0; i < pdsch_grant.nof_layers; i++) {
+          printf("d_tx[%d]=", i);
+          srslte_vec_fprint_c(stdout, pdsch_tx.d[i], nof_re);
+          printf("d_rx[%d]=", i);
+          srslte_vec_fprint_c(stdout, pdsch_rx.d[i], nof_re);
+        }
+        goto clean_exit;
+      }
+
       if (!pdsch_res[0].crc) {
         ERROR("Failed to match CRC; n_prb=%d; mcs=%d; TBS=%d;\n", n_prb, mcs, pdsch_grant.tb[0].tbs);
         goto clean_exit;
       }
 
-      if (memcmp(data_tx, data_rx, pdsch_grant.tb[0].tbs / 8) != 0) {
+      if (memcmp(data_tx[0], data_rx[0], pdsch_grant.tb[0].tbs / 8) != 0) {
         ERROR("Failed to match Tx/Rx data; n_prb=%d; mcs=%d; TBS=%d;\n", n_prb, mcs, pdsch_grant.tb[0].tbs);
         printf("Tx data: ");
         srslte_vec_fprint_byte(stdout, data_tx[0], pdsch_grant.tb[0].tbs / 8);
@@ -257,5 +278,5 @@ clean_exit:
   srslte_softbuffer_tx_free(&softbuffer_tx);
   srslte_softbuffer_rx_free(&softbuffer_rx);
 
-  return SRSLTE_SUCCESS;
+  return ret;
 }