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 help(const char * p)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 parse_attr_cb(const struct nlattr * attr,void * data)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 queue_cb(const struct nlmsghdr * nlh,void * data)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 * nfq_build_cfg_request(char * buf,uint8_t command,int queue_num)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 * nfq_build_cfg_params(char * buf,uint8_t mode,int range,int queue_num)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 * nfq_build_verdict(char * buf,int id,int queue_num,uint32_t verd)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 print_stats(void)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 open_queue(void)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 sleep_ms(uint32_t delay)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 mainloop(void)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 parse_opts(int argc,char ** argv)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 main(int argc,char * argv[])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