libmnl  1.0.3
nf-queue.c
00001 /* This example is placed in the public domain. */
00002 #include <stdio.h>
00003 #include <stdlib.h>
00004 #include <unistd.h>
00005 #include <string.h>
00006 #include <time.h>
00007 #include <arpa/inet.h>
00008 
00009 #include <libmnl/libmnl.h>
00010 #include <linux/netfilter.h>
00011 #include <linux/netfilter/nfnetlink.h>
00012 
00013 #ifndef aligned_be64
00014 #define aligned_be64 u_int64_t __attribute__((aligned(8)))
00015 #endif
00016 
00017 #include <linux/netfilter/nfnetlink_queue.h>
00018 
00019 static int parse_attr_cb(const struct nlattr *attr, void *data)
00020 {
00021         const struct nlattr **tb = data;
00022         int type = mnl_attr_get_type(attr);
00023 
00024         /* skip unsupported attribute in user-space */
00025         if (mnl_attr_type_valid(attr, NFQA_MAX) < 0)
00026                 return MNL_CB_OK;
00027 
00028         switch(type) {
00029         case NFQA_MARK:
00030         case NFQA_IFINDEX_INDEV:
00031         case NFQA_IFINDEX_OUTDEV:
00032         case NFQA_IFINDEX_PHYSINDEV:
00033         case NFQA_IFINDEX_PHYSOUTDEV:
00034                 if (mnl_attr_validate(attr, MNL_TYPE_U32) < 0) {
00035                         perror("mnl_attr_validate");
00036                         return MNL_CB_ERROR;
00037                 }
00038                 break;
00039         case NFQA_TIMESTAMP:
00040                 if (mnl_attr_validate2(attr, MNL_TYPE_UNSPEC,
00041                     sizeof(struct nfqnl_msg_packet_timestamp)) < 0) {
00042                         perror("mnl_attr_validate");
00043                         return MNL_CB_ERROR;
00044                 }
00045                 break;
00046         case NFQA_HWADDR:
00047                 if (mnl_attr_validate2(attr, MNL_TYPE_UNSPEC,
00048                     sizeof(struct nfqnl_msg_packet_hw)) < 0) {
00049                         perror("mnl_attr_validate");
00050                         return MNL_CB_ERROR;
00051                 }
00052                 break;
00053         case NFQA_PAYLOAD:
00054                 break;
00055         }
00056         tb[type] = attr;
00057         return MNL_CB_OK;
00058 }
00059 
00060 static int queue_cb(const struct nlmsghdr *nlh, void *data)
00061 {
00062         struct nlattr *tb[NFQA_MAX+1] = {};
00063         struct nfqnl_msg_packet_hdr *ph = NULL;
00064         uint32_t id = 0;
00065 
00066         mnl_attr_parse(nlh, sizeof(struct nfgenmsg), parse_attr_cb, tb);
00067         if (tb[NFQA_PACKET_HDR]) {
00068                 ph = mnl_attr_get_payload(tb[NFQA_PACKET_HDR]);
00069                 id = ntohl(ph->packet_id);
00070         }
00071         printf("packet received (id=%u hw=0x%04x hook=%u)\n",
00072                 id, ntohs(ph->hw_protocol), ph->hook);
00073 
00074         return MNL_CB_OK + id;
00075 }
00076 
00077 static struct nlmsghdr *
00078 nfq_build_cfg_pf_request(char *buf, uint8_t command)
00079 {
00080         struct nlmsghdr *nlh = mnl_nlmsg_put_header(buf);
00081         nlh->nlmsg_type = (NFNL_SUBSYS_QUEUE << 8) | NFQNL_MSG_CONFIG;
00082         nlh->nlmsg_flags = NLM_F_REQUEST;
00083 
00084         struct nfgenmsg *nfg = mnl_nlmsg_put_extra_header(nlh, sizeof(*nfg));
00085         nfg->nfgen_family = AF_UNSPEC;
00086         nfg->version = NFNETLINK_V0;
00087 
00088         struct nfqnl_msg_config_cmd cmd = {
00089                 .command = command,
00090                 .pf = htons(AF_INET),
00091         };
00092         mnl_attr_put(nlh, NFQA_CFG_CMD, sizeof(cmd), &cmd);
00093 
00094         return nlh;
00095 }
00096 
00097 static struct nlmsghdr *
00098 nfq_build_cfg_request(char *buf, uint8_t command, int queue_num)
00099 {
00100         struct nlmsghdr *nlh = mnl_nlmsg_put_header(buf);
00101         nlh->nlmsg_type = (NFNL_SUBSYS_QUEUE << 8) | NFQNL_MSG_CONFIG;
00102         nlh->nlmsg_flags = NLM_F_REQUEST;
00103 
00104         struct nfgenmsg *nfg = mnl_nlmsg_put_extra_header(nlh, sizeof(*nfg));
00105         nfg->nfgen_family = AF_UNSPEC;
00106         nfg->version = NFNETLINK_V0;
00107         nfg->res_id = htons(queue_num);
00108 
00109         struct nfqnl_msg_config_cmd cmd = {
00110                 .command = command,
00111                 .pf = htons(AF_INET),
00112         };
00113         mnl_attr_put(nlh, NFQA_CFG_CMD, sizeof(cmd), &cmd);
00114 
00115         return nlh;
00116 }
00117 
00118 static struct nlmsghdr *
00119 nfq_build_cfg_params(char *buf, uint8_t mode, int range, int queue_num)
00120 {
00121         struct nlmsghdr *nlh = mnl_nlmsg_put_header(buf);
00122         nlh->nlmsg_type = (NFNL_SUBSYS_QUEUE << 8) | NFQNL_MSG_CONFIG;
00123         nlh->nlmsg_flags = NLM_F_REQUEST;
00124 
00125         struct nfgenmsg *nfg = mnl_nlmsg_put_extra_header(nlh, sizeof(*nfg));
00126         nfg->nfgen_family = AF_UNSPEC;
00127         nfg->version = NFNETLINK_V0;
00128         nfg->res_id = htons(queue_num);
00129 
00130         struct nfqnl_msg_config_params params = {
00131                 .copy_range = htonl(range),
00132                 .copy_mode = mode,
00133         };
00134         mnl_attr_put(nlh, NFQA_CFG_PARAMS, sizeof(params), &params);
00135 
00136         return nlh;
00137 }
00138 
00139 static struct nlmsghdr *
00140 nfq_build_verdict(char *buf, int id, int queue_num, int verd)
00141 {
00142         struct nlmsghdr *nlh;
00143         struct nfgenmsg *nfg;
00144 
00145         nlh = mnl_nlmsg_put_header(buf);
00146         nlh->nlmsg_type = (NFNL_SUBSYS_QUEUE << 8) | NFQNL_MSG_VERDICT;
00147         nlh->nlmsg_flags = NLM_F_REQUEST;
00148         nfg = mnl_nlmsg_put_extra_header(nlh, sizeof(*nfg));
00149         nfg->nfgen_family = AF_UNSPEC;
00150         nfg->version = NFNETLINK_V0;
00151         nfg->res_id = htons(queue_num);
00152 
00153         struct nfqnl_msg_verdict_hdr vh = {
00154                 .verdict = htonl(verd),
00155                 .id = htonl(id),
00156         };
00157         mnl_attr_put(nlh, NFQA_VERDICT_HDR, sizeof(vh), &vh);
00158 
00159         return nlh;
00160 }
00161 
00162 int main(int argc, char *argv[])
00163 {
00164         struct mnl_socket *nl;
00165         char buf[MNL_SOCKET_BUFFER_SIZE];
00166         struct nlmsghdr *nlh;
00167         int ret;
00168         unsigned int portid, queue_num;
00169 
00170         if (argc != 2) {
00171                 printf("Usage: %s [queue_num]\n", argv[0]);
00172                 exit(EXIT_FAILURE);
00173         }
00174         queue_num = atoi(argv[1]);
00175 
00176         nl = mnl_socket_open(NETLINK_NETFILTER);
00177         if (nl == NULL) {
00178                 perror("mnl_socket_open");
00179                 exit(EXIT_FAILURE);
00180         }
00181 
00182         if (mnl_socket_bind(nl, 0, MNL_SOCKET_AUTOPID) < 0) {
00183                 perror("mnl_socket_bind");
00184                 exit(EXIT_FAILURE);
00185         }
00186         portid = mnl_socket_get_portid(nl);
00187 
00188         nlh = nfq_build_cfg_pf_request(buf, NFQNL_CFG_CMD_PF_UNBIND);
00189 
00190         if (mnl_socket_sendto(nl, nlh, nlh->nlmsg_len) < 0) {
00191                 perror("mnl_socket_send");
00192                 exit(EXIT_FAILURE);
00193         }
00194 
00195         nlh = nfq_build_cfg_pf_request(buf, NFQNL_CFG_CMD_PF_BIND);
00196 
00197         if (mnl_socket_sendto(nl, nlh, nlh->nlmsg_len) < 0) {
00198                 perror("mnl_socket_send");
00199                 exit(EXIT_FAILURE);
00200         }
00201 
00202         nlh = nfq_build_cfg_request(buf, NFQNL_CFG_CMD_BIND, queue_num);
00203 
00204         if (mnl_socket_sendto(nl, nlh, nlh->nlmsg_len) < 0) {
00205                 perror("mnl_socket_send");
00206                 exit(EXIT_FAILURE);
00207         }
00208 
00209         nlh = nfq_build_cfg_params(buf, NFQNL_COPY_PACKET, 0xFFFF, queue_num);
00210 
00211         if (mnl_socket_sendto(nl, nlh, nlh->nlmsg_len) < 0) {
00212                 perror("mnl_socket_send");
00213                 exit(EXIT_FAILURE);
00214         }
00215 
00216         ret = mnl_socket_recvfrom(nl, buf, sizeof(buf));
00217         if (ret == -1) {
00218                 perror("mnl_socket_recvfrom");
00219                 exit(EXIT_FAILURE);
00220         }
00221         while (ret > 0) {
00222                 uint32_t id;
00223 
00224                 ret = mnl_cb_run(buf, ret, 0, portid, queue_cb, NULL);
00225                 if (ret < 0){
00226                         perror("mnl_cb_run");
00227                         exit(EXIT_FAILURE);
00228                 }
00229 
00230                 id = ret - MNL_CB_OK;
00231                 nlh = nfq_build_verdict(buf, id, queue_num, NF_ACCEPT);
00232                 if (mnl_socket_sendto(nl, nlh, nlh->nlmsg_len) < 0) {
00233                         perror("mnl_socket_send");
00234                         exit(EXIT_FAILURE);
00235                 }
00236 
00237                 ret = mnl_socket_recvfrom(nl, buf, sizeof(buf));
00238                 if (ret == -1) {
00239                         perror("mnl_socket_recvfrom");
00240                         exit(EXIT_FAILURE);
00241                 }
00242         }
00243 
00244         mnl_socket_close(nl);
00245 
00246         return 0;
00247 }