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), ¶ms); |
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: buf, command: 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: buf, mode: NFQNL_COPY_PACKET, range: 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(delay: opts.delay_ms); |
315 | |
316 | nlh = nfq_build_verdict(buf, id, queue_num: opts.queue_num, verd: 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(p: 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 | } |
396 | |