diff --git a/lib/include/srslte/common/pdu.h b/lib/include/srslte/common/pdu.h
index 5d37a0d63..20fe91816 100644
--- a/lib/include/srslte/common/pdu.h
+++ b/lib/include/srslte/common/pdu.h
@@ -133,11 +133,16 @@ public:
   // Section 6.1.2
   void parse_packet(uint8_t *ptr) {
     uint8_t *init_ptr = ptr; 
-    nof_subheaders = 0; 
-    while(subheaders[nof_subheaders].read_subheader(&ptr)) {
+    nof_subheaders = 0;
+    bool ret = false;
+    do {
+      if (nof_subheaders < (int) max_subheaders) {
+        ret = subheaders[nof_subheaders].read_subheader(&ptr);
+      }
+    } while (ret && nof_subheaders < (int) max_subheaders);
+    if (nof_subheaders + 1 < (int) max_subheaders) {
       nof_subheaders++;
     }
-    nof_subheaders++;
     for (int i=0;i<nof_subheaders;i++) {
       subheaders[i].read_payload(&ptr);
     }
diff --git a/lib/src/common/pdu.cc b/lib/src/common/pdu.cc
index b0cdef638..1fce24ffc 100644
--- a/lib/src/common/pdu.cc
+++ b/lib/src/common/pdu.cc
@@ -122,7 +122,7 @@ uint8_t* sch_pdu::write_packet(srslte::log *log_h)
   sch_subh padding; 
   padding.set_padding();
 
-  if (nof_subheaders <= 0) {
+  if (nof_subheaders <= 0 && nof_subheaders < max_subheaders) {
     log_h->error("Trying to write packet with invalid number of subheaders (nof_subheaders=%d).\n", nof_subheaders);
     log_h->console("Trying to write packet with invalid number of subheaders (nof_subheaders=%d).\n", nof_subheaders);
     return NULL;