diff --git a/lib/include/srslte/common/pcap.h b/lib/include/srslte/common/pcap.h index bc21568a0..21b91c929 100644 --- a/lib/include/srslte/common/pcap.h +++ b/lib/include/srslte/common/pcap.h @@ -17,6 +17,8 @@ #include #include +#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 } diff --git a/lib/src/common/mac_pcap.cc b/lib/src/common/mac_pcap.cc index a0b86c3c8..6875338f7 100644 --- a/lib/src/common/mac_pcap.cc +++ b/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."); diff --git a/lib/src/common/pcap.c b/lib/src/common/pcap.c index df469202e..e48319b73 100644 --- a/lib/src/common/pcap.c +++ b/lib/src/common/pcap.c @@ -12,6 +12,7 @@ #include "srslte/common/pcap.h" #include +#include #include #include #include @@ -49,60 +50,123 @@ void LTE_PCAP_Close(FILE* fd) } } -/* Write an individual 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) +/* Packs MAC Contect to a buffer */ +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; + int offset = 0; + uint16_t tmp16; - /* Can't write if file wasn't successfully opened */ - if (fd == NULL) { + if (buffer == NULL || length < PCAP_CONTEXT_HEADER_MAX) { 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; - context_header[offset++] = context->direction; - context_header[offset++] = context->rntiType; + buffer[offset++] = context->radioType; + buffer[offset++] = context->direction; + buffer[offset++] = context->rntiType; /* RNTI */ - context_header[offset++] = MAC_LTE_RNTI_TAG; - tmp16 = htons(context->rnti); - memcpy(context_header + offset, &tmp16, 2); + buffer[offset++] = MAC_LTE_RNTI_TAG; + tmp16 = htons(context->rnti); + memcpy(buffer + offset, &tmp16, 2); offset += 2; /* UEId */ - context_header[offset++] = MAC_LTE_UEID_TAG; - tmp16 = htons(context->ueid); - memcpy(context_header + offset, &tmp16, 2); + buffer[offset++] = MAC_LTE_UEID_TAG; + tmp16 = htons(context->ueid); + 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; - tmp16 = (context->sysFrameNumber << 4) | context->subFrameNumber; - tmp16 = htons(tmp16); - memcpy(context_header + offset, &tmp16, 2); + buffer[offset++] = MAC_LTE_FRAME_SUBFRAME_TAG; + tmp16 = (context->sysFrameNumber << 4) | context->subFrameNumber; + tmp16 = htons(tmp16); + memcpy(buffer + offset, &tmp16, 2); offset += 2; /* CRC Status */ - context_header[offset++] = MAC_LTE_CRC_STATUS_TAG; - context_header[offset++] = context->crcStatusOK; - - /* CC index */ - context_header[offset++] = MAC_LTE_CARRIER_ID_TAG; - context_header[offset++] = context->cc_idx; + buffer[offset++] = MAC_LTE_CRC_STATUS_TAG; + buffer[offset++] = context->crcStatusOK; /* NB-IoT mode tag */ - context_header[offset++] = MAC_LTE_NB_MODE_TAG; - context_header[offset++] = context->nbiotMode; + buffer[offset++] = MAC_LTE_NB_MODE_TAG; + 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,8 +222,8 @@ 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] = {}; - int offset = 0; + char context_header[PCAP_CONTEXT_HEADER_MAX] = {}; + int offset = 0; uint16_t tmp16; /* Can't write if file wasn't successfully opened */ @@ -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 * **************************************************************************/ - -/* 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) +inline int NR_PCAP_PACK_MAC_CONTEXT_TO_BUFFER(mac_nr_context_info_t* context, uint8_t* buffer, unsigned int length) { - char context_header[256] = {}; - int offset = 0; + int offset = 0; + 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; + if (buffer == NULL || length < PCAP_CONTEXT_HEADER_MAX) { + printf("Error: Writing buffer null or length to small \n"); + return -1; } - // 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; - context_header[offset++] = context->direction; - context_header[offset++] = context->rntiType; + buffer[offset++] = context->radioType; + buffer[offset++] = context->direction; + buffer[offset++] = context->rntiType; /* RNTI */ - context_header[offset++] = MAC_LTE_RNTI_TAG; - tmp16 = htons(context->rnti); - memcpy(context_header + offset, &tmp16, 2); + buffer[offset++] = MAC_LTE_RNTI_TAG; + tmp16 = htons(context->rnti); + memcpy(buffer + offset, &tmp16, 2); offset += 2; /* UEId */ - context_header[offset++] = MAC_LTE_UEID_TAG; - tmp16 = htons(context->ueid); - memcpy(context_header + offset, &tmp16, 2); + buffer[offset++] = MAC_LTE_UEID_TAG; + tmp16 = htons(context->ueid); + memcpy(buffer + offset, &tmp16, 2); offset += 2; /* HARQID */ - context_header[offset++] = MAC_NR_HARQID; - context_header[offset++] = context->harqid; + buffer[offset++] = MAC_NR_HARQID; + buffer[offset++] = context->harqid; /* PHR Type2 other cell */ - context_header[offset++] = MAC_NR_PHR_TYPE2_OTHERCELL_TAG; - context_header[offset++] = context->phr_type2_othercell; + buffer[offset++] = MAC_NR_PHR_TYPE2_OTHERCELL_TAG; + 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; - tmp16 = (context->system_frame_number << 4) | context->sub_frame_number; - tmp16 = htons(tmp16); - memcpy(context_header + offset, &tmp16, 2); + buffer[offset++] = MAC_LTE_FRAME_SUBFRAME_TAG; + tmp16 = (context->system_frame_number << 4) | context->sub_frame_number; + tmp16 = htons(tmp16); + 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 */