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