diff options
Diffstat (limited to 'nfqnl_test.c')
| -rw-r--r-- | nfqnl_test.c | 869 |
1 files changed, 0 insertions, 869 deletions
diff --git a/nfqnl_test.c b/nfqnl_test.c deleted file mode 100644 index a6b20c8..0000000 --- a/nfqnl_test.c +++ /dev/null @@ -1,869 +0,0 @@ -#include <stdlib.h> -#include <stdbool.h> -#include <string.h> -#include <errno.h> -#include <unistd.h> -#include <arpa/inet.h> - -#include <netinet/ip.h> -#include <netinet/ip6.h> - -#include <linux/tcp.h> -#include <linux/netfilter.h> // for NF_ACCEPT -#include <libnetfilter_queue/libnetfilter_queue.h> - -#include "log.h" - -#define GTP_TPDU 255 -#define GTP1U_PORT 2152 -#define GTP1_F_MASK 0x07 - -#define UDP_HEADER_LEN 8 -#define TCP_HEADER_LEN 20 -#define TCP_OPTLENMAX 40 - -#define IPV4_HEADER_LEN 20 -#define IPV6_HEADER_LEN 40 - -#define IP_GET_RAW_VER(raw_pkt) ((((raw_pkt)[0] & 0xf0) >> 4)) - -#define IPV6_GET_PLEN(ip6_hdr) ((uint16_t)ntohs((ip6_hdr)->ip6_hdrun.ip6_un1.ip6_un1_plen)) -#define IPV6_GET_NH(ip6_hdr) ((ip6_hdr)->ip6_hdrun.ip6_un1.ip6_un1_nxt) -#define IPV6_GET_SRC_ADDR(ip6_hdr) ((ip6_hdr)->ip6_hdrun2.ip6_un2.ip6_src) -#define IPV6_GET_DST_ADDR(ip6_hdr) ((ip6_hdr)->ip6_hdrun2.ip6_un2.ip6_dst) - -#define IPV4_GET_HLEN(ip4_hdr) (((ip4_hdr)->ip_verhl & 0x0f) << 2) -#define IPV4_GET_IPPROTO(ip4_hdr) ((ip4_hdr)->ip_proto) -#define IPV4_GET_IPLEN(ip4_hdr) ((uint16_t)ntohs((ip4_hdr)->ip_len)) -#define IPV4_GET_SRC_ADDR(ip4_hdr) ((ip4_hdr)->ip4_hdrun1.ip4_un1.ip_src) -#define IPV4_GET_DST_ADDR(ip4_hdr) ((ip4_hdr)->ip4_hdrun1.ip4_un1.ip_dst) - -#define UDP_GET_LEN(udp_hdr) ((uint16_t)ntohs((udp_hdr)->udp_len)) -#define UDP_GET_SRC_PORT(udp_hdr) ((uint16_t)ntohs((udp_hdr)->udp_src_port)) -#define UDP_GET_DST_PORT(udp_hdr) ((uint16_t)ntohs((udp_hdr)->udp_dst_port)) - -#define TCP_GET_HLEN(tcp_hdr) ((((tcp_hdr)->th_offx2 & 0xf0) >> 4) << 2) -#define TCP_GET_SRC_PORT(tcp_hdr) ((uint16_t)ntohs((tcp_hdr)->th_sport)) -#define TCP_GET_DST_PORT(tcp_hdr) ((uint16_t)ntohs((tcp_hdr)->th_dport)) - -#define GTP1_GET_TYPE(gtp1_hdr) ((gtp1_hdr)->type) -#define GTP1_GET_FLAGS(gtp1_hdr) ((gtp1_hdr)->flags >> 5) -#define GTP1_GET_HLEN(gtp1_hdr) (((gtp1_hdr)->flags & GTP1_F_MASK) > 0 ? 12 : 8) - -/////////////////////////////////////////////////////////////////////////////// -// IPv4 IPv6 UDP GPT Header Struct -/////////////////////////////////////////////////////////////////////////////// - -typedef struct ipv6_header_s -{ - union - { - struct ip6_un1_ - { - uint32_t ip6_un1_flow; /* 20 bits of flow-ID */ - uint16_t ip6_un1_plen; /* payload length */ - uint8_t ip6_un1_nxt; /* next header */ - uint8_t ip6_un1_hlim; /* hop limit */ - } ip6_un1; - uint8_t ip6_un2_vfc; /* 4 bits version, top 4 bits class */ - } ip6_hdrun; - - union - { - struct - { - uint32_t ip6_src[4]; - uint32_t ip6_dst[4]; - } ip6_un2; - uint16_t ip6_addrs[16]; - } ip6_hdrun2; -} ipv6_header_t; - -typedef struct ipv4_header_s -{ - uint8_t ip_verhl; // version & header length - uint8_t ip_tos; - uint16_t ip_len; - uint16_t ip_id; - uint16_t ip_off; - uint8_t ip_ttl; - uint8_t ip_proto; - uint16_t ip_csum; - union - { - struct - { - struct in_addr ip_src; - struct in_addr ip_dst; - } ip4_un1; - uint16_t ip_addrs[4]; - } ip4_hdrun1; -} ipv4_header_t; - -typedef struct tcp_header_s -{ - uint16_t th_sport; /**< source port */ - uint16_t th_dport; /**< destination port */ - uint32_t th_seq; /**< sequence number */ - uint32_t th_ack; /**< acknowledgement number */ - uint8_t th_offx2; /**< offset and reserved */ - uint8_t th_flags; /**< pkt flags */ - uint16_t th_win; /**< pkt window */ - uint16_t th_sum; /**< checksum */ - uint16_t th_urp; /**< urgent pointer */ -} __attribute__((__packed__)) tcp_header_t; - -typedef struct udp_header_s -{ - uint16_t udp_src_port; - uint16_t udp_dst_port; - uint16_t udp_len; - uint16_t udp_sum; -} __attribute__((__packed__)) udp_header_t; - -enum gtp_version_e -{ - GTP_V0 = 0, - GTP_V1, -}; - -/* According to 3GPP TS 29.060. */ -typedef struct gtp1_header_s -{ - uint8_t flags; - uint8_t type; - uint16_t length; - uint32_t tid; -} __attribute__((packed)) gtp1_header_t; - -/////////////////////////////////////////////////////////////////////////////// -// IPv4 IPv6 UDP GPT Header Parser -/////////////////////////////////////////////////////////////////////////////// - -typedef enum network_mode_s -{ - NONE = 0, - IPv4 = 1, - IPv6 = 2, - TCP = 3, - UDP = 4, -} network_mode_t; - -typedef struct pkt_info_s -{ - uint32_t id; - uint8_t *payload; - uint32_t payload_len; -} pkt_info_t; - -typedef struct ipv4_info_s -{ - char src_addr[INET_ADDRSTRLEN]; - char dst_addr[INET_ADDRSTRLEN]; - - ipv4_header_t *hdr; - uint8_t *payload; - - uint32_t hdr_len; - uint32_t opts_len; - uint32_t payload_len; -} ipv4_info_t; - -typedef struct ipv6_info_s -{ - char src_addr[INET6_ADDRSTRLEN]; - char dst_addr[INET6_ADDRSTRLEN]; - - ipv6_header_t *hdr; - uint8_t *payload; - - uint32_t hdr_len; - uint32_t payload_len; -} ipv6_info_t; - -typedef struct udp_info_s -{ - uint16_t src_port; - uint16_t dst_port; - - udp_header_t *hdr; - uint8_t *payload; - - uint32_t hdr_len; - uint32_t payload_len; -} udp_info_t; - -typedef struct tcp_info_s -{ - uint16_t src_port; - uint16_t dst_port; - - tcp_header_t *hdr; - uint8_t *payload; - - uint32_t opt_len; - uint32_t hdr_len; - uint32_t payload_len; -} tcp_info_t; - -typedef struct gtp_info_s -{ - gtp1_header_t *hdr; - uint8_t *payload; - - uint32_t hdr_len; - uint32_t payload_len; -} gtp_info_t; - -typedef struct pkt_paser_s -{ - pkt_info_t raw_pkt; - - ipv4_info_t external_ipv4; - ipv6_info_t external_ipv6; - udp_info_t external_udp; - gtp_info_t external_gtp; - ipv4_info_t internal_ipv4; - ipv6_info_t internal_ipv6; - udp_info_t internal_udp; - tcp_info_t internal_tcp; - - network_mode_t external_ip_version; - network_mode_t external_l4_version; - network_mode_t internal_ip_version; - network_mode_t internal_l4_version; - -} pkt_paser_t; - -/////////////////////////////////////////////////////////////////////////////// -// Packet Parser API -/////////////////////////////////////////////////////////////////////////////// - -static void dump_info(pkt_paser_t *pkt_parser) -{ - - LOG_DEBUG("raw_pkt: {id: %u, data_len: %u}", pkt_parser->raw_pkt.id, pkt_parser->raw_pkt.payload_len); - - if (pkt_parser->external_ipv4.hdr) - { - LOG_DEBUG("id: %u, external_ipv4: {src_addr:%s,dst_addr:%s,hdr_len: %u, opt_len: %u, data_len: %u}", - pkt_parser->raw_pkt.id, - pkt_parser->external_ipv4.src_addr, - pkt_parser->external_ipv4.dst_addr, - pkt_parser->external_ipv4.hdr_len, - pkt_parser->external_ipv4.opts_len, - pkt_parser->external_ipv4.payload_len); - } - if (pkt_parser->external_ipv6.hdr) - { - LOG_DEBUG("id: %u, external_ipv6: {src_addr:%s,dst_addr:%s,hdr_len: %u, data_len: %u}", - pkt_parser->raw_pkt.id, - pkt_parser->external_ipv6.src_addr, - pkt_parser->external_ipv6.dst_addr, - pkt_parser->external_ipv6.hdr_len, - pkt_parser->external_ipv6.payload_len); - } - if (pkt_parser->external_udp.hdr) - { - LOG_DEBUG("id: %u, external_udp: {src_port: %u, dst_port: %u, hdr_len: %u, data_len: %u}", - pkt_parser->raw_pkt.id, - pkt_parser->external_udp.src_port, - pkt_parser->external_udp.dst_port, - pkt_parser->external_udp.hdr_len, - pkt_parser->external_udp.payload_len); - } - - if (pkt_parser->external_gtp.hdr) - { - LOG_DEBUG("id: %u, external_gtp: {hdr_len: %u, data_len: %u}", - pkt_parser->raw_pkt.id, - pkt_parser->external_gtp.hdr_len, - pkt_parser->external_gtp.payload_len); - } - if (pkt_parser->internal_ipv4.hdr) - { - LOG_DEBUG("id: %u, internal_ipv4: {src_addr:%s,dst_addr:%s,hdr_len: %u, opt_len: %u, data_len: %u}", - pkt_parser->raw_pkt.id, - pkt_parser->internal_ipv4.src_addr, - pkt_parser->internal_ipv4.dst_addr, - pkt_parser->internal_ipv4.hdr_len, - pkt_parser->internal_ipv4.opts_len, - pkt_parser->internal_ipv4.payload_len); - } - if (pkt_parser->internal_ipv6.hdr) - { - LOG_DEBUG("id: %u, interna_ipv6: {src_addr:%s,dst_addr:%s,hdr_len: %u, data_len: %u}", - pkt_parser->raw_pkt.id, - pkt_parser->internal_ipv6.src_addr, - pkt_parser->internal_ipv6.dst_addr, - pkt_parser->internal_ipv6.hdr_len, - pkt_parser->internal_ipv6.payload_len); - } - if (pkt_parser->internal_udp.hdr) - { - LOG_DEBUG("id: %u, internal_udp: {src_port: %u, dst_port: %u, hdr_len: %u, data_len: %u}", - pkt_parser->raw_pkt.id, - pkt_parser->internal_udp.src_port, - pkt_parser->internal_udp.dst_port, - pkt_parser->internal_udp.hdr_len, - pkt_parser->internal_udp.payload_len); - } - if (pkt_parser->internal_tcp.hdr) - { - LOG_DEBUG("id: %u, internal_tcp: {src_port: %u, dst_port: %u, hdr_len: %u, data_len:%u}", - pkt_parser->raw_pkt.id, - pkt_parser->internal_tcp.src_port, - pkt_parser->internal_tcp.dst_port, - pkt_parser->internal_tcp.hdr_len, - pkt_parser->internal_tcp.payload_len); - } -} - -static int decode_gtp(gtp_info_t *packet, const uint8_t *data, uint32_t len) -{ - if (len < sizeof(gtp1_header_t)) - { - LOG_ERROR("Parser GTP Header: packet length too small %d", len); - return -1; - } - - packet->hdr = (gtp1_header_t *)data; - if (GTP1_GET_FLAGS(packet->hdr) != GTP_V1) - { - LOG_ERROR("Parser GTP Header: invalid gtp flags %d", GTP1_GET_FLAGS(packet->hdr)); - return -1; - } - - if (GTP1_GET_TYPE(packet->hdr) != GTP_TPDU) - { - LOG_ERROR("Parser GTP Header: invalid gtp type %d", GTP1_GET_TYPE(packet->hdr)); - return -1; - } - - /* From 29.060: "This field shall be present if and only if any one or - * more of the S, PN and E flags are set.". - * - * If any of the bit is set, then the remaining ones also have to be set. - */ - packet->hdr_len = GTP1_GET_HLEN(packet->hdr); - packet->payload = (uint8_t *)data + packet->hdr_len; - packet->payload_len = len - packet->hdr_len; - - return 0; -} - -static int decode_udp(udp_info_t *packet, const uint8_t *data, uint32_t len) -{ - if (len < UDP_HEADER_LEN) - { - LOG_ERROR("Parser UDP Header: packet length too small %d", len); - return -1; - } - - packet->hdr = (udp_header_t *)data; - // 检查 UDP header len - if (len < UDP_GET_LEN(packet->hdr)) - { - LOG_ERROR("Parser UDP Header: UDP packet too small %d", len); - return -1; - } - - // 检查 UDP header len - if (len != UDP_GET_LEN(packet->hdr)) - { - LOG_ERROR("Parser UDP Header: invalid UDP header length %d", UDP_GET_LEN(packet->hdr)); - return -1; - } - - packet->src_port = UDP_GET_SRC_PORT(packet->hdr); - packet->dst_port = UDP_GET_DST_PORT(packet->hdr); - - packet->hdr_len = UDP_HEADER_LEN; - packet->payload = (uint8_t *)data + UDP_HEADER_LEN; - packet->payload_len = len - UDP_HEADER_LEN; - - return 0; -} - -static int decode_tcp(tcp_info_t *packet, const uint8_t *data, uint32_t len) -{ - if (len < TCP_HEADER_LEN) - { - LOG_ERROR("Parser TCP Header: packet length too small %d", len); - return -1; - } - - packet->hdr = (tcp_header_t *)data; - uint8_t hlen = TCP_GET_HLEN(packet->hdr); - if (len < hlen) - { - LOG_ERROR("Parser TCP Header: TCP packet too small %d", len); - return -1; - } - - uint8_t tcp_opt_len = hlen - TCP_HEADER_LEN; - if (tcp_opt_len > TCP_OPTLENMAX) - { - LOG_ERROR("Parser TCP Header: invalid opt length %d", tcp_opt_len); - return -1; - } - - packet->opt_len = tcp_opt_len; - packet->src_port = TCP_GET_SRC_PORT(packet->hdr); - packet->dst_port = TCP_GET_DST_PORT(packet->hdr); - - packet->hdr_len = hlen; - packet->payload = (uint8_t *)data + packet->hdr_len; - packet->payload_len = len = packet->hdr_len; - - return 0; -} - -static int decode_ipv6(ipv6_info_t *packet, const uint8_t *data, uint32_t len) -{ - if (len < IPV6_HEADER_LEN) - { - LOG_ERROR("Parser IPv6 Header: packet length too small %d", len); - return -1; - } - - // 检查 IPv6 header version - if (IP_GET_RAW_VER(data) != 6) - { - LOG_ERROR("Parser IPv6 Header: unknown IP version %d", IP_GET_RAW_VER(data)); - return -1; - } - - packet->hdr = (ipv6_header_t *)data; - if (len < (IPV6_HEADER_LEN + IPV6_GET_PLEN(packet->hdr))) - { - LOG_ERROR("Parser IPv6 Header: trunc packet"); - return -1; - } - - if (len != (IPV6_HEADER_LEN + IPV6_GET_PLEN(packet->hdr))) - { - LOG_ERROR("Parser IPv6 Header: invalid payload length %d", IPV6_GET_PLEN(packet->hdr)); - return -1; - } - - inet_ntop(AF_INET6, &IPV6_GET_SRC_ADDR(packet->hdr), packet->src_addr, sizeof(packet->src_addr)); - inet_ntop(AF_INET6, &IPV6_GET_DST_ADDR(packet->hdr), packet->dst_addr, sizeof(packet->dst_addr)); - - packet->hdr_len = IPV6_HEADER_LEN; - packet->payload = (uint8_t *)data + packet->hdr_len; - packet->payload_len = len - packet->hdr_len; - - return 0; -} - -static int decode_ipv4(ipv4_info_t *packet, const uint8_t *data, uint32_t len) -{ - // 检查包长是否大于 IPv4 header - if (len < IPV4_HEADER_LEN) - { - LOG_ERROR("Parser IPv4 Header: packet length too small %d", len); - return -1; - } - - // 检查 IPv4 header version - if (IP_GET_RAW_VER(data) != 4) - { - LOG_ERROR("Parser IPv4 Header: unknown IP version %d", IP_GET_RAW_VER(data)); - return -1; - } - - packet->hdr = (ipv4_header_t *)data; - // 检查 IPv4 header length - if (IPV4_GET_HLEN(packet->hdr) < IPV4_HEADER_LEN) - { - LOG_ERROR("Parser IPv4 Header: invalid IP header length %d", IPV4_GET_HLEN(packet->hdr)); - return -1; - } - - // 检查 IPv4 header total length - if (IPV4_GET_IPLEN(packet->hdr) < IPV4_GET_HLEN(packet->hdr)) - { - LOG_ERROR("Parser IPv4 Header: invalid IP header total length %d", IPV4_GET_IPLEN(packet->hdr)); - return -1; - } - - // 检查是否 IP 分片 - if (len < IPV4_GET_IPLEN(packet->hdr)) - { - LOG_ERROR("Parser IPv4 Header: trunc packet"); - return -1; - } - - inet_ntop(AF_INET, &IPV4_GET_SRC_ADDR(packet->hdr), packet->src_addr, sizeof(packet->src_addr)); - inet_ntop(AF_INET, &IPV4_GET_DST_ADDR(packet->hdr), packet->dst_addr, sizeof(packet->dst_addr)); - - packet->hdr_len = IPV4_GET_HLEN(packet->hdr); - packet->opts_len = packet->hdr_len - IPV4_HEADER_LEN; - packet->payload_len = len - packet->hdr_len; - packet->payload = (uint8_t *)data + packet->hdr_len; - - return 0; -} - -static int decode_internal_ip_pkt(pkt_paser_t *pkt_parser, const uint8_t *data, uint32_t len) -{ - int next_protocol = 0; - uint8_t *payload = NULL; - uint32_t payload_len = 0; - - // IPv4/IPv6 - if (len < IPV4_HEADER_LEN) - { - LOG_ERROR("Parser Internal Raw header: packet length too small %d", len); - return -1; - } - - if (IP_GET_RAW_VER(data) == 4) - { - if (decode_ipv4(&(pkt_parser->internal_ipv4), data, len) == -1) - { - return -1; - } - - pkt_parser->internal_ip_version = IPv4; - payload = pkt_parser->internal_ipv4.payload; - payload_len = pkt_parser->internal_ipv4.payload_len; - next_protocol = IPV4_GET_IPPROTO(pkt_parser->internal_ipv4.hdr); - } - else if (IP_GET_RAW_VER(data) == 6) - { - if (decode_ipv6(&(pkt_parser->internal_ipv6), data, len) == -1) - { - return -1; - } - pkt_parser->internal_ip_version = IPv6; - payload = pkt_parser->internal_ipv6.payload; - payload_len = pkt_parser->internal_ipv6.payload_len; - next_protocol = IPV6_GET_NH(pkt_parser->internal_ipv6.hdr); - } - else - { - LOG_ERROR("Unknown Internal IP version %d", IP_GET_RAW_VER(data)); - return -1; - } - - // TCP/UDP - if (next_protocol == IPPROTO_UDP) - { - if (decode_udp(&(pkt_parser->internal_udp), payload, payload_len) == -1) - { - return -1; - } - pkt_parser->internal_l4_version = UDP; - return 0; - } - else if (next_protocol == IPPROTO_TCP) - { - if (decode_tcp(&(pkt_parser->internal_tcp), payload, payload_len) == -1) - { - return -1; - } - pkt_parser->internal_l4_version = TCP; - return 0; - } - else - { - LOG_ERROR("Unknown Internal L4 next_protocol version %d", next_protocol); - return -1; - } -} - -static int decode_external_ip_pkt(pkt_paser_t *pkt_parser, const uint8_t *data, uint32_t len) -{ - int next_protocol = 0; - uint8_t *payload = NULL; - uint32_t payload_len = 0; - - // IPv4/IPv6 - if (len < IPV4_HEADER_LEN) - { - LOG_ERROR("Parser External Raw header: packet length too small %d", len); - return -1; - } - - if (IP_GET_RAW_VER(data) == 4) - { - if (decode_ipv4(&(pkt_parser->external_ipv4), data, len) == -1) - { - return -1; - } - pkt_parser->external_ip_version = IPv4; - payload = pkt_parser->external_ipv4.payload; - payload_len = pkt_parser->external_ipv4.payload_len; - next_protocol = IPV4_GET_IPPROTO(pkt_parser->external_ipv4.hdr); - } - else if (IP_GET_RAW_VER(data) == 6) - { - if (decode_ipv6(&(pkt_parser->external_ipv6), data, len) == -1) - { - return -1; - } - pkt_parser->external_ip_version = IPv6; - payload = pkt_parser->external_ipv6.payload; - payload_len = pkt_parser->external_ipv6.payload_len; - next_protocol = IPV6_GET_NH(pkt_parser->external_ipv6.hdr); - } - else - { - LOG_ERROR("Unknown External IP version %d", IP_GET_RAW_VER(data)); - return -1; - } - - // TCP/UDP - if (next_protocol == IPPROTO_UDP) - { - if (decode_udp(&(pkt_parser->external_udp), payload, payload_len) == -1) - { - return -1; - } - pkt_parser->external_l4_version = UDP; - } - else if (next_protocol == IPPROTO_TCP) - { - pkt_parser->external_l4_version = TCP; - LOG_ERROR("Unknown External L4 next_protocol version %d", next_protocol); - return -1; - } - else - { - LOG_ERROR("Unknown External L4 next_protocol version %d", next_protocol); - return -1; - } - - // GTP - if (decode_gtp(&(pkt_parser->external_gtp), pkt_parser->external_udp.payload, pkt_parser->external_udp.payload_len) == -1) - { - return -1; - } - - return 0; -} - -/////////////////////////////////////////////////////////////////////////////// -// NFQ API -/////////////////////////////////////////////////////////////////////////////// - -/* - * nfmsg : message objetc that contains the packet - * nfa : Netlink packet data handle - */ -static int packet_handler_cb(struct nfq_q_handle *qh, struct nfgenmsg *nfmsg, struct nfq_data *nfa, void *data) -{ - int offest = 0; - int raw_ip_fd = 0; - int packet_len = 0; - pkt_paser_t pkt_parser = {0}; - unsigned char *packet_data = NULL; - struct nfqnl_msg_packet_hdr *packet_hdr = NULL; - - packet_hdr = nfq_get_msg_packet_hdr(nfa); - if (packet_hdr == NULL) - { - LOG_ERROR("Failed at nfq_get_msg_packet_hdr()"); - goto end; - } - - packet_len = nfq_get_payload(nfa, &packet_data); - if (packet_len <= 0) - { - LOG_ERROR("Failed at nfq_get_payload()"); - goto end; - } - - pkt_parser.raw_pkt.id = ntohl(packet_hdr->packet_id); - pkt_parser.raw_pkt.payload = packet_data; - pkt_parser.raw_pkt.payload_len = packet_len; - if (decode_external_ip_pkt(&pkt_parser, pkt_parser.raw_pkt.payload, pkt_parser.raw_pkt.payload_len) == -1) - { - goto end; - } - - if (decode_internal_ip_pkt(&pkt_parser, pkt_parser.external_gtp.payload, pkt_parser.external_gtp.payload_len) == -1) - { - goto end; - } - - /* - * NF_DROP : discarded the packet - * NF_ACCEPT : the packet passes, continue iterations - * NF_QUEUE : inject the packet into a different queue (the target queue number is in the high 16 bits of the verdict) - * NF_REPEAT : iterate the same cycle once more - * NF_STOP : accept, but don't continue iterations - */ - // nfq_set_verdict() - // nfq_set_verdict2() - // nfq_set_verdict_batch() - // nfq_set_verdict_batch2() - // nfq_set_verdict_mark() - - if (pkt_parser.external_ip_version == IPv4) - { - offest += pkt_parser.external_ipv4.hdr_len; - } - if (pkt_parser.external_ip_version == IPv6) - { - offest += pkt_parser.external_ipv6.hdr_len; - } - - offest += pkt_parser.external_udp.hdr_len; - offest += pkt_parser.external_gtp.hdr_len; - - dump_info(&pkt_parser); - LOG_DEBUG("Offset : %d", offest); - - if (offest > 0) - { - if ((pkt_parser.external_ip_version == IPv4 && pkt_parser.internal_ip_version == IPv4) || (pkt_parser.external_ip_version == IPv6 && pkt_parser.internal_ip_version == IPv6)) - { - return nfq_set_verdict(qh, pkt_parser.raw_pkt.id, NF_ACCEPT, packet_len - offest, packet_data + offest); - } - if (pkt_parser.external_ip_version == IPv4 && pkt_parser.internal_ip_version == IPv6) - { - struct sockaddr_in6 saddr6 = {0}; - saddr6.sin6_family = PF_INET6; - memcpy(saddr6.sin6_addr.s6_addr, pkt_parser.internal_ipv6.hdr->ip6_hdrun2.ip6_un2.ip6_dst, sizeof(struct in6_addr)); - raw_ip_fd = socket(PF_INET6, SOCK_RAW, IPPROTO_RAW); - if (sendto(raw_ip_fd, packet_data + offest, packet_len - offest, 0, (struct sockaddr *)&saddr6, sizeof(saddr6)) == -1) - { - LOG_ERROR("Failed at send(), %d: %s", errno, strerror(errno)); - close(raw_ip_fd); - goto end; - } - close(raw_ip_fd); - } - else if (pkt_parser.external_ip_version == IPv6 && pkt_parser.internal_ip_version == IPv4) - { - struct sockaddr_in saddr4 = {0}; - saddr4.sin_family = PF_INET; - saddr4.sin_addr.s_addr = inet_addr(pkt_parser.internal_ipv4.dst_addr); - raw_ip_fd = socket(PF_INET, SOCK_RAW, IPPROTO_RAW); - if (sendto(raw_ip_fd, packet_data + offest, packet_len - offest, 0, (struct sockaddr *)&saddr4, sizeof(saddr4)) == -1) - { - LOG_ERROR("Failed at send(), %d: %s", errno, strerror(errno)); - close(raw_ip_fd); - goto end; - } - close(raw_ip_fd); - } - return nfq_set_verdict(qh, pkt_parser.raw_pkt.id, NF_DROP, 0, NULL); - } - -end: - return nfq_set_verdict(qh, pkt_parser.raw_pkt.id, NF_ACCEPT, 0, NULL); -} - -/* - * doc : http://www.netfilter.org/projects/libnetfilter_queue/doxygen/html/ - * Library setup : http://www.netfilter.org/projects/libnetfilter_queue/doxygen/html/group__LibrarySetup.html - * Queue handling : http://www.netfilter.org/projects/libnetfilter_queue/doxygen/html/group__Queue.html - * Message parsing : http://www.netfilter.org/projects/libnetfilter_queue/doxygen/html/group__Parsing.html - */ -int main(int argc, char **argv) -{ - int fd; - int rv; - uint32_t queue = 1; - struct nfq_handle *handle; - struct nfq_q_handle *q_handle; - char buf[4096] __attribute__((aligned)); - - if (argc == 2) - { - queue = atoi(argv[1]); - if (queue > 65535) - { - fprintf(stderr, "Usage: %s [<0-65535>]\n", argv[0]); - return 0; - } - } - LOG_DEBUG("Using queue: %d", queue); - - handle = nfq_open(); - if (handle == NULL) - { - LOG_ERROR("Failed at nfq_open(), %d: %s", errno, strerror(errno)); - goto error; - } - - if (nfq_unbind_pf(handle, AF_INET) < 0) - { - LOG_ERROR("Failed at nfq_unbind_pf(), %d: %s", errno, strerror(errno)); - goto error; - } - - if (nfq_bind_pf(handle, AF_INET) < 0) - { - LOG_ERROR("Failed at nfq_bind_pf(), %d: %s", errno, strerror(errno)); - goto error; - } - - q_handle = nfq_create_queue(handle, queue, &packet_handler_cb, NULL); - if (q_handle == NULL) - { - LOG_ERROR("Failed at nfq_create_queue(), %d: %s", errno, strerror(errno)); - goto error; - } - - /* - * NFQNL_COPY_NONE - noop, do not use it - * NFQNL_COPY_META - copy only packet metadata - * NFQNL_COPY_PACKET - copy entire packet - */ - if (nfq_set_mode(q_handle, NFQNL_COPY_PACKET, 0xffff) < 0) - { - LOG_ERROR("Failed at nfq_set_mode(NFQNL_COPY_PACKET), %d: %s", errno, strerror(errno)); - goto error; - } - - if (nfq_set_queue_maxlen(q_handle, 65535) < 0) - { - LOG_ERROR("Failed at nfq_set_queue_maxlen(65535), %d: %s", errno, strerror(errno)); - goto error; - } - - LOG_DEBUG("Waiting for packets..."); - - fd = nfq_fd(handle); - for (;;) - { - if ((rv = recv(fd, buf, sizeof(buf), 0)) >= 0) - { - nfq_handle_packet(handle, buf, rv); - continue; - } - /* - * if your application is too slow to digest the packets that - * are sent from kernel-space, the socket buffer that we use - * to enqueue packets may fill up returning ENOBUFS. Depending - * on your application, this error may be ignored. Please, see - * the doxygen documentation of this library on how to improve - * this situation. - */ - if (rv < 0 && errno == ENOBUFS) - { - LOG_ERROR("Losing packets !!!"); - continue; - } - LOG_ERROR("Failed at recv(), %d: %s", errno, strerror(errno)); - break; - } - -error: - if (q_handle) - { - nfq_destroy_queue(q_handle); - } - - if (handle) - { - nfq_close(handle); - } - - return 0; -} |