diff --git a/srsue/hdr/stack/upper/tft_packet_filter.h b/srsue/hdr/stack/upper/tft_packet_filter.h index 385d67213..8aebcd926 100644 --- a/srsue/hdr/stack/upper/tft_packet_filter.h +++ b/srsue/hdr/stack/upper/tft_packet_filter.h @@ -23,6 +23,7 @@ #define SRSUE_PACKET_FILTER_H #include "srslte/asn1/liblte_mme.h" +#include "srslte/common/buffer_pool.h" namespace srsue { @@ -54,8 +55,19 @@ const uint8_t SECURITY_PARAMETER_INDEX_TYPE = 0b01100000; const uint8_t TYPE_OF_SERVICE_TYPE = 0b01110000; const uint8_t FLOW_LABEL_TYPE = 0b10000000; +// Helper const +const uint8_t IPV4_ADDR_SIZE = 4; +const uint8_t IPV6_ADDR_SIZE = 16; +const uint8_t UDP_PROTOCOL = 0x11; +const uint8_t TCP_PROTOCOL = 0x06; + // TS 24.008 Table 10.5.162 -struct tft_packet_filter_t { +class tft_packet_filter_t +{ +public: + tft_packet_filter_t(const LIBLTE_MME_PACKET_FILTER_STRUCT& tft); + bool match(const srslte::unique_byte_buffer_t& pdu); + uint8_t id; uint8_t eval_precedence; uint16_t active_filters; @@ -71,12 +83,15 @@ struct tft_packet_filter_t { uint16_t remote_port_range[2]; uint32_t security_parameter_index; uint32_t type_of_service; - uint32_t flow_label; + uint8_t flow_label[3]; - tft_packet_filter_t(const LIBLTE_MME_PACKET_FILTER_STRUCT& tft); + bool match_ip(const srslte::unique_byte_buffer_t& pdu); + bool match_protocol(const srslte::unique_byte_buffer_t& pdu); + bool match_type_of_service(const srslte::unique_byte_buffer_t& pdu); + bool match_flow_label(const srslte::unique_byte_buffer_t& pdu); + bool match_port(const srslte::unique_byte_buffer_t& pdu); }; - } // namespace srsue #endif // SRSUE_GW_METRICS_H diff --git a/srsue/src/stack/upper/gw.cc b/srsue/src/stack/upper/gw.cc index 3271cf37e..24e997f21 100644 --- a/srsue/src/stack/upper/gw.cc +++ b/srsue/src/stack/upper/gw.cc @@ -359,7 +359,7 @@ void gw::run_thread() uint8_t gw::check_tft_filter_match(const srslte::unique_byte_buffer_t& pdu) { - uint8_t lcid = DRB1_LCID; + uint8_t lcid = default_lcid; if(!tft_filter_map.empty()){ for (std::pair& filter_pair : tft_filter_map) { bool match = filter_pair.second.match(pdu); diff --git a/srsue/src/stack/upper/tft_packet_filter.cc b/srsue/src/stack/upper/tft_packet_filter.cc index cd07e8a5a..6133cf8ed 100644 --- a/srsue/src/stack/upper/tft_packet_filter.cc +++ b/srsue/src/stack/upper/tft_packet_filter.cc @@ -20,6 +20,8 @@ */ #include "srsue/hdr/stack/upper/tft_packet_filter.h" +#include +#include namespace srsue { @@ -31,6 +33,7 @@ tft_packet_filter_t::tft_packet_filter_t(const LIBLTE_MME_PACKET_FILTER_STRUCT& int idx = 0; while (idx < tft.filter_size) { switch (tft.filter[idx]) { + // IPv4 case IPV4_REMOTE_ADDR_TYPE: idx++; active_filters = IPV4_REMOTE_ADDR_FLAG; @@ -43,18 +46,14 @@ tft_packet_filter_t::tft_packet_filter_t(const LIBLTE_MME_PACKET_FILTER_STRUCT& memcpy(&ipv4_local_addr, &tft.filter[idx], 4); idx += 4; break; + //IPv6 case IPV6_REMOTE_ADDR_TYPE: - idx++; - active_filters = IPV6_REMOTE_ADDR_FLAG; - memcpy(&ipv4_local_addr, &tft.filter[idx], 16); - idx += 16; break; case IPV6_REMOTE_ADDR_LENGTH_TYPE: break; case IPV6_LOCAL_ADDR_LENGTH_TYPE: break; - case PROTOCOL_ID_TYPE: - break; + // Ports case SINGLE_LOCAL_PORT_TYPE: idx++; active_filters = SINGLE_LOCAL_PORT_FLAG; @@ -67,16 +66,135 @@ tft_packet_filter_t::tft_packet_filter_t(const LIBLTE_MME_PACKET_FILTER_STRUCT& break; case REMOTE_PORT_RANGE_TYPE: break; - case SECURITY_PARAMETER_INDEX_TYPE: + // Protocol/Next Header + case PROTOCOL_ID_TYPE: break; + // Type of service/Traffic class case TYPE_OF_SERVICE_TYPE: break; + //Flow label case FLOW_LABEL_TYPE: break; + // IPsec security parameter + case SECURITY_PARAMETER_INDEX_TYPE: + break; default: return; } } } +bool tft_packet_filter_t::match(const srslte::unique_byte_buffer_t& pdu) +{ + bool match = true; + struct iphdr* ip_pkt = (struct iphdr*)pdu->msg; + struct ipv6hdr* ip6_pkt = (struct ipv6hdr*)pdu->msg; + + // Match IP Header to active filters + if (!match_ip(pdu)) { + return false; + } + + // Check Protocol ID/Next Header Field + if (!match_protocol(pdu)) { + return false; + } + + return true; +} + +bool tft_packet_filter_t::match_ip(const srslte::unique_byte_buffer_t& pdu) +{ + struct iphdr* ip_pkt = (struct iphdr*)pdu->msg; + struct ipv6hdr* ip6_pkt = (struct ipv6hdr*)pdu->msg; + + if (ip_pkt->version == 4) { + // Check match on IPv4 packet + if (active_filters & IPV4_REMOTE_ADDR_TYPE) { + if (memcmp(&ipv4_remote_addr, &ip_pkt->daddr, IPV4_ADDR_SIZE) != 0) { + return false; + } + } + if (active_filters & IPV4_LOCAL_ADDR_TYPE) { + if (memcmp(&ipv4_local_addr, &ip_pkt->saddr, IPV4_ADDR_SIZE) != 0) { + return false; + } + } + } else if (ip_pkt->version == 6) { + // Check match on IPv6 (TODO) + } else { + // Error + return false; + } + return true; +} + +bool tft_packet_filter_t::match_protocol(const srslte::unique_byte_buffer_t& pdu) +{ + struct iphdr* ip_pkt = (struct iphdr*)pdu->msg; + struct ipv6hdr* ip6_pkt = (struct ipv6hdr*)pdu->msg; + + if (active_filters & PROTOCOL_ID_TYPE) { + if (ip_pkt->version == 4) { + // Check match on IPv4 packet + if (ip_pkt->protocol != protocol_id) { + return false; + } + } else if (ip_pkt->version == 6) { + // Check match on IPv6 (TODO) + if (ip6_pkt->nexthdr != protocol_id) { + return false; + } + } else { + // Error + return false; + } + } + return true; +} + +bool tft_packet_filter_t::match_type_of_service(const srslte::unique_byte_buffer_t& pdu) +{ + struct iphdr* ip_pkt = (struct iphdr*)pdu->msg; + + if (ip_pkt->version == 4 && (active_filters & TYPE_OF_SERVICE_FLAG)) { + // Check match on IPv4 packet + if (ip_pkt->tos != type_of_service) { + return false; + } + } + return true; +} + +bool tft_packet_filter_t::match_flow_label(const srslte::unique_byte_buffer_t& pdu) +{ + struct ipv6hdr* ip6_pkt = (struct ipv6hdr*)pdu->msg; + + if (ip6_pkt->version == 6 && (active_filters & FLOW_LABEL_FLAG)) { + // Check match on IPv4 packet + if (memcmp(ip6_pkt->flow_lbl, flow_label, 3) != 0) { + return false; + } + } + return true; +} + +bool tft_packet_filter_t::match_port(const srslte::unique_byte_buffer_t& pdu) +{ + struct iphdr* ip_pkt = (struct iphdr*)pdu->msg; + struct ipv6hdr* ip6_pkt = (struct ipv6hdr*)pdu->msg; + + if (ip_pkt->version == 4) { + switch (ip_pkt->protocol) { + case UDP_PROTOCOL: + printf("UDP protocol"); + case TCP_PROTOCOL: + printf("TCP protocol"); + default: + printf("Unhandled protocol"); + return false; + } + } + return true; +} } // namespace srsue