Starting to fix handling of RX SRB PDUs in PDCP.

lib/include/srslte/upper/pdcp_entity_lte.h

@@ -80,6 +80,7 @@ private:
   uint32_t last_submitted_pdcp_rx_sn = 0;
   uint32_t maximum_pdcp_sn           = 0;
 
+  bool handle_srb_pdu(const srslte::unique_byte_buffer_t& pdu);
   void handle_um_drb_pdu(const srslte::unique_byte_buffer_t& pdu);
   void handle_am_drb_pdu(const srslte::unique_byte_buffer_t& pdu);
 };
@@ -90,7 +91,7 @@ private:
  ***************************************************************************/
 
 void pdcp_pack_control_pdu(uint32_t sn, byte_buffer_t* sdu);
-void pdcp_unpack_control_pdu(byte_buffer_t* sdu, uint32_t* sn);
+void pdcp_unpack_control_pdu(byte_buffer_t* sdu, uint32_t* sn, uint8_t* mac);
 
 void pdcp_pack_data_pdu_short_sn(uint32_t sn, byte_buffer_t* sdu);
 void pdcp_unpack_data_pdu_short_sn(byte_buffer_t* sdu, uint32_t* sn);

lib/src/upper/pdcp_entity_lte.cc

@@ -153,45 +153,54 @@ void pdcp_entity_lte::write_pdu(unique_byte_buffer_t pdu)
   }
 
   std::unique_lock<std::mutex> lock(mutex);
-  if (is_drb()) {
-    // Handle DRB messages
-    if (rlc->rb_is_um(lcid)) {
+  if (is_srb()) {
+    bool integrity_valid = handle_srb_pdu(pdu);
+    if (integrity_valid) {
+      rrc->write_pdu(lcid, std::move(pdu));
+    }
+  } else if (is_drb() && rlc->rb_is_um(lcid)) {
     handle_um_drb_pdu(pdu);
-    } else {
+    gw->write_pdu(lcid, std::move(pdu));
+  } else if (is_drb() && !rlc->rb_is_um(lcid)) {
     handle_am_drb_pdu(pdu);
-    }
     gw->write_pdu(lcid, std::move(pdu));
   } else {
-    // Handle SRB messages
-    if (is_srb()) {
-      uint32_t sn = *pdu->msg & 0x1F;
+    log->error("Invalid PDCP/RLC configuration");
+  }
+}
+
+/****************************************************************************
+ * Rx data/control handler functions
+ * Ref: 3GPP TS 36.323 v10.1.0 Section 5.1.2
+ ***************************************************************************/
+// SRBs (5.1.2.2)
+// Returns a boolean indicating weather integrity has passed
+bool pdcp_entity_lte::handle_srb_pdu(const srslte::unique_byte_buffer_t& pdu)
+{
+
+  uint32_t sn; 
+  uint8_t mac[4];
+  pdcp_unpack_control_pdu(pdu.get(), &sn, mac);
+
+  // TODO Fix count computation
+  uint32_t count = sn;
+ 
   if (do_encryption) {
-        cipher_decrypt(
-            &pdu->msg[cfg.hdr_len_bytes], pdu->N_bytes - cfg.hdr_len_bytes, sn, &(pdu->msg[cfg.hdr_len_bytes]));
+    cipher_decrypt(pdu->msg, pdu->N_bytes, count, pdu->msg);
     log->info_hex(pdu->msg, pdu->N_bytes, "RX %s PDU (decrypted)", rrc->get_rb_name(lcid).c_str());
   }
 
   if (do_integrity) {
     if (not integrity_verify(pdu->msg, pdu->N_bytes - 4, sn, &(pdu->msg[pdu->N_bytes - 4]))) {
       log->error_hex(pdu->msg, pdu->N_bytes, "%s Dropping PDU", rrc->get_rb_name(lcid).c_str());
-          goto exit;
+      return false;
     }
   }
 
-      pdcp_unpack_control_pdu(pdu.get(), &sn);
   log->info_hex(pdu->msg, pdu->N_bytes, "RX %s PDU SN: %d", rrc->get_rb_name(lcid).c_str(), sn);
-    }
-    // pass to RRC
-    rrc->write_pdu(lcid, std::move(pdu));
-  }
-exit:
-  rx_count++;
+  return true;
 }
 
-/****************************************************************************
- * Rx data/control handler functions
- * Ref: 3GPP TS 36.323 v10.1.0 Section 5.1.2
- ***************************************************************************/
 // DRBs mapped on RLC UM (5.1.2.1.3)
 void pdcp_entity_lte::handle_um_drb_pdu(const srslte::unique_byte_buffer_t& pdu)
 {
@@ -332,17 +341,15 @@ void pdcp_pack_control_pdu(uint32_t sn, byte_buffer_t* sdu)
   sdu->msg[sdu->N_bytes++] = PDCP_CONTROL_MAC_I & 0xFF;
 }
 
-void pdcp_unpack_control_pdu(byte_buffer_t* pdu, uint32_t* sn)
+void pdcp_unpack_control_pdu(byte_buffer_t* sdu, uint32_t* sn, uint8_t *mac)
 {
   // Strip header
-  *sn = *pdu->msg & 0x1F;
-  pdu->msg++;
-  pdu->N_bytes--;
-
-  // Strip MAC
-  pdu->N_bytes -= 4;
+  *sn = sdu->msg[0] & 0x1Fu;
+  sdu->msg++;
+  sdu->N_bytes--;
   
-  // TODO: integrity check MAC
+  memcpy(mac, sdu->msg, 4);
+  sdu->N_bytes -= 4;
 }
 
 void pdcp_pack_data_pdu_short_sn(uint32_t sn, byte_buffer_t* sdu)
@@ -350,13 +357,14 @@ void pdcp_pack_data_pdu_short_sn(uint32_t sn, byte_buffer_t* sdu)
   // Make room and add header
   sdu->msg--;
   sdu->N_bytes++;
-  sdu->msg[0] = (PDCP_D_C_DATA_PDU << 7) | (sn & 0x7F);
+  sdu->msg[0] = (PDCP_D_C_DATA_PDU << 7) | (sn & 0x7Fu);
 }
 
+
 void pdcp_unpack_data_pdu_short_sn(byte_buffer_t* sdu, uint32_t* sn)
 {
   // Strip header
-  *sn = sdu->msg[0] & 0x7F;
+  *sn = sdu->msg[0] & 0x7Fu;
   sdu->msg++;
   sdu->N_bytes--;
 }
@@ -366,14 +374,14 @@ void pdcp_pack_data_pdu_long_sn(uint32_t sn, byte_buffer_t* sdu)
   // Make room and add header
   sdu->msg -= 2;
   sdu->N_bytes += 2;
-  sdu->msg[0] = (PDCP_D_C_DATA_PDU << 7) | ((sn >> 8) & 0x0F);
-  sdu->msg[1] = sn & 0xFF;
+  sdu->msg[0] = (PDCP_D_C_DATA_PDU << 7) | ((sn >> 8) & 0x0Fu);
+  sdu->msg[1] = sn & 0xFFu;
 }
 
 void pdcp_unpack_data_pdu_long_sn(byte_buffer_t* sdu, uint32_t* sn)
 {
   // Strip header
-  *sn = (sdu->msg[0] & 0x0F) << 8;
+  *sn = (sdu->msg[0] & 0x0Fu) << 8;
   *sn |= sdu->msg[1];
   sdu->msg += 2;
   sdu->N_bytes -= 2;