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 int verbose; 21 unsigned int queue_num; 22 unsigned int timeout; 23 }; 24 25 static unsigned int queue_stats[5]; 26 static struct options opts; 27 28 static void help(const char *p) 29 { 30 printf("Usage: %s [-c|-v [-vv] ] [-t timeout] [-q queue_num]\n", p); 31 } 32 33 static int parse_attr_cb(const struct nlattr *attr, void *data) 34 { 35 const struct nlattr **tb = data; 36 int type = mnl_attr_get_type(attr); 37 38 /* skip unsupported attribute in user-space */ 39 if (mnl_attr_type_valid(attr, NFQA_MAX) < 0) 40 return MNL_CB_OK; 41 42 switch (type) { 43 case NFQA_MARK: 44 case NFQA_IFINDEX_INDEV: 45 case NFQA_IFINDEX_OUTDEV: 46 case NFQA_IFINDEX_PHYSINDEV: 47 case NFQA_IFINDEX_PHYSOUTDEV: 48 if (mnl_attr_validate(attr, MNL_TYPE_U32) < 0) { 49 perror("mnl_attr_validate"); 50 return MNL_CB_ERROR; 51 } 52 break; 53 case NFQA_TIMESTAMP: 54 if (mnl_attr_validate2(attr, MNL_TYPE_UNSPEC, 55 sizeof(struct nfqnl_msg_packet_timestamp)) < 0) { 56 perror("mnl_attr_validate2"); 57 return MNL_CB_ERROR; 58 } 59 break; 60 case NFQA_HWADDR: 61 if (mnl_attr_validate2(attr, MNL_TYPE_UNSPEC, 62 sizeof(struct nfqnl_msg_packet_hw)) < 0) { 63 perror("mnl_attr_validate2"); 64 return MNL_CB_ERROR; 65 } 66 break; 67 case NFQA_PAYLOAD: 68 break; 69 } 70 tb[type] = attr; 71 return MNL_CB_OK; 72 } 73 74 static int queue_cb(const struct nlmsghdr *nlh, void *data) 75 { 76 struct nlattr *tb[NFQA_MAX+1] = { 0 }; 77 struct nfqnl_msg_packet_hdr *ph = NULL; 78 uint32_t id = 0; 79 80 (void)data; 81 82 mnl_attr_parse(nlh, sizeof(struct nfgenmsg), parse_attr_cb, tb); 83 if (tb[NFQA_PACKET_HDR]) { 84 ph = mnl_attr_get_payload(tb[NFQA_PACKET_HDR]); 85 id = ntohl(ph->packet_id); 86 87 if (opts.verbose > 0) 88 printf("packet hook=%u, hwproto 0x%x", 89 ntohs(ph->hw_protocol), ph->hook); 90 91 if (ph->hook >= 5) { 92 fprintf(stderr, "Unknown hook %d\n", ph->hook); 93 return MNL_CB_ERROR; 94 } 95 96 if (opts.verbose > 0) { 97 uint32_t skbinfo = 0; 98 99 if (tb[NFQA_SKB_INFO]) 100 skbinfo = ntohl(mnl_attr_get_u32(tb[NFQA_SKB_INFO])); 101 if (skbinfo & NFQA_SKB_CSUMNOTREADY) 102 printf(" csumnotready"); 103 if (skbinfo & NFQA_SKB_GSO) 104 printf(" gso"); 105 if (skbinfo & NFQA_SKB_CSUM_NOTVERIFIED) 106 printf(" csumnotverified"); 107 puts(""); 108 } 109 110 if (opts.count_packets) 111 queue_stats[ph->hook]++; 112 } 113 114 return MNL_CB_OK + id; 115 } 116 117 static struct nlmsghdr * 118 nfq_build_cfg_request(char *buf, uint8_t command, int queue_num) 119 { 120 struct nlmsghdr *nlh = mnl_nlmsg_put_header(buf); 121 struct nfqnl_msg_config_cmd cmd = { 122 .command = command, 123 .pf = htons(AF_INET), 124 }; 125 struct nfgenmsg *nfg; 126 127 nlh->nlmsg_type = (NFNL_SUBSYS_QUEUE << 8) | NFQNL_MSG_CONFIG; 128 nlh->nlmsg_flags = NLM_F_REQUEST; 129 130 nfg = mnl_nlmsg_put_extra_header(nlh, sizeof(*nfg)); 131 132 nfg->nfgen_family = AF_UNSPEC; 133 nfg->version = NFNETLINK_V0; 134 nfg->res_id = htons(queue_num); 135 136 mnl_attr_put(nlh, NFQA_CFG_CMD, sizeof(cmd), &cmd); 137 138 return nlh; 139 } 140 141 static struct nlmsghdr * 142 nfq_build_cfg_params(char *buf, uint8_t mode, int range, int queue_num) 143 { 144 struct nlmsghdr *nlh = mnl_nlmsg_put_header(buf); 145 struct nfqnl_msg_config_params params = { 146 .copy_range = htonl(range), 147 .copy_mode = mode, 148 }; 149 struct nfgenmsg *nfg; 150 151 nlh->nlmsg_type = (NFNL_SUBSYS_QUEUE << 8) | NFQNL_MSG_CONFIG; 152 nlh->nlmsg_flags = NLM_F_REQUEST; 153 154 nfg = mnl_nlmsg_put_extra_header(nlh, sizeof(*nfg)); 155 nfg->nfgen_family = AF_UNSPEC; 156 nfg->version = NFNETLINK_V0; 157 nfg->res_id = htons(queue_num); 158 159 mnl_attr_put(nlh, NFQA_CFG_PARAMS, sizeof(params), ¶ms); 160 161 return nlh; 162 } 163 164 static struct nlmsghdr * 165 nfq_build_verdict(char *buf, int id, int queue_num, int verd) 166 { 167 struct nfqnl_msg_verdict_hdr vh = { 168 .verdict = htonl(verd), 169 .id = htonl(id), 170 }; 171 struct nlmsghdr *nlh; 172 struct nfgenmsg *nfg; 173 174 nlh = mnl_nlmsg_put_header(buf); 175 nlh->nlmsg_type = (NFNL_SUBSYS_QUEUE << 8) | NFQNL_MSG_VERDICT; 176 nlh->nlmsg_flags = NLM_F_REQUEST; 177 nfg = mnl_nlmsg_put_extra_header(nlh, sizeof(*nfg)); 178 nfg->nfgen_family = AF_UNSPEC; 179 nfg->version = NFNETLINK_V0; 180 nfg->res_id = htons(queue_num); 181 182 mnl_attr_put(nlh, NFQA_VERDICT_HDR, sizeof(vh), &vh); 183 184 return nlh; 185 } 186 187 static void print_stats(void) 188 { 189 unsigned int last, total; 190 int i; 191 192 if (!opts.count_packets) 193 return; 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 = NFQA_CFG_F_GSO | NFQA_CFG_F_UID_GID; 238 mnl_attr_put_u32(nlh, NFQA_CFG_FLAGS, htonl(flags)); 239 mnl_attr_put_u32(nlh, NFQA_CFG_MASK, htonl(flags)); 240 241 if (mnl_socket_sendto(nl, nlh, nlh->nlmsg_len) < 0) { 242 perror("mnl_socket_sendto"); 243 exit(EXIT_FAILURE); 244 } 245 246 memset(&tv, 0, sizeof(tv)); 247 tv.tv_sec = opts.timeout; 248 if (opts.timeout && setsockopt(mnl_socket_get_fd(nl), 249 SOL_SOCKET, SO_RCVTIMEO, 250 &tv, sizeof(tv))) { 251 perror("setsockopt(SO_RCVTIMEO)"); 252 exit(EXIT_FAILURE); 253 } 254 255 return nl; 256 } 257 258 static int mainloop(void) 259 { 260 unsigned int buflen = 64 * 1024 + MNL_SOCKET_BUFFER_SIZE; 261 struct mnl_socket *nl; 262 struct nlmsghdr *nlh; 263 unsigned int portid; 264 char *buf; 265 int ret; 266 267 buf = malloc(buflen); 268 if (!buf) { 269 perror("malloc"); 270 exit(EXIT_FAILURE); 271 } 272 273 nl = open_queue(); 274 portid = mnl_socket_get_portid(nl); 275 276 for (;;) { 277 uint32_t id; 278 279 ret = mnl_socket_recvfrom(nl, buf, buflen); 280 if (ret == -1) { 281 if (errno == ENOBUFS) 282 continue; 283 284 if (errno == EAGAIN) { 285 errno = 0; 286 ret = 0; 287 break; 288 } 289 290 perror("mnl_socket_recvfrom"); 291 exit(EXIT_FAILURE); 292 } 293 294 ret = mnl_cb_run(buf, ret, 0, portid, queue_cb, NULL); 295 if (ret < 0) { 296 perror("mnl_cb_run"); 297 exit(EXIT_FAILURE); 298 } 299 300 id = ret - MNL_CB_OK; 301 nlh = nfq_build_verdict(buf, id, opts.queue_num, NF_ACCEPT); 302 if (mnl_socket_sendto(nl, nlh, nlh->nlmsg_len) < 0) { 303 perror("mnl_socket_sendto"); 304 exit(EXIT_FAILURE); 305 } 306 } 307 308 mnl_socket_close(nl); 309 310 return ret; 311 } 312 313 static void parse_opts(int argc, char **argv) 314 { 315 int c; 316 317 while ((c = getopt(argc, argv, "chvt:q:")) != -1) { 318 switch (c) { 319 case 'c': 320 opts.count_packets = true; 321 break; 322 case 'h': 323 help(argv[0]); 324 exit(0); 325 break; 326 case 'q': 327 opts.queue_num = atoi(optarg); 328 if (opts.queue_num > 0xffff) 329 opts.queue_num = 0; 330 break; 331 case 't': 332 opts.timeout = atoi(optarg); 333 break; 334 case 'v': 335 opts.verbose++; 336 break; 337 } 338 } 339 } 340 341 int main(int argc, char *argv[]) 342 { 343 int ret; 344 345 parse_opts(argc, argv); 346 347 ret = mainloop(); 348 if (opts.count_packets) 349 print_stats(); 350 351 return ret; 352 } 353