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