From 9d0db108c6e6fda07f383be7b3ea4dd84bb1cd3a Mon Sep 17 00:00:00 2001
From: Pedro Alvarez <pedro.alvarez@softwareradiosystems.com>
Date: Fri, 5 Jul 2019 18:34:59 +0100
Subject: [PATCH] Continue to add PDCP tx test

---
 lib/include/srslte/common/int_helpers.h | 27 +++++++++++++--------
 lib/src/upper/pdcp_entity_nr.cc         | 32 +++++++++++++++++++++----
 2 files changed, 45 insertions(+), 14 deletions(-)

diff --git a/lib/include/srslte/common/int_helpers.h b/lib/include/srslte/common/int_helpers.h
index 074e572c0..945c3bf69 100644
--- a/lib/include/srslte/common/int_helpers.h
+++ b/lib/include/srslte/common/int_helpers.h
@@ -28,15 +28,12 @@ namespace srslte {
  * Safe conversions between byte buffers and integer types.
  * Note: these don't perform endian conversion - use e.g. htonl/ntohl if required
  *****************************************************************************/
-inline void uint8_to_uint32(uint8_t *buf, uint32_t *i)
+inline void uint8_to_uint32(uint8_t* buf, uint32_t* i)
 {
-  *i =  (uint32_t)buf[0] << 24 |
-        (uint32_t)buf[1] << 16 |
-        (uint32_t)buf[2] << 8  |
-        (uint32_t)buf[3];
+  *i = (uint32_t)buf[0] << 24 | (uint32_t)buf[1] << 16 | (uint32_t)buf[2] << 8 | (uint32_t)buf[3];
 }
 
-inline void uint32_to_uint8(uint32_t i, uint8_t *buf)
+inline void uint32_to_uint8(uint32_t i, uint8_t* buf)
 {
   buf[0] = (i >> 24) & 0xFF;
   buf[1] = (i >> 16) & 0xFF;
@@ -44,18 +41,28 @@ inline void uint32_to_uint8(uint32_t i, uint8_t *buf)
   buf[3] = i & 0xFF;
 }
 
-inline void uint8_to_uint16(uint8_t *buf, uint16_t *i)
+inline void uint8_to_uint16(uint8_t* buf, uint16_t* i)
 {
-  *i =  (uint32_t)buf[0] << 8  |
-        (uint32_t)buf[1];
+  *i = (uint32_t)buf[0] << 8 | (uint32_t)buf[1];
 }
 
-inline void uint16_to_uint8(uint16_t i, uint8_t *buf)
+inline void uint16_to_uint8(uint16_t i, uint8_t* buf)
 {
   buf[0] = (i >> 8) & 0xFF;
   buf[1] = i & 0xFF;
 }
 
+inline void uint8_to_uint24(uint8_t* buf, uint32_t* i)
+{
+  *i = (uint32_t)buf[0] << 16 | (uint32_t)buf[1] << 8 | (uint32_t)buf[2];
+}
+
+inline void uint24_to_uint8(uint32_t i, uint8_t* buf)
+{
+  buf[0] = (i >> 16) & 0xFF;
+  buf[1] = (i >> 8) & 0xFF;
+  buf[2] = i & 0xFF;
+}
 }; //namespace
 
 #endif // SRSLTE_INT_HELPERS_H
diff --git a/lib/src/upper/pdcp_entity_nr.cc b/lib/src/upper/pdcp_entity_nr.cc
index 9ca3e0e86..b97abb0be 100644
--- a/lib/src/upper/pdcp_entity_nr.cc
+++ b/lib/src/upper/pdcp_entity_nr.cc
@@ -76,12 +76,15 @@ void pdcp_entity_nr::write_sdu(unique_byte_buffer_t sdu, bool blocking)
   // Start discard timer TODO
   // Perform header compression TODO
 
-  // Integrity protection and ciphering
-  integrity_generate(sdu->msg, sdu->N_bytes - 4, tx_next, &sdu->msg[sdu->N_bytes - 4]);
-  cipher_encrypt(sdu->msg, sdu->N_bytes - 4, tx_next, &sdu->msg[sdu->N_bytes - 4]);
+  // Integrity protection
+  uint8_t mac[4]; 
+  integrity_generate(sdu->msg, sdu->N_bytes, tx_next, mac);
+
+  // Ciphering
+  cipher_encrypt(sdu->msg, sdu->N_bytes, tx_next, sdu->msg);
 
   // Write PDCP header info
-  //write_header(pdu, tx_next);
+  write_header(pdu, tx_next);
   
   // Write to lower layers
   rlc->write_sdu(lcid, std::move(sdu), blocking);
@@ -163,4 +166,25 @@ uint32_t pdcp_entity_nr::get_rcvd_sn(const unique_byte_buffer_t& pdu)
   }
   return rcvd_sn;
 }
+
+void pdcp_entity_nr::write_data_header(const srslte::unique_byte_buffer_t& msg, uint32_t sn)
+{
+
+  switch (sn_len) {
+    case PDCP_SN_LEN_12:
+      sdu->msg -= 2;
+      sdu->N_bytes += 2;
+      srslte::uint16_to_uint8_t(0x3F & sn);
+      if (is_data()) {
+        pdu->msg[0] = 0x80; // On DRB Data PDUs we must set the D flag.
+      }
+      break;
+    case PDCP_SN_LEN_18:
+      sdu->msg -= 3;
+      sdu->N_bytes += 3;
+      pdu->msg[0] = 0x80; // Data PDU and SN 18, D flag present
+      *sdu->msg = sn & 0x3F;
+      break;
+  }
+}
 } // namespace srslte