Continue to add PDCP tx test

lib/include/srslte/common/int_helpers.h

@@ -30,10 +30,7 @@ namespace srslte {
  *****************************************************************************/
 inline void uint8_to_uint32(uint8_t* buf, uint32_t* i)
 {
-  *i =  (uint32_t)buf[0] << 24 |
+  *i = (uint32_t)buf[0] << 24 | (uint32_t)buf[1] << 16 | (uint32_t)buf[2] << 8 | (uint32_t)buf[3];
-        (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)
@@ -46,8 +43,7 @@ inline void uint32_to_uint8(uint32_t i, uint8_t *buf)
 
 inline void uint8_to_uint16(uint8_t* buf, uint16_t* i)
 {
-  *i =  (uint32_t)buf[0] << 8  |
+  *i = (uint32_t)buf[0] << 8 | (uint32_t)buf[1];
-        (uint32_t)buf[1];
 }
 
 inline void uint16_to_uint8(uint16_t i, uint8_t* buf)
@@ -56,6 +52,17 @@ inline void uint16_to_uint8(uint16_t i, uint8_t *buf)
   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

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 protection
-  integrity_generate(sdu->msg, sdu->N_bytes - 4, tx_next, &sdu->msg[sdu->N_bytes - 4]);
+  uint8_t mac[4]; 
-  cipher_encrypt(sdu->msg, sdu->N_bytes - 4, tx_next, &sdu->msg[sdu->N_bytes - 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