diff --git a/lib/include/srslte/upper/gtpu.h b/lib/include/srslte/upper/gtpu.h index 55c8189cc..2230d76b7 100644 --- a/lib/include/srslte/upper/gtpu.h +++ b/lib/include/srslte/upper/gtpu.h @@ -40,57 +40,83 @@ namespace srslte { * | 8 | 7 | 6 | 5 | 4 | 3 | 2 | 1 | * * 1 | Version |PT | * | E | S |PN | - * 2 | Message Type | - * 3 | Length (1st Octet) | - * 4 | Length (2nd Octet) | - * 5 | TEID (1st Octet) | - * 6 | TEID (2nd Octet) | - * 7 | TEID (3rd Octet) | - * 8 | TEID (4th Octet) | + * 2 | Message Type | + * 3 | Length (1st Octet) | + * 4 | Length (2nd Octet) | + * 5 | TEID (1st Octet) | + * 6 | TEID (2nd Octet) | + * 7 | TEID (3rd Octet) | + * 8 | TEID (4th Octet) | + * 9 | Seq Number (1st Octet) | + * 10 | Seq Number (2st Octet) | + * 11 | N-PDU | + * 12 | Next Extension Header Type | ***************************************************************************/ -#define GTPU_HEADER_LEN 8 +#define GTPU_BASE_HEADER_LEN 8 +#define GTPU_EXTENDED_HEADER_LEN 12 + +#define GTPU_FLAGS_VERSION_MASK 0xE0 +#define GTPU_FLAGS_VERSION_V1 0x20 +#define GTPU_FLAGS_GTP_PRIME_PROTOCOL 0x00 +#define GTPU_FLAGS_GTP_PROTOCOL 0x10 +#define GTPU_FLAGS_EXTENDED_HDR 0x04 +#define GTPU_FLAGS_SEQUENCE 0x02 +#define GTPU_FLAGS_PACKET_NUM 0x01 + +#define GTPU_MSG_ECHO_REQUEST 1 +#define GTPU_MSG_ECHO_RESPONSE 2 +#define GTPU_MSG_ERROR_INDICATION 26 +#define GTPU_MSG_SUPPORTED_EXTENSION_HEADERS_NOTIFICATION 31 +#define GTPU_MSG_END_MARKER 254 +#define GTPU_MSG_DATA_PDU 255 typedef struct{ uint8_t flags; uint8_t message_type; uint16_t length; uint32_t teid; + uint16_t seq_number; + uint8_t n_pdu; + uint8_t next_ext_hdr_type; }gtpu_header_t; bool gtpu_read_header(srslte::byte_buffer_t *pdu, gtpu_header_t *header, srslte::log *gtpu_log); bool gtpu_write_header(gtpu_header_t *header, srslte::byte_buffer_t *pdu, srslte::log *gtpu_log); -inline void uint8_to_uint32(uint8_t *buf, uint32_t *i) +inline bool gtpu_supported_flags_check(gtpu_header_t *header, srslte::log *gtpu_log) { - *i = (uint32_t)buf[0] << 24 | - (uint32_t)buf[1] << 16 | - (uint32_t)buf[2] << 8 | - (uint32_t)buf[3]; + //flags + if( (header->flags & GTPU_FLAGS_VERSION_MASK) != GTPU_FLAGS_VERSION_V1 ) { + gtpu_log->error("gtpu_header - Unhandled GTP-U Version. Flags: 0x%x\n", header->flags); + return false; + } + if( !(header->flags & GTPU_FLAGS_GTP_PROTOCOL) ) { + gtpu_log->error("gtpu_header - Unhandled Protocol Type. Flags: 0x%x\n\n", header->flags); + return false; + } + if( header->flags & GTPU_FLAGS_EXTENDED_HDR ) { + gtpu_log->error("gtpu_header - Unhandled Header Extensions. Flags: 0x%x\n\n", header->flags); + return false; + } + if( header->flags & GTPU_FLAGS_PACKET_NUM ) { + gtpu_log->error("gtpu_header - Unhandled Packet Number. Flags: 0x%x\n\n", header->flags); + return false; + } + return true; } -inline void uint32_to_uint8(uint32_t i, uint8_t *buf) +inline bool gtpu_supported_msg_type_check(gtpu_header_t *header, srslte::log *gtpu_log) { - buf[0] = (i >> 24) & 0xFF; - buf[1] = (i >> 16) & 0xFF; - buf[2] = (i >> 8) & 0xFF; - buf[3] = i & 0xFF; + //msg_tpye + if( header->message_type != GTPU_MSG_DATA_PDU && header->message_type != GTPU_MSG_ECHO_REQUEST && header->message_type != GTPU_MSG_ECHO_RESPONSE) { + gtpu_log->error("gtpu_header - Unhandled message type: 0x%x\n", header->message_type); + return false; + } + return true; } -inline void uint8_to_uint16(uint8_t *buf, uint16_t *i) -{ - *i = (uint32_t)buf[0] << 8 | - (uint32_t)buf[1]; -} - -inline void uint16_to_uint8(uint16_t i, uint8_t *buf) -{ - buf[0] = (i >> 8) & 0xFF; - buf[1] = i & 0xFF; -} - - }//namespace #endif diff --git a/lib/src/upper/gtpu.cc b/lib/src/upper/gtpu.cc index a8046aa4d..537b1043c 100644 --- a/lib/src/upper/gtpu.cc +++ b/lib/src/upper/gtpu.cc @@ -26,7 +26,7 @@ #include "srslte/upper/gtpu.h" - +#include "srslte/common/int_helpers.h" namespace srslte { @@ -37,24 +37,37 @@ namespace srslte { bool gtpu_write_header(gtpu_header_t *header, srslte::byte_buffer_t *pdu, srslte::log *gtpu_log) { - if(header->flags != 0x30) { - gtpu_log->error("gtpu_write_header - Unhandled header flags: 0x%x\n", header->flags); - return false; - } - if(header->message_type != 0xFF) { - gtpu_log->error("gtpu_write_header - Unhandled message type: 0x%x\n", header->message_type); - return false; - } - if(pdu->get_headroom() < GTPU_HEADER_LEN) { - gtpu_log->error("gtpu_write_header - No room in PDU for header\n"); + //flags + if(!gtpu_supported_flags_check(header,gtpu_log)){ + gtpu_log->error("gtpu_write_header - Unhandled GTP-U Flags. Flags: 0x%x\n", header->flags); return false; } - pdu->msg -= GTPU_HEADER_LEN; - pdu->N_bytes += GTPU_HEADER_LEN; + //msg type + if(!gtpu_supported_msg_type_check(header,gtpu_log)){ + gtpu_log->error("gtpu_write_header - Unhandled GTP-U Message Type. Message Type: 0x%x\n", header->message_type); + return false; + } + //If E, S or PN are set, the header is longer + if (header->flags & (GTPU_FLAGS_EXTENDED_HDR | GTPU_FLAGS_SEQUENCE | GTPU_FLAGS_PACKET_NUM)) { + if(pdu->get_headroom() < GTPU_EXTENDED_HEADER_LEN) { + gtpu_log->error("gtpu_write_header - No room in PDU for header\n"); + return false; + } + pdu->msg -= GTPU_EXTENDED_HEADER_LEN; + pdu->N_bytes += GTPU_EXTENDED_HEADER_LEN; + } else { + if(pdu->get_headroom() < GTPU_BASE_HEADER_LEN) { + gtpu_log->error("gtpu_write_header - No room in PDU for header\n"); + return false; + } + pdu->msg -= GTPU_BASE_HEADER_LEN; + pdu->N_bytes += GTPU_BASE_HEADER_LEN; + } + + //write mandatory fields uint8_t *ptr = pdu->msg; - *ptr = header->flags; ptr++; *ptr = header->message_type; @@ -62,7 +75,30 @@ bool gtpu_write_header(gtpu_header_t *header, srslte::byte_buffer_t *pdu, srslte uint16_to_uint8(header->length, ptr); ptr += 2; uint32_to_uint8(header->teid, ptr); - + //write optional fields, if E, S or PN are set. + if (header->flags & (GTPU_FLAGS_EXTENDED_HDR | GTPU_FLAGS_SEQUENCE | GTPU_FLAGS_PACKET_NUM)) { + //S + if (header->flags & GTPU_FLAGS_SEQUENCE ) { + uint16_to_uint8(header->seq_number, ptr); + } else { + uint16_to_uint8(0, ptr); + } + ptr+=2; + //PN + if (header->flags & GTPU_FLAGS_PACKET_NUM ) { + *ptr = header->n_pdu; + } else { + header->n_pdu = 0; + } + ptr++; + //E + if (header->flags & GTPU_FLAGS_EXTENDED_HDR ) { + *ptr = header->next_ext_hdr_type; + } else { + *ptr = 0; + } + ptr++; + } return true; } @@ -70,26 +106,44 @@ bool gtpu_read_header(srslte::byte_buffer_t *pdu, gtpu_header_t *header, srslte: { uint8_t *ptr = pdu->msg; - pdu->msg += GTPU_HEADER_LEN; - pdu->N_bytes -= GTPU_HEADER_LEN; - - header->flags = *ptr; + header->flags = *ptr; ptr++; - header->message_type = *ptr; + header->message_type = *ptr; ptr++; uint8_to_uint16(ptr, &header->length); ptr += 2; uint8_to_uint32(ptr, &header->teid); - if(header->flags != 0x30) { - gtpu_log->error("gtpu_read_header - Unhandled header flags: 0x%x\n", header->flags); + //flags + if(!gtpu_supported_flags_check(header,gtpu_log)){ + gtpu_log->error("gtpu_read_header - Unhandled GTP-U Flags. Flags: 0x%x\n", header->flags); return false; } - if(header->message_type != 0xFF) { - gtpu_log->error("gtpu_read_header - Unhandled message type: 0x%x\n", header->message_type); + + //message_type + if(!gtpu_supported_msg_type_check(header,gtpu_log)){ + gtpu_log->error("gtpu_read_header - Unhandled GTP-U Message Type. Flags: 0x%x\n", header->message_type); return false; } + //If E, S or PN are set, header is longer + if (header->flags & (GTPU_FLAGS_EXTENDED_HDR | GTPU_FLAGS_SEQUENCE | GTPU_FLAGS_PACKET_NUM)) { + pdu->msg += GTPU_EXTENDED_HEADER_LEN; + pdu->N_bytes -= GTPU_EXTENDED_HEADER_LEN; + + uint8_to_uint16(ptr, &header->seq_number); + ptr+=2; + + header->n_pdu = *ptr; + ptr++; + + header->next_ext_hdr_type = *ptr; + ptr++; + } else { + pdu->msg += GTPU_BASE_HEADER_LEN; + pdu->N_bytes -= GTPU_BASE_HEADER_LEN; + } + return true; } diff --git a/srsenb/hdr/upper/gtpu.h b/srsenb/hdr/upper/gtpu.h index 519b831d9..3771ec402 100644 --- a/srsenb/hdr/upper/gtpu.h +++ b/srsenb/hdr/upper/gtpu.h @@ -40,24 +40,6 @@ namespace srsenb { -/**************************************************************************** - * GTPU Header - * Ref: 3GPP TS 29.281 v10.1.0 Section 5 - * - * | 8 | 7 | 6 | 5 | 4 | 3 | 2 | 1 | - * - * 1 | Version |PT | * | E | S |PN | - * 2 | Message Type | - * 3 | Length (1st Octet) | - * 4 | Length (2nd Octet) | - * 5 | TEID (1st Octet) | - * 6 | TEID (2nd Octet) | - * 7 | TEID (3rd Octet) | - * 8 | TEID (4th Octet) | - ***************************************************************************/ - -#define GTPU_HEADER_LEN 8 - class gtpu :public gtpu_interface_rrc ,public gtpu_interface_pdcp diff --git a/srsenb/src/upper/gtpu.cc b/srsenb/src/upper/gtpu.cc index 777943125..d6cc3fc89 100644 --- a/srsenb/src/upper/gtpu.cc +++ b/srsenb/src/upper/gtpu.cc @@ -122,8 +122,8 @@ void gtpu::write_pdu(uint16_t rnti, uint32_t lcid, srslte::byte_buffer_t* pdu) { gtpu_log->info_hex(pdu->msg, pdu->N_bytes, "TX PDU, RNTI: 0x%x, LCID: %d, n_bytes=%d", rnti, lcid, pdu->N_bytes); gtpu_header_t header; - header.flags = 0x30; - header.message_type = 0xFF; + header.flags = GTPU_FLAGS_VERSION_V1 | GTPU_FLAGS_GTP_PROTOCOL; + header.message_type = GTPU_MSG_DATA_PDU; header.length = pdu->N_bytes; header.teid = rnti_bearers[rnti].teids_out[lcid]; @@ -132,7 +132,10 @@ void gtpu::write_pdu(uint16_t rnti, uint32_t lcid, srslte::byte_buffer_t* pdu) servaddr.sin_addr.s_addr = htonl(rnti_bearers[rnti].spgw_addrs[lcid]); servaddr.sin_port = htons(GTPU_PORT); - gtpu_write_header(&header, pdu, gtpu_log); + if(!gtpu_write_header(&header, pdu, gtpu_log)){ + gtpu_log->error("Error writing GTP-U Header. Flags 0x%x, Message Type 0x%x\n", header.flags, header.message_type); + return; + } if (sendto(fd, pdu->msg, pdu->N_bytes, MSG_EOR, (struct sockaddr*)&servaddr, sizeof(struct sockaddr_in))<0) { perror("sendto"); } @@ -224,48 +227,50 @@ void gtpu::run_thread() pdu->N_bytes = (uint32_t) n; - if(pdu->msg[1] == 0x01) { - if(n<10) { - continue; - } - // Echo request - send response - uint16_t seq = 0; - uint8_to_uint16(&pdu->msg[8], &seq); - echo_response(client.sin_addr.s_addr, client.sin_port, seq); + gtpu_header_t header; + if(!gtpu_read_header(pdu, &header,gtpu_log)){ + continue; + } - }else{ - gtpu_header_t header; - gtpu_read_header(pdu, &header,gtpu_log); + switch(header.message_type) { - uint16_t rnti = 0; - uint16_t lcid = 0; - teidin_to_rntilcid(header.teid, &rnti, &lcid); + case GTPU_MSG_ECHO_REQUEST: + // Echo request - send response + echo_response(client.sin_addr.s_addr, client.sin_port, header.seq_number); + break; - pthread_mutex_lock(&mutex); - bool user_exists = (rnti_bearers.count(rnti) > 0); - pthread_mutex_unlock(&mutex); + case GTPU_MSG_DATA_PDU: - if(!user_exists) { - gtpu_log->error("Unrecognized RNTI for DL PDU: 0x%x - dropping packet\n", rnti); - continue; - } + uint16_t rnti = 0; + uint16_t lcid = 0; + teidin_to_rntilcid(header.teid, &rnti, &lcid); - if(lcid < SRSENB_N_SRB || lcid >= SRSENB_N_RADIO_BEARERS) { - gtpu_log->error("Invalid LCID for DL PDU: %d - dropping packet\n", lcid); - continue; - } + pthread_mutex_lock(&mutex); + bool user_exists = (rnti_bearers.count(rnti) > 0); + pthread_mutex_unlock(&mutex); - gtpu_log->info_hex(pdu->msg, pdu->N_bytes, "RX GTPU PDU rnti=0x%x, lcid=%d, n_bytes=%d", rnti, lcid, pdu->N_bytes); - - pdcp->write_sdu(rnti, lcid, pdu); - - do { - pdu = pool_allocate; - if (!pdu) { - gtpu_log->console("GTPU Buffer pool empty. Trying again...\n"); - usleep(10000); + if(!user_exists) { + gtpu_log->error("Unrecognized RNTI for DL PDU: 0x%x - dropping packet\n", rnti); + continue; } - } while(!pdu); + + if(lcid < SRSENB_N_SRB || lcid >= SRSENB_N_RADIO_BEARERS) { + gtpu_log->error("Invalid LCID for DL PDU: %d - dropping packet\n", lcid); + continue; + } + + gtpu_log->info_hex(pdu->msg, pdu->N_bytes, "RX GTPU PDU rnti=0x%x, lcid=%d, n_bytes=%d", rnti, lcid, pdu->N_bytes); + + pdcp->write_sdu(rnti, lcid, pdu); + + do { + pdu = pool_allocate; + if (!pdu) { + gtpu_log->console("GTPU Buffer pool empty. Trying again...\n"); + usleep(10000); + } + } while(!pdu); + break; } } running = false; @@ -275,20 +280,27 @@ void gtpu::echo_response(in_addr_t addr, in_port_t port, uint16_t seq) { gtpu_log->info("TX GTPU Echo Response, Seq: %d\n", seq); - uint8_t resp[12]; - bzero(resp, 12); - resp[0] = 0x32; //flags - resp[1] = 0x02; //type - uint16_to_uint8(4, &resp[2]); //length - uint32_to_uint8(0, &resp[4]); //TEID - uint16_to_uint8(seq, &resp[8]); //seq + gtpu_header_t header; + srslte::byte_buffer_t *pdu = pool_allocate; + + //header + header.flags = GTPU_FLAGS_VERSION_V1 | GTPU_FLAGS_GTP_PROTOCOL | GTPU_FLAGS_SEQUENCE; + header.message_type = GTPU_MSG_ECHO_RESPONSE; + header.teid = 0; + header.length = 4; + header.seq_number = seq; + header.n_pdu = 0; + header.next_ext_hdr_type = 0; + + gtpu_write_header(&header,pdu,gtpu_log); struct sockaddr_in servaddr; servaddr.sin_family = AF_INET; servaddr.sin_addr.s_addr = addr; servaddr.sin_port = port; - sendto(fd, resp, 12, MSG_EOR, (struct sockaddr*)&servaddr, sizeof(struct sockaddr_in)); + sendto(fd, pdu->msg, 12, MSG_EOR, (struct sockaddr*)&servaddr, sizeof(struct sockaddr_in)); + pool->deallocate(pdu); } /**************************************************************************** diff --git a/srsepc/src/mbms-gw/mbms-gw.cc b/srsepc/src/mbms-gw/mbms-gw.cc index b373df4e7..dc8199e27 100644 --- a/srsepc/src/mbms-gw/mbms-gw.cc +++ b/srsepc/src/mbms-gw/mbms-gw.cc @@ -287,29 +287,26 @@ mbms_gw::handle_sgi_md_pdu(srslte::byte_buffer_t *msg) srslte::gtpu_header_t header; //Setup GTP-U header - header.flags = 0x30; - header.message_type = 0xFF; + header.flags = GTPU_FLAGS_VERSION_V1 | GTPU_FLAGS_GTP_PROTOCOL; + header.message_type = GTPU_MSG_DATA_PDU; header.length = msg->N_bytes; header.teid = 0xAAAA; //FIXME Harcoded TEID for now //Sanity Check IP packet - if(msg->N_bytes < 20) - { + if (msg->N_bytes < 20) { m_mbms_gw_log->error("IPv4 min len: %d, drop msg len %d\n", 20, msg->N_bytes); return; } //IP Headers struct iphdr *iph = (struct iphdr *) msg->msg; - if(iph->version != 4) - { + if(iph->version != 4) { m_mbms_gw_log->warning("IPv6 not supported yet.\n"); return; } //Write GTP-U header into packet - if(!srslte::gtpu_write_header(&header, msg, m_mbms_gw_log)) - { + if (!srslte::gtpu_write_header(&header, msg, m_mbms_gw_log)) { m_mbms_gw_log->console("Error writing GTP-U header on PDU\n"); } diff --git a/srsepc/src/spgw/spgw.cc b/srsepc/src/spgw/spgw.cc index 20311cc16..0a867d51c 100644 --- a/srsepc/src/spgw/spgw.cc +++ b/srsepc/src/spgw/spgw.cc @@ -330,28 +330,24 @@ spgw::handle_sgi_pdu(srslte::byte_buffer_t *msg) srslte::gtpc_f_teid_ie enb_fteid; struct iphdr *iph = (struct iphdr *) msg->msg; - if(iph->version != 4) - { + if (iph->version != 4) { m_spgw_log->warning("IPv6 not supported yet.\n"); return; } - if(iph->tot_len < 20) - { + if (iph->tot_len < 20) { m_spgw_log->warning("Invalid IP header length.\n"); return; } pthread_mutex_lock(&m_mutex); gtp_fteid_it = m_ip_to_teid.find(iph->daddr); - if(gtp_fteid_it != m_ip_to_teid.end()) - { + if (gtp_fteid_it != m_ip_to_teid.end()) { ip_found = true; enb_fteid = gtp_fteid_it->second; } pthread_mutex_unlock(&m_mutex); - if(ip_found == false) - { + if (ip_found == false) { //m_spgw_log->console("IP Packet is not for any UE\n"); return; } @@ -363,14 +359,13 @@ spgw::handle_sgi_pdu(srslte::byte_buffer_t *msg) //Setup GTP-U header srslte::gtpu_header_t header; - header.flags = 0x30; - header.message_type = 0xFF; + header.flags = GTPU_FLAGS_VERSION_V1 | GTPU_FLAGS_GTP_PROTOCOL; + header.message_type = GTPU_MSG_DATA_PDU; header.length = msg->N_bytes; header.teid = enb_fteid.teid; //Write header into packet - if(!srslte::gtpu_write_header(&header, msg, m_spgw_log)) - { + if (!srslte::gtpu_write_header(&header, msg, m_spgw_log)) { m_spgw_log->console("Error writing GTP-U header on PDU\n"); }