tsg-packetadapter/nfqnl_test.c

#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;
	struct sockaddr_in saddr = {0};

	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)
		{
			saddr.sin_family = AF_INET6;
			saddr.sin_addr.s_addr = inet_addr(pkt_parser.internal_ipv6.dst_addr);
		}
		else if (pkt_parser.external_ip_version == IPv6 && pkt_parser.internal_ip_version == IPv4)
		{
			saddr.sin_family = AF_INET;
			saddr.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 *)&saddr, sizeof(saddr)) == -1)
		{
			LOG_ERROR("Failed at send(), %d: %s", errno, strerror(errno));
			goto end;
		}
		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;
}