Refactored functions for pcap udp packing and write lte mac pcap with udp frame

lib/include/srslte/common/pcap.h

@@ -17,6 +17,8 @@
 #include <stdbool.h>
 #include <stdio.h>
 
+#define PCAP_CONTEXT_HEADER_MAX 256
+
 #define MAC_LTE_DLT 147
 #define NAS_LTE_DLT 148
 #define UDP_DLT 149 // UDP needs to be selected as protocol
@@ -189,6 +191,8 @@ void LTE_PCAP_Close(FILE* fd);
 
 /* Write an individual MAC PDU (PCAP packet header + mac-context + mac-pdu) */
 int LTE_PCAP_MAC_WritePDU(FILE* fd, MAC_Context_Info_t* context, const unsigned char* PDU, unsigned int length);
+int LTE_PCAP_MAC_UDP_WritePDU(FILE* fd, MAC_Context_Info_t* context, const unsigned char* PDU, unsigned int length);
+int LTE_PCAP_PACK_MAC_CONTEXT_TO_BUFFER(MAC_Context_Info_t* context, uint8_t* PDU, unsigned int length);
 
 /* Write an individual NAS PDU (PCAP packet header + nas-context + nas-pdu) */
 int LTE_PCAP_NAS_WritePDU(FILE* fd, NAS_Context_Info_t* context, const unsigned char* PDU, unsigned int length);
@@ -200,7 +204,8 @@ int LTE_PCAP_RLC_WritePDU(FILE* fd, RLC_Context_Info_t* context, const unsigned
 int LTE_PCAP_S1AP_WritePDU(FILE* fd, S1AP_Context_Info_t* context, const unsigned char* PDU, unsigned int length);
 
 /* Write an individual NR MAC PDU (PCAP packet header + UDP header + nr-mac-context + mac-pdu) */
-int NR_PCAP_MAC_WritePDU(FILE* fd, mac_nr_context_info_t* context, const unsigned char* PDU, unsigned int length);
+int NR_PCAP_MAC_UDP_WritePDU(FILE* fd, mac_nr_context_info_t* context, const unsigned char* PDU, unsigned int length);
+int NR_PCAP_PACK_MAC_CONTEXT_TO_BUFFER(mac_nr_context_info_t* context, uint8_t* buffer, unsigned int length);
 
 #ifdef __cplusplus
 }

lib/src/common/mac_pcap.cc

@@ -43,7 +43,7 @@ uint32_t mac_pcap::open(std::string filename_, uint32_t ue_id_)
   // set DLT for selected RAT
   switch (rat) {
     case srslte_rat_t::lte:
-      dlt = MAC_LTE_DLT;
+      dlt = UDP_DLT;
       break;
     case srslte_rat_t::nr:
       dlt = UDP_DLT;
@@ -101,10 +101,10 @@ void mac_pcap::write_pdu(pcap_pdu_t& pdu)
   if (pdu.pdu != nullptr) {
     switch (rat) {
       case srslte_rat_t::lte:
-        LTE_PCAP_MAC_WritePDU(pcap_file, &pdu.context, pdu.pdu->msg, pdu.pdu->N_bytes);
+        LTE_PCAP_MAC_UDP_WritePDU(pcap_file, &pdu.context, pdu.pdu->msg, pdu.pdu->N_bytes);
         break;
       case srslte_rat_t::nr:
-        NR_PCAP_MAC_WritePDU(pcap_file, &pdu.context_nr, pdu.pdu->msg, pdu.pdu->N_bytes);
+        NR_PCAP_MAC_UDP_WritePDU(pcap_file, &pdu.context_nr, pdu.pdu->msg, pdu.pdu->N_bytes);
         break;
       default:
         logger.error("Error writing PDU to PCAP. Unsupported RAT selected.");

lib/src/common/pcap.c

@@ -12,6 +12,7 @@
 
 #include "srslte/common/pcap.h"
 #include <arpa/inet.h>
+#include <linux/udp.h>
 #include <stdio.h>
 #include <string.h>
 #include <sys/time.h>
@@ -49,60 +50,123 @@ void LTE_PCAP_Close(FILE* fd)
   }
 }
 
-/* Write an individual PDU (PCAP packet header + mac-context + mac-pdu) */
+/* Packs MAC Contect to a buffer */
-int LTE_PCAP_MAC_WritePDU(FILE* fd, MAC_Context_Info_t* context, const unsigned char* PDU, unsigned int length)
+inline int LTE_PCAP_PACK_MAC_CONTEXT_TO_BUFFER(MAC_Context_Info_t* context, uint8_t* buffer, unsigned int length)
 {
-  pcaprec_hdr_t packet_header;
-  char          context_header[256];
   int      offset = 0;
   uint16_t tmp16;
 
-  /* Can't write if file wasn't successfully opened */
+  if (buffer == NULL || length < PCAP_CONTEXT_HEADER_MAX) {
-  if (fd == NULL) {
     printf("Error: Can't write to empty file handle\n");
-    return 0;
+    return -1;
   }
 
   /*****************************************************************/
   /* Context information (same as written by UDP heuristic clients */
-  context_header[offset++] = context->radioType;
+  buffer[offset++] = context->radioType;
-  context_header[offset++] = context->direction;
+  buffer[offset++] = context->direction;
-  context_header[offset++] = context->rntiType;
+  buffer[offset++] = context->rntiType;
 
   /* RNTI */
-  context_header[offset++] = MAC_LTE_RNTI_TAG;
+  buffer[offset++] = MAC_LTE_RNTI_TAG;
   tmp16            = htons(context->rnti);
-  memcpy(context_header + offset, &tmp16, 2);
+  memcpy(buffer + offset, &tmp16, 2);
   offset += 2;
 
   /* UEId */
-  context_header[offset++] = MAC_LTE_UEID_TAG;
+  buffer[offset++] = MAC_LTE_UEID_TAG;
   tmp16            = htons(context->ueid);
-  memcpy(context_header + offset, &tmp16, 2);
+  memcpy(buffer + offset, &tmp16, 2);
   offset += 2;
 
   /* Subframe Number and System Frame Number */
   /* SFN is stored in 12 MSB and SF in 4 LSB */
-  context_header[offset++] = MAC_LTE_FRAME_SUBFRAME_TAG;
+  buffer[offset++] = MAC_LTE_FRAME_SUBFRAME_TAG;
   tmp16            = (context->sysFrameNumber << 4) | context->subFrameNumber;
   tmp16            = htons(tmp16);
-  memcpy(context_header + offset, &tmp16, 2);
+  memcpy(buffer + offset, &tmp16, 2);
   offset += 2;
 
   /* CRC Status */
-  context_header[offset++] = MAC_LTE_CRC_STATUS_TAG;
+  buffer[offset++] = MAC_LTE_CRC_STATUS_TAG;
-  context_header[offset++] = context->crcStatusOK;
+  buffer[offset++] = context->crcStatusOK;
-
-  /* CC index */
-  context_header[offset++] = MAC_LTE_CARRIER_ID_TAG;
-  context_header[offset++] = context->cc_idx;
 
   /* NB-IoT mode tag */
-  context_header[offset++] = MAC_LTE_NB_MODE_TAG;
+  buffer[offset++] = MAC_LTE_NB_MODE_TAG;
-  context_header[offset++] = context->nbiotMode;
+  buffer[offset++] = context->nbiotMode;
 
   /* Data tag immediately preceding PDU */
-  context_header[offset++] = MAC_LTE_PAYLOAD_TAG;
+  buffer[offset++] = MAC_LTE_PAYLOAD_TAG;
+  return offset;
+}
+
+/* Write an individual PDU (PCAP packet header + mac-context + mac-pdu) */
+inline int LTE_PCAP_MAC_WritePDU(FILE* fd, MAC_Context_Info_t* context, const unsigned char* PDU, unsigned int length)
+{
+  pcaprec_hdr_t packet_header;
+  uint8_t       context_header[PCAP_CONTEXT_HEADER_MAX];
+  int           offset = 0;
+
+  /* Can't write if file wasn't successfully opened */
+  if (fd == NULL) {
+    printf("Error: Can't write to empty file handle\n");
+    return 0;
+  }
+
+  offset += LTE_PCAP_PACK_MAC_CONTEXT_TO_BUFFER(context, &context_header[offset], PCAP_CONTEXT_HEADER_MAX);
+
+  /****************************************************************/
+  /* PCAP Header                                                  */
+  struct timeval t;
+  gettimeofday(&t, NULL);
+  packet_header.ts_sec   = t.tv_sec;
+  packet_header.ts_usec  = t.tv_usec;
+  packet_header.incl_len = offset + length;
+  packet_header.orig_len = offset + length;
+
+  /***************************************************************/
+  /* Now write everything to the file                            */
+  fwrite(&packet_header, sizeof(pcaprec_hdr_t), 1, fd);
+  fwrite(context_header, 1, offset, fd);
+  fwrite(PDU, 1, length, fd);
+
+  return 1;
+}
+
+/* Write an individual PDU (PCAP packet header + mac-context + mac-pdu) */
+inline int
+LTE_PCAP_MAC_UDP_WritePDU(FILE* fd, MAC_Context_Info_t* context, const unsigned char* PDU, unsigned int length)
+{
+  pcaprec_hdr_t  packet_header;
+  uint8_t        context_header[PCAP_CONTEXT_HEADER_MAX] = {};
+  int            offset                                  = 0;
+  struct udphdr* udp_header;
+  // uint16_t       tmp16;
+
+  /* Can't write if file wasn't successfully opened */
+  if (fd == NULL) {
+    printf("Error: Can't write to empty file handle\n");
+    return 0;
+  }
+  // Add dummy UDP header, start with src and dest port
+  udp_header       = (struct udphdr*)context_header;
+  udp_header->dest = htons(0xdead);
+  offset += 2;
+  udp_header->source = htons(0xbeef);
+  offset += 2;
+  // length to be filled later
+  udp_header->len = 0x0000;
+  offset += 2;
+  // dummy CRC
+  udp_header->check = 0x0000;
+  offset += 2;
+
+  // Start magic string
+  memcpy(&context_header[offset], MAC_LTE_START_STRING, strlen(MAC_LTE_START_STRING));
+  offset += strlen(MAC_LTE_START_STRING);
+
+  offset += LTE_PCAP_PACK_MAC_CONTEXT_TO_BUFFER(context, &context_header[offset], PCAP_CONTEXT_HEADER_MAX);
+  udp_header->len = htons(length + offset);
 
   /****************************************************************/
   /* PCAP Header                                                  */
@@ -158,7 +222,7 @@ int LTE_PCAP_NAS_WritePDU(FILE* fd, NAS_Context_Info_t* context, const unsigned
 int LTE_PCAP_RLC_WritePDU(FILE* fd, RLC_Context_Info_t* context, const unsigned char* PDU, unsigned int length)
 {
   pcaprec_hdr_t packet_header;
-  char          context_header[256] = {};
+  char          context_header[PCAP_CONTEXT_HEADER_MAX] = {};
   int           offset                                  = 0;
   uint16_t      tmp16;
 
@@ -271,72 +335,92 @@ int LTE_PCAP_S1AP_WritePDU(FILE* fd, S1AP_Context_Info_t* context, const unsigne
 /**************************************************************************
  * API functions for writing MAC-NR PCAP files                           *
  **************************************************************************/
-
+inline int NR_PCAP_PACK_MAC_CONTEXT_TO_BUFFER(mac_nr_context_info_t* context, uint8_t* buffer, unsigned int length)
-/* Write an individual NR MAC PDU (PCAP packet header + UDP header + nr-mac-context + mac-pdu) */
-int NR_PCAP_MAC_WritePDU(FILE* fd, mac_nr_context_info_t* context, const unsigned char* PDU, unsigned int length)
 {
-  char context_header[256] = {};
   int      offset = 0;
+  uint16_t tmp16;
 
-  /* Can't write if file wasn't successfully opened */
+  if (buffer == NULL || length < PCAP_CONTEXT_HEADER_MAX) {
-  if (fd == NULL) {
+    printf("Error: Writing buffer null or length to small \n");
-    printf("Error: Can't write to empty file handle\n");
+    return -1;
-    return 0;
   }
 
-  // Add dummy UDP header, start with src and dest port
-  context_header[offset++] = 0xde;
-  context_header[offset++] = 0xad;
-  context_header[offset++] = 0xbe;
-  context_header[offset++] = 0xef;
-  // length
-  uint16_t tmp16 = htons(length + 31);
-  memcpy(context_header + offset, &tmp16, 2);
-  offset += 2;
-  // dummy CRC
-  context_header[offset++] = 0x00;
-  context_header[offset++] = 0x00;
-
-  // Start magic string
-  memcpy(&context_header[offset], MAC_NR_START_STRING, strlen(MAC_NR_START_STRING));
-  offset += strlen(MAC_NR_START_STRING);
-
   /*****************************************************************/
   /* Context information (same as written by UDP heuristic clients */
-  context_header[offset++] = context->radioType;
+  buffer[offset++] = context->radioType;
-  context_header[offset++] = context->direction;
+  buffer[offset++] = context->direction;
-  context_header[offset++] = context->rntiType;
+  buffer[offset++] = context->rntiType;
 
   /* RNTI */
-  context_header[offset++] = MAC_LTE_RNTI_TAG;
+  buffer[offset++] = MAC_LTE_RNTI_TAG;
   tmp16            = htons(context->rnti);
-  memcpy(context_header + offset, &tmp16, 2);
+  memcpy(buffer + offset, &tmp16, 2);
   offset += 2;
 
   /* UEId */
-  context_header[offset++] = MAC_LTE_UEID_TAG;
+  buffer[offset++] = MAC_LTE_UEID_TAG;
   tmp16            = htons(context->ueid);
-  memcpy(context_header + offset, &tmp16, 2);
+  memcpy(buffer + offset, &tmp16, 2);
   offset += 2;
 
   /* HARQID */
-  context_header[offset++] = MAC_NR_HARQID;
+  buffer[offset++] = MAC_NR_HARQID;
-  context_header[offset++] = context->harqid;
+  buffer[offset++] = context->harqid;
 
   /* PHR Type2 other cell */
-  context_header[offset++] = MAC_NR_PHR_TYPE2_OTHERCELL_TAG;
+  buffer[offset++] = MAC_NR_PHR_TYPE2_OTHERCELL_TAG;
-  context_header[offset++] = context->phr_type2_othercell;
+  buffer[offset++] = context->phr_type2_othercell;
 
   /* Subframe Number and System Frame Number */
   /* SFN is stored in 12 MSB and SF in 4 LSB */
-  context_header[offset++] = MAC_LTE_FRAME_SUBFRAME_TAG;
+  buffer[offset++] = MAC_LTE_FRAME_SUBFRAME_TAG;
   tmp16            = (context->system_frame_number << 4) | context->sub_frame_number;
   tmp16            = htons(tmp16);
-  memcpy(context_header + offset, &tmp16, 2);
+  memcpy(buffer + offset, &tmp16, 2);
   offset += 2;
 
   /* Data tag immediately preceding PDU */
-  context_header[offset++] = MAC_LTE_PAYLOAD_TAG;
+  buffer[offset++] = MAC_LTE_PAYLOAD_TAG;
+  return offset;
+}
+
+/* Write an individual NR MAC PDU (PCAP packet header + UDP header + nr-mac-context + mac-pdu) */
+int NR_PCAP_MAC_UDP_WritePDU(FILE* fd, mac_nr_context_info_t* context, const unsigned char* PDU, unsigned int length)
+{
+  uint8_t        context_header[PCAP_CONTEXT_HEADER_MAX] = {};
+  struct udphdr* udp_header;
+  int            offset = 0;
+
+  /* Can't write if file wasn't successfully opened */
+  if (fd == NULL) {
+    printf("Error: Can't write to empty file handle\n");
+    return -1;
+  }
+
+  // Add dummy UDP header, start with src and dest port
+  udp_header       = (struct udphdr*)context_header;
+  udp_header->dest = htons(0xdead);
+  offset += 2;
+  udp_header->source = htons(0xbeef);
+  offset += 2;
+  // length to be filled later
+  udp_header->len = 0x0000;
+  offset += 2;
+  // dummy CRC
+  udp_header->check = 0x0000;
+  offset += 2;
+
+  // Start magic string
+  memcpy(&context_header[offset], MAC_NR_START_STRING, strlen(MAC_NR_START_STRING));
+  offset += strlen(MAC_NR_START_STRING);
+
+  offset += NR_PCAP_PACK_MAC_CONTEXT_TO_BUFFER(context, &context_header[offset], PCAP_CONTEXT_HEADER_MAX);
+
+  udp_header->len = htons(offset + length);
+
+  if (offset != 31) {
+    printf("ERROR Does not match offset %d != 31\n", offset);
+  }
 
   /****************************************************************/
   /* PCAP Header                                                  */