tools/testing/selftests/netfilter/nf-queue.c

   1 // SPDX-License-Identifier: GPL-2.0
   2
   3 #include <errno.h>
   4 #include <stdbool.h>
   5 #include <stdio.h>
   6 #include <stdint.h>
   7 #include <stdlib.h>
   8 #include <unistd.h>
   9 #include <string.h>
  10 #include <time.h>
  11 #include <arpa/inet.h>
  12
  13 #include <libmnl/libmnl.h>
  14 #include <linux/netfilter.h>
  15 #include <linux/netfilter/nfnetlink.h>
  16 #include <linux/netfilter/nfnetlink_queue.h>
  17
  18 struct options {
  19         bool count_packets;
  20         bool gso_enabled;
  21         int verbose;
  22         unsigned int queue_num;
  23         unsigned int timeout;
  24         uint32_t verdict;
  25         uint32_t delay_ms;
  26 };
  27
  28 static unsigned int queue_stats[5];
  29 static struct options opts;
  30
  31 static void help(const char *p)
  32 {
  33         printf("Usage: %s [-c|-v [-vv] ] [-t timeout] [-q queue_num] [-Qdst_queue ] [ -d ms_delay ] [-G]\n", p);
  34 }
  35
  36 static int parse_attr_cb(const struct nlattr *attr, void *data)
  37 {
  38         const struct nlattr **tb = data;
  39         int type = mnl_attr_get_type(attr);
  40
  41         /* skip unsupported attribute in user-space */
  42         if (mnl_attr_type_valid(attr, NFQA_MAX) < 0)
  43                 return MNL_CB_OK;
  44
  45         switch (type) {
  46         case NFQA_MARK:
  47         case NFQA_IFINDEX_INDEV:
  48         case NFQA_IFINDEX_OUTDEV:
  49         case NFQA_IFINDEX_PHYSINDEV:
  50         case NFQA_IFINDEX_PHYSOUTDEV:
  51                 if (mnl_attr_validate(attr, MNL_TYPE_U32) < 0) {
  52                         perror("mnl_attr_validate");
  53                         return MNL_CB_ERROR;
  54                 }
  55                 break;
  56         case NFQA_TIMESTAMP:
  57                 if (mnl_attr_validate2(attr, MNL_TYPE_UNSPEC,
  58                     sizeof(struct nfqnl_msg_packet_timestamp)) < 0) {
  59                         perror("mnl_attr_validate2");
  60                         return MNL_CB_ERROR;
  61                 }
  62                 break;
  63         case NFQA_HWADDR:
  64                 if (mnl_attr_validate2(attr, MNL_TYPE_UNSPEC,
  65                     sizeof(struct nfqnl_msg_packet_hw)) < 0) {
  66                         perror("mnl_attr_validate2");
  67                         return MNL_CB_ERROR;
  68                 }
  69                 break;
  70         case NFQA_PAYLOAD:
  71                 break;
  72         }
  73         tb[type] = attr;
  74         return MNL_CB_OK;
  75 }
  76
  77 static int queue_cb(const struct nlmsghdr *nlh, void *data)
  78 {
  79         struct nlattr *tb[NFQA_MAX+1] = { 0 };
  80         struct nfqnl_msg_packet_hdr *ph = NULL;
  81         uint32_t id = 0;
  82
  83         (void)data;
  84
  85         mnl_attr_parse(nlh, sizeof(struct nfgenmsg), parse_attr_cb, tb);
  86         if (tb[NFQA_PACKET_HDR]) {
  87                 ph = mnl_attr_get_payload(tb[NFQA_PACKET_HDR]);
  88                 id = ntohl(ph->packet_id);
  89
  90                 if (opts.verbose > 0)
  91                         printf("packet hook=%u, hwproto 0x%x",
  92                                 ntohs(ph->hw_protocol), ph->hook);
  93
  94                 if (ph->hook >= 5) {
  95                         fprintf(stderr, "Unknown hook %d\n", ph->hook);
  96                         return MNL_CB_ERROR;
  97                 }
  98
  99                 if (opts.verbose > 0) {
 100                         uint32_t skbinfo = 0;
 101
 102                         if (tb[NFQA_SKB_INFO])
 103                                 skbinfo = ntohl(mnl_attr_get_u32(tb[NFQA_SKB_INFO]));
 104                         if (skbinfo & NFQA_SKB_CSUMNOTREADY)
 105                                 printf(" csumnotready");
 106                         if (skbinfo & NFQA_SKB_GSO)
 107                                 printf(" gso");
 108                         if (skbinfo & NFQA_SKB_CSUM_NOTVERIFIED)
 109                                 printf(" csumnotverified");
 110                         puts("");
 111                 }
 112
 113                 if (opts.count_packets)
 114                         queue_stats[ph->hook]++;
 115         }
 116
 117         return MNL_CB_OK + id;
 118 }
 119
 120 static struct nlmsghdr *
 121 nfq_build_cfg_request(char *buf, uint8_t command, int queue_num)
 122 {
 123         struct nlmsghdr *nlh = mnl_nlmsg_put_header(buf);
 124         struct nfqnl_msg_config_cmd cmd = {
 125                 .command = command,
 126                 .pf = htons(AF_INET),
 127         };
 128         struct nfgenmsg *nfg;
 129
 130         nlh->nlmsg_type = (NFNL_SUBSYS_QUEUE << 8) | NFQNL_MSG_CONFIG;
 131         nlh->nlmsg_flags = NLM_F_REQUEST;
 132
 133         nfg = mnl_nlmsg_put_extra_header(nlh, sizeof(*nfg));
 134
 135         nfg->nfgen_family = AF_UNSPEC;
 136         nfg->version = NFNETLINK_V0;
 137         nfg->res_id = htons(queue_num);
 138
 139         mnl_attr_put(nlh, NFQA_CFG_CMD, sizeof(cmd), &cmd);
 140
 141         return nlh;
 142 }
 143
 144 static struct nlmsghdr *
 145 nfq_build_cfg_params(char *buf, uint8_t mode, int range, int queue_num)
 146 {
 147         struct nlmsghdr *nlh = mnl_nlmsg_put_header(buf);
 148         struct nfqnl_msg_config_params params = {
 149                 .copy_range = htonl(range),
 150                 .copy_mode = mode,
 151         };
 152         struct nfgenmsg *nfg;
 153
 154         nlh->nlmsg_type = (NFNL_SUBSYS_QUEUE << 8) | NFQNL_MSG_CONFIG;
 155         nlh->nlmsg_flags = NLM_F_REQUEST;
 156
 157         nfg = mnl_nlmsg_put_extra_header(nlh, sizeof(*nfg));
 158         nfg->nfgen_family = AF_UNSPEC;
 159         nfg->version = NFNETLINK_V0;
 160         nfg->res_id = htons(queue_num);
 161
 162         mnl_attr_put(nlh, NFQA_CFG_PARAMS, sizeof(params), &params);
 163
 164         return nlh;
 165 }
 166
 167 static struct nlmsghdr *
 168 nfq_build_verdict(char *buf, int id, int queue_num, uint32_t verd)
 169 {
 170         struct nfqnl_msg_verdict_hdr vh = {
 171                 .verdict = htonl(verd),
 172                 .id = htonl(id),
 173         };
 174         struct nlmsghdr *nlh;
 175         struct nfgenmsg *nfg;
 176
 177         nlh = mnl_nlmsg_put_header(buf);
 178         nlh->nlmsg_type = (NFNL_SUBSYS_QUEUE << 8) | NFQNL_MSG_VERDICT;
 179         nlh->nlmsg_flags = NLM_F_REQUEST;
 180         nfg = mnl_nlmsg_put_extra_header(nlh, sizeof(*nfg));
 181         nfg->nfgen_family = AF_UNSPEC;
 182         nfg->version = NFNETLINK_V0;
 183         nfg->res_id = htons(queue_num);
 184
 185         mnl_attr_put(nlh, NFQA_VERDICT_HDR, sizeof(vh), &vh);
 186
 187         return nlh;
 188 }
 189
 190 static void print_stats(void)
 191 {
 192         unsigned int last, total;
 193         int i;
 194
 195         total = 0;
 196         last = queue_stats[0];
 197
 198         for (i = 0; i < 5; i++) {
 199                 printf("hook %d packets %08u\n", i, queue_stats[i]);
 200                 last = queue_stats[i];
 201                 total += last;
 202         }
 203
 204         printf("%u packets total\n", total);
 205 }
 206
 207 struct mnl_socket *open_queue(void)
 208 {
 209         char buf[MNL_SOCKET_BUFFER_SIZE];
 210         unsigned int queue_num;
 211         struct mnl_socket *nl;
 212         struct nlmsghdr *nlh;
 213         struct timeval tv;
 214         uint32_t flags;
 215
 216         nl = mnl_socket_open(NETLINK_NETFILTER);
 217         if (nl == NULL) {
 218                 perror("mnl_socket_open");
 219                 exit(EXIT_FAILURE);
 220         }
 221
 222         if (mnl_socket_bind(nl, 0, MNL_SOCKET_AUTOPID) < 0) {
 223                 perror("mnl_socket_bind");
 224                 exit(EXIT_FAILURE);
 225         }
 226
 227         queue_num = opts.queue_num;
 228         nlh = nfq_build_cfg_request(buf, NFQNL_CFG_CMD_BIND, queue_num);
 229
 230         if (mnl_socket_sendto(nl, nlh, nlh->nlmsg_len) < 0) {
 231                 perror("mnl_socket_sendto");
 232                 exit(EXIT_FAILURE);
 233         }
 234
 235         nlh = nfq_build_cfg_params(buf, NFQNL_COPY_PACKET, 0xFFFF, queue_num);
 236
 237         flags = opts.gso_enabled ? NFQA_CFG_F_GSO : 0;
 238         flags |= NFQA_CFG_F_UID_GID;
 239         mnl_attr_put_u32(nlh, NFQA_CFG_FLAGS, htonl(flags));
 240         mnl_attr_put_u32(nlh, NFQA_CFG_MASK, htonl(flags));
 241
 242         if (mnl_socket_sendto(nl, nlh, nlh->nlmsg_len) < 0) {
 243                 perror("mnl_socket_sendto");
 244                 exit(EXIT_FAILURE);
 245         }
 246
 247         memset(&tv, 0, sizeof(tv));
 248         tv.tv_sec = opts.timeout;
 249         if (opts.timeout && setsockopt(mnl_socket_get_fd(nl),
 250                                        SOL_SOCKET, SO_RCVTIMEO,
 251                                        &tv, sizeof(tv))) {
 252                 perror("setsockopt(SO_RCVTIMEO)");
 253                 exit(EXIT_FAILURE);
 254         }
 255
 256         return nl;
 257 }
 258
 259 static void sleep_ms(uint32_t delay)
 260 {
 261         struct timespec ts = { .tv_sec = delay / 1000 };
 262
 263         delay %= 1000;
 264
 265         ts.tv_nsec = delay * 1000llu * 1000llu;
 266
 267         nanosleep(&ts, NULL);
 268 }
 269
 270 static int mainloop(void)
 271 {
 272         unsigned int buflen = 64 * 1024 + MNL_SOCKET_BUFFER_SIZE;
 273         struct mnl_socket *nl;
 274         struct nlmsghdr *nlh;
 275         unsigned int portid;
 276         char *buf;
 277         int ret;
 278
 279         buf = malloc(buflen);
 280         if (!buf) {
 281                 perror("malloc");
 282                 exit(EXIT_FAILURE);
 283         }
 284
 285         nl = open_queue();
 286         portid = mnl_socket_get_portid(nl);
 287
 288         for (;;) {
 289                 uint32_t id;
 290
 291                 ret = mnl_socket_recvfrom(nl, buf, buflen);
 292                 if (ret == -1) {
 293                         if (errno == ENOBUFS || errno == EINTR)
 294                                 continue;
 295
 296                         if (errno == EAGAIN) {
 297                                 errno = 0;
 298                                 ret = 0;
 299                                 break;
 300                         }
 301
 302                         perror("mnl_socket_recvfrom");
 303                         exit(EXIT_FAILURE);
 304                 }
 305
 306                 ret = mnl_cb_run(buf, ret, 0, portid, queue_cb, NULL);
 307                 if (ret < 0) {
 308                         perror("mnl_cb_run");
 309                         exit(EXIT_FAILURE);
 310                 }
 311
 312                 id = ret - MNL_CB_OK;
 313                 if (opts.delay_ms)
 314                         sleep_ms(opts.delay_ms);
 315
 316                 nlh = nfq_build_verdict(buf, id, opts.queue_num, opts.verdict);
 317                 if (mnl_socket_sendto(nl, nlh, nlh->nlmsg_len) < 0) {
 318                         perror("mnl_socket_sendto");
 319                         exit(EXIT_FAILURE);
 320                 }
 321         }
 322
 323         mnl_socket_close(nl);
 324
 325         return ret;
 326 }
 327
 328 static void parse_opts(int argc, char **argv)
 329 {
 330         int c;
 331
 332         while ((c = getopt(argc, argv, "chvt:q:Q:d:G")) != -1) {
 333                 switch (c) {
 334                 case 'c':
 335                         opts.count_packets = true;
 336                         break;
 337                 case 'h':
 338                         help(argv[0]);
 339                         exit(0);
 340                         break;
 341                 case 'q':
 342                         opts.queue_num = atoi(optarg);
 343                         if (opts.queue_num > 0xffff)
 344                                 opts.queue_num = 0;
 345                         break;
 346                 case 'Q':
 347                         opts.verdict = atoi(optarg);
 348                         if (opts.verdict > 0xffff) {
 349                                 fprintf(stderr, "Expected destination queue number\n");
 350                                 exit(1);
 351                         }
 352
 353                         opts.verdict <<= 16;
 354                         opts.verdict |= NF_QUEUE;
 355                         break;
 356                 case 'd':
 357                         opts.delay_ms = atoi(optarg);
 358                         if (opts.delay_ms == 0) {
 359                                 fprintf(stderr, "Expected nonzero delay (in milliseconds)\n");
 360                                 exit(1);
 361                         }
 362                         break;
 363                 case 't':
 364                         opts.timeout = atoi(optarg);
 365                         break;
 366                 case 'G':
 367                         opts.gso_enabled = false;
 368                         break;
 369                 case 'v':
 370                         opts.verbose++;
 371                         break;
 372                 }
 373         }
 374
 375         if (opts.verdict != NF_ACCEPT && (opts.verdict >> 16 == opts.queue_num)) {
 376                 fprintf(stderr, "Cannot use same destination and source queue\n");
 377                 exit(1);
 378         }
 379 }
 380
 381 int main(int argc, char *argv[])
 382 {
 383         int ret;
 384
 385         opts.verdict = NF_ACCEPT;
 386         opts.gso_enabled = true;
 387
 388         parse_opts(argc, argv);
 389
 390         ret = mainloop();
 391         if (opts.count_packets)
 392                 print_stats();
 393
 394         return ret;
 395 }