From 86a43b4d325ddc850fa9dc4711670880f35b11e8 Mon Sep 17 00:00:00 2001 From: lijia Date: Wed, 24 Oct 2018 09:36:45 +0800 Subject: create new project. --- src/packet_io/nf-queue.c | 213 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 213 insertions(+) create mode 100644 src/packet_io/nf-queue.c (limited to 'src/packet_io/nf-queue.c') diff --git a/src/packet_io/nf-queue.c b/src/packet_io/nf-queue.c new file mode 100644 index 0000000..c2bc6cc --- /dev/null +++ b/src/packet_io/nf-queue.c @@ -0,0 +1,213 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include + +/* only for NFQA_CT, not needed otherwise: */ +#include + +static struct mnl_socket *nl; + +static struct nlmsghdr * +nfq_hdr_put(char *buf, int type, uint32_t queue_num) +{ + struct nlmsghdr *nlh = mnl_nlmsg_put_header(buf); + nlh->nlmsg_type = (NFNL_SUBSYS_QUEUE << 8) | type; + nlh->nlmsg_flags = NLM_F_REQUEST; + + struct nfgenmsg *nfg = mnl_nlmsg_put_extra_header(nlh, sizeof(*nfg)); + nfg->nfgen_family = AF_UNSPEC; + nfg->version = NFNETLINK_V0; + nfg->res_id = htons(queue_num); + + return nlh; +} + +static void +nfq_send_verdict(int queue_num, uint32_t id) +{ + char buf[MNL_SOCKET_BUFFER_SIZE]; + struct nlmsghdr *nlh; + struct nlattr *nest; + + nlh = nfq_hdr_put(buf, NFQNL_MSG_VERDICT, queue_num); + nfq_nlmsg_verdict_put(nlh, id, NF_ACCEPT); + + /* example to set the connmark. First, start NFQA_CT section: */ + nest = mnl_attr_nest_start(nlh, NFQA_CT); + + /* then, add the connmark attribute: */ + mnl_attr_put_u32(nlh, CTA_MARK, htonl(42)); + /* more conntrack attributes, e.g. CTA_LABEL, could be set here */ + + /* end conntrack section */ + mnl_attr_nest_end(nlh, nest); + + if (mnl_socket_sendto(nl, nlh, nlh->nlmsg_len) < 0) { + perror("mnl_socket_send"); + exit(EXIT_FAILURE); + } +} + +static int queue_cb(const struct nlmsghdr *nlh, void *data) +{ + struct nfqnl_msg_packet_hdr *ph = NULL; + struct nlattr *attr[NFQA_MAX+1] = {}; + uint32_t id = 0, skbinfo; + struct nfgenmsg *nfg; + uint16_t plen; + + if (nfq_nlmsg_parse(nlh, attr) < 0) { + perror("problems parsing"); + return MNL_CB_ERROR; + } + + nfg = mnl_nlmsg_get_payload(nlh); + + if (attr[NFQA_PACKET_HDR] == NULL) { + fputs("metaheader not set\n", stderr); + return MNL_CB_ERROR; + } + + ph = mnl_attr_get_payload(attr[NFQA_PACKET_HDR]); + + plen = mnl_attr_get_payload_len(attr[NFQA_PAYLOAD]); + /* void *payload = mnl_attr_get_payload(attr[NFQA_PAYLOAD]); */ + + skbinfo = attr[NFQA_SKB_INFO] ? ntohl(mnl_attr_get_u32(attr[NFQA_SKB_INFO])) : 0; + + if (attr[NFQA_CAP_LEN]) { + uint32_t orig_len = ntohl(mnl_attr_get_u32(attr[NFQA_CAP_LEN])); + if (orig_len != plen) + printf("truncated "); + } + + if (skbinfo & NFQA_SKB_GSO) + printf("GSO "); + + id = ntohl(ph->packet_id); + printf("packet received (id=%u hw=0x%04x hook=%u, payload len %u", + id, ntohs(ph->hw_protocol), ph->hook, plen); + + /* + * ip/tcp checksums are not yet valid, e.g. due to GRO/GSO. + * The application should behave as if the checksums are correct. + * + * If these packets are later forwarded/sent out, the checksums will + * be corrected by kernel/hardware. + */ + if (skbinfo & NFQA_SKB_CSUMNOTREADY) + printf(", checksum not ready"); + puts(")"); + + nfq_send_verdict(ntohs(nfg->res_id), id); + + return MNL_CB_OK; +} + +int main(int argc, char *argv[]) +{ + char *buf; + /* largest possible packet payload, plus netlink data overhead: */ + size_t sizeof_buf = 0xffff + (MNL_SOCKET_BUFFER_SIZE/2); + struct nlmsghdr *nlh; + int ret; + unsigned int portid, queue_num; + + if (argc != 2) { + printf("Usage: %s [queue_num]\n", argv[0]); + exit(EXIT_FAILURE); + } + queue_num = atoi(argv[1]); + + nl = mnl_socket_open(NETLINK_NETFILTER); + if (nl == NULL) { + perror("mnl_socket_open"); + exit(EXIT_FAILURE); + } + + if (mnl_socket_bind(nl, 0, MNL_SOCKET_AUTOPID) < 0) { + perror("mnl_socket_bind"); + exit(EXIT_FAILURE); + } + portid = mnl_socket_get_portid(nl); + + buf = malloc(sizeof_buf); + if (!buf) { + perror("allocate receive buffer"); + exit(EXIT_FAILURE); + } + + /* PF_(UN)BIND is not needed with kernels 3.8 and later */ + nlh = nfq_hdr_put(buf, NFQNL_MSG_CONFIG, 0); + nfq_nlmsg_cfg_put_cmd(nlh, AF_INET, NFQNL_CFG_CMD_PF_UNBIND); + + if (mnl_socket_sendto(nl, nlh, nlh->nlmsg_len) < 0) { + perror("mnl_socket_send"); + exit(EXIT_FAILURE); + } + + nlh = nfq_hdr_put(buf, NFQNL_MSG_CONFIG, 0); + nfq_nlmsg_cfg_put_cmd(nlh, AF_INET, NFQNL_CFG_CMD_PF_BIND); + + if (mnl_socket_sendto(nl, nlh, nlh->nlmsg_len) < 0) { + perror("mnl_socket_send"); + exit(EXIT_FAILURE); + } + + nlh = nfq_hdr_put(buf, NFQNL_MSG_CONFIG, queue_num); + nfq_nlmsg_cfg_put_cmd(nlh, AF_INET, NFQNL_CFG_CMD_BIND); + + if (mnl_socket_sendto(nl, nlh, nlh->nlmsg_len) < 0) { + perror("mnl_socket_send"); + exit(EXIT_FAILURE); + } + + nlh = nfq_hdr_put(buf, NFQNL_MSG_CONFIG, queue_num); + nfq_nlmsg_cfg_put_params(nlh, NFQNL_COPY_PACKET, 0xffff); + + mnl_attr_put_u32(nlh, NFQA_CFG_FLAGS, htonl(NFQA_CFG_F_GSO)); + mnl_attr_put_u32(nlh, NFQA_CFG_MASK, htonl(NFQA_CFG_F_GSO)); + + if (mnl_socket_sendto(nl, nlh, nlh->nlmsg_len) < 0) { + perror("mnl_socket_send"); + exit(EXIT_FAILURE); + } + + /* ENOBUFS is signalled to userspace when packets were lost + * on kernel side. In most cases, userspace isn't interested + * in this information, so turn it off. + */ + ret = 1; + mnl_socket_setsockopt(nl, NETLINK_NO_ENOBUFS, &ret, sizeof(int)); + + for (;;) { + ret = mnl_socket_recvfrom(nl, buf, sizeof_buf); + if (ret == -1) { + perror("mnl_socket_recvfrom"); + exit(EXIT_FAILURE); + } + + ret = mnl_cb_run(buf, ret, 0, portid, queue_cb, NULL); + if (ret < 0){ + perror("mnl_cb_run"); + exit(EXIT_FAILURE); + } + } + + mnl_socket_close(nl); + + return 0; +} -- cgit v1.2.3