| 1 | + | /* |
| 2 | + | * ---------------------------------------------------------------------------- |
| 3 | + | * "THE BEER-WARE LICENSE" (Revision 42): |
| 4 | + | * David Bouman (pql) wrote this file. As long as you retain this notice you |
| 5 | + | * can do whatever you want with this stuff. If we meet some day, and you think |
| 6 | + | * this stuff is worth it, you can buy me a beer in return. Signed, David. |
| 7 | + | * ---------------------------------------------------------------------------- |
| 8 | + | */ |
| 9 | + | |
| 10 | + | #define _GNU_SOURCE |
| 11 | + | #include <stdlib.h> |
| 12 | + | #include <time.h> |
| 13 | + | #include <string.h> |
| 14 | + | #include <netinet/in.h> |
| 15 | + | #include <arpa/inet.h> |
| 16 | + | #include <stdint.h> |
| 17 | + | #include <sys/types.h> |
| 18 | + | #include <sched.h> |
| 19 | + | #include <signal.h> |
| 20 | + | #include <linux/netfilter.h> |
| 21 | + | #include <linux/netfilter/nf_tables.h> |
| 22 | + | #include <linux/netfilter/nfnetlink.h> |
| 23 | + | #include <libmnl/libmnl.h> |
| 24 | + | #include <libnftnl/table.h> |
| 25 | + | #include <libnftnl/chain.h> |
| 26 | + | #include <libnftnl/rule.h> |
| 27 | + | #include <libnftnl/set.h> |
| 28 | + | #include <libnftnl/expr.h> |
| 29 | + | #include <net/if.h> |
| 30 | + | #include <linux/if_packet.h> |
| 31 | + | |
| 32 | + | #include "helpers.h" |
| 33 | + | |
| 34 | + | #define U32_NLA_SIZE (NLA_HDRLEN + sizeof(uint32_t)) |
| 35 | + | #define S8_NLA_SIZE (NLA_HDRLEN + 8) |
| 36 | + | |
| 37 | + | static uint64_t default_batch_req_handler(struct mnl_socket* nl, int portid, int table_seq) |
| 38 | + | { |
| 39 | + | char buf[MNL_SOCKET_BUFFER_SIZE]; |
| 40 | + | |
| 41 | + | int ret = mnl_socket_recvfrom(nl, buf, sizeof(buf)); |
| 42 | + | |
| 43 | + | while (ret > 0) { |
| 44 | + | ret = mnl_cb_run(buf, ret, table_seq, portid, NULL, NULL); |
| 45 | + | if (ret <= 0) break; |
| 46 | + | ret = mnl_socket_recvfrom(nl, buf, sizeof(buf)); |
| 47 | + | } |
| 48 | + | return ret; |
| 49 | + | } |
| 50 | + | |
| 51 | + | int64_t send_batch_request(struct mnl_socket* nl, uint16_t msg, uint16_t msg_flags, uint16_t family, void** object, int* seq, uint64_t (*result_handler)(struct mnl_socket*, int, int)) |
| 52 | + | { |
| 53 | + | char buf[MNL_SOCKET_BUFFER_SIZE]; |
| 54 | + | struct mnl_nlmsg_batch* batch = mnl_nlmsg_batch_start(buf, sizeof buf); |
| 55 | + | uint8_t msg_type = msg & 0xff; |
| 56 | + | uint8_t nft_type = (msg >> 8) & 0xff; |
| 57 | + | nftnl_batch_begin(mnl_nlmsg_batch_current(batch), (*seq)++); |
| 58 | + | mnl_nlmsg_batch_next(batch); |
| 59 | + | int table_seq = *seq; |
| 60 | + | struct nlmsghdr* nlh; |
| 61 | + | |
| 62 | + | if (result_handler == NULL) { |
| 63 | + | result_handler = default_batch_req_handler; |
| 64 | + | } |
| 65 | + | |
| 66 | + | if (msg == NFT_MSG_NEWSET) { |
| 67 | + | nlh = nftnl_set_nlmsg_build_hdr( |
| 68 | + | mnl_nlmsg_batch_current(batch), |
| 69 | + | NFT_MSG_NEWSET, family, |
| 70 | + | msg_flags | NLM_F_ACK, (*seq)++); |
| 71 | + | } else { |
| 72 | + | nlh = nftnl_nlmsg_build_hdr( |
| 73 | + | mnl_nlmsg_batch_current(batch), |
| 74 | + | msg_type, family, |
| 75 | + | msg_flags | NLM_F_ACK, (*seq)++ |
| 76 | + | ); |
| 77 | + | } |
| 78 | + | if (msg == NFT_MSG_NEWSET) { |
| 79 | + | nftnl_set_nlmsg_build_payload(nlh, *object); |
| 80 | + | nftnl_set_free(*object); |
| 81 | + | } else { |
| 82 | + | switch(nft_type) { |
| 83 | + | case NFT_TYPE_TABLE: |
| 84 | + | nftnl_table_nlmsg_build_payload(nlh, *object); |
| 85 | + | nftnl_table_free(*object); |
| 86 | + | break; |
| 87 | + | case NFT_TYPE_CHAIN: |
| 88 | + | nftnl_chain_nlmsg_build_payload(nlh, *object); |
| 89 | + | nftnl_chain_free(*object); |
| 90 | + | break; |
| 91 | + | case NFT_TYPE_RULE: |
| 92 | + | nftnl_rule_nlmsg_build_payload(nlh, *object); |
| 93 | + | // offload mnl_attr_put_u32(nlh, NFTA_CHAIN_FLAGS, htonl(2)); |
| 94 | + | nftnl_rule_free(*object); |
| 95 | + | break; |
| 96 | + | default: |
| 97 | + | return -1; |
| 98 | + | } |
| 99 | + | } |
| 100 | + | |
| 101 | + | *object = NULL; |
| 102 | + | |
| 103 | + | mnl_nlmsg_batch_next(batch); |
| 104 | + | nftnl_batch_end(mnl_nlmsg_batch_current(batch), (*seq)++); |
| 105 | + | mnl_nlmsg_batch_next(batch); |
| 106 | + | |
| 107 | + | int ret = mnl_socket_sendto( |
| 108 | + | nl, |
| 109 | + | mnl_nlmsg_batch_head(batch), |
| 110 | + | mnl_nlmsg_batch_size(batch) |
| 111 | + | ); |
| 112 | + | |
| 113 | + | if (ret < 0) { |
| 114 | + | perror("mnl_socket_send"); |
| 115 | + | return -1; |
| 116 | + | } |
| 117 | + | |
| 118 | + | int portid = mnl_socket_get_portid(nl); |
| 119 | + | |
| 120 | + | mnl_nlmsg_batch_stop(batch); |
| 121 | + | |
| 122 | + | result_handler(nl, portid, table_seq); |
| 123 | + | } |
| 124 | + | |
| 125 | + | struct nftnl_table* build_table(char* name, uint16_t family) |
| 126 | + | { |
| 127 | + | struct nftnl_table* t = nftnl_table_alloc(); |
| 128 | + | |
| 129 | + | nftnl_table_set_u32(t, NFTNL_TABLE_FAMILY, family); |
| 130 | + | nftnl_table_set_str(t, NFTNL_TABLE_NAME, name); |
| 131 | + | |
| 132 | + | return t; |
| 133 | + | } |
| 134 | + | |
| 135 | + | struct nftnl_chain* build_chain(char* table_name, char* chain_name, char *dev_name, struct unft_base_chain_param* base_param) |
| 136 | + | { |
| 137 | + | struct nftnl_chain* c; |
| 138 | + | |
| 139 | + | c = nftnl_chain_alloc(); |
| 140 | + | |
| 141 | + | nftnl_chain_set_str(c, NFTNL_CHAIN_NAME, chain_name); |
| 142 | + | nftnl_chain_set_str(c, NFTNL_CHAIN_TABLE, table_name); |
| 143 | + | if (dev_name) |
| 144 | + | nftnl_chain_set_str(c, NFTNL_CHAIN_DEV, dev_name); |
| 145 | + | |
| 146 | + | if (base_param) { |
| 147 | + | nftnl_chain_set_u32(c, NFTNL_CHAIN_HOOKNUM, base_param->hook_num); |
| 148 | + | nftnl_chain_set_u32(c, NFTNL_CHAIN_PRIO, base_param->prio); |
| 149 | + | } |
| 150 | + | |
| 151 | + | return c; |
| 152 | + | } |
| 153 | + | |
| 154 | + | struct nftnl_rule* build_rule(char* table_name, char* chain_name, uint16_t family, uint64_t* handle) |
| 155 | + | { |
| 156 | + | struct nftnl_rule* r = NULL; |
| 157 | + | uint8_t proto; |
| 158 | + | |
| 159 | + | r = nftnl_rule_alloc(); |
| 160 | + | |
| 161 | + | nftnl_rule_set_str(r, NFTNL_RULE_TABLE, table_name); |
| 162 | + | nftnl_rule_set_str(r, NFTNL_RULE_CHAIN, chain_name); |
| 163 | + | nftnl_rule_set_u32(r, NFTNL_RULE_FAMILY, family); |
| 164 | + | |
| 165 | + | if (handle) { |
| 166 | + | nftnl_rule_set_u64(r, NFTNL_RULE_POSITION, *handle); |
| 167 | + | } |
| 168 | + | |
| 169 | + | return r; |
| 170 | + | } |
| 171 | + | |
| 172 | + | struct nftnl_set* build_set(char *table_name, char *set_name, uint16_t family) { |
| 173 | + | // Create a new set object |
| 174 | + | struct nftnl_set *set = nftnl_set_alloc(); |
| 175 | + | |
| 176 | + | // Set the name of the set |
| 177 | + | nftnl_set_set_str(set, NFTNL_SET_TABLE, table_name); |
| 178 | + | nftnl_set_set_str(set, NFTNL_SET_NAME, set_name); |
| 179 | + | nftnl_set_set_u32(set, NFTNL_SET_FLAGS, NFT_SET_MAP); |
| 180 | + | nftnl_set_set_u32(set, NFTNL_SET_DATA_TYPE, NFT_DATA_VALUE); |
| 181 | + | nftnl_set_set_u32(set, NFTNL_SET_KEY_LEN, 4); |
| 182 | + | nftnl_set_set_u32(set, NFTNL_SET_DATA_LEN, 4); |
| 183 | + | nftnl_set_set_u32(set, NFTNL_SET_FAMILY, family); |
| 184 | + | nftnl_set_set_u32(set, NFTNL_SET_ID, 1); |
| 185 | + | |
| 186 | + | //nftnl_set_add_expr(set, expr); |
| 187 | + | return set; |
| 188 | + | } |
| 189 | + | |
| 190 | + | #define NFTA_BITWISE_OP NFTA_BITWISE_XOR + 1 |
| 191 | + | #define NFTA_BITWISE_DATA NFTA_BITWISE_OP + 1 |
| 192 | + | |
| 193 | + | void rule_add_bit_shift( |
| 194 | + | struct nftnl_rule* r, uint32_t shift_type, uint32_t bitwise_len, |
| 195 | + | uint32_t bitwise_sreg, uint32_t bitwise_dreg, void* data, uint32_t data_len) |
| 196 | + | { |
| 197 | + | |
| 198 | + | if(bitwise_len > 0xff) { |
| 199 | + | puts("bitwise_len > 0xff"); |
| 200 | + | exit(EXIT_FAILURE); |
| 201 | + | } |
| 202 | + | |
| 203 | + | struct nftnl_expr* e; |
| 204 | + | e = nftnl_expr_alloc("bitwise"); |
| 205 | + | |
| 206 | + | nftnl_expr_set_u32(e, NFTA_BITWISE_SREG, bitwise_sreg); |
| 207 | + | nftnl_expr_set_u32(e, NFTA_BITWISE_DREG, bitwise_dreg); |
| 208 | + | nftnl_expr_set_u32(e, NFTA_BITWISE_OP, shift_type); |
| 209 | + | nftnl_expr_set_u32(e, NFTA_BITWISE_LEN, bitwise_len); |
| 210 | + | nftnl_expr_set_data(e, NFTA_BITWISE_DATA, data, data_len); |
| 211 | + | |
| 212 | + | nftnl_rule_add_expr(r, e); |
| 213 | + | } |
| 214 | + | |
| 215 | + | void rule_add_memcpy(struct nftnl_rule* r, uint32_t len, uint32_t sreg, uint32_t dreg) |
| 216 | + | { |
| 217 | + | uint32_t data = 0; |
| 218 | + | rule_add_bit_shift(r, NFT_BITWISE_LSHIFT, len, sreg, dreg, &data, sizeof(data)); |
| 219 | + | } |
| 220 | + | |
| 221 | + | void rule_add_dynset(struct nftnl_rule* r, char *set_name, uint32_t reg_key, uint32_t reg_data) { |
| 222 | + | struct nftnl_expr *expr = nftnl_expr_alloc("dynset"); |
| 223 | + | nftnl_expr_set_str(expr, NFTNL_EXPR_DYNSET_SET_NAME, set_name); |
| 224 | + | nftnl_expr_set_u32(expr, NFTNL_EXPR_DYNSET_OP, NFT_DYNSET_OP_UPDATE); |
| 225 | + | //nftnl_expr_set_u32(expr, NFTNL_EXPR_DYNSET_SET_ID, 1); |
| 226 | + | nftnl_expr_set_u32(expr, NFTNL_EXPR_DYNSET_SREG_KEY, reg_key); |
| 227 | + | nftnl_expr_set_u32(expr, NFTNL_EXPR_DYNSET_SREG_DATA, reg_data); |
| 228 | + | nftnl_rule_add_expr(r, expr); |
| 229 | + | } |
| 230 | + | |
| 231 | + | void rule_add_payload(struct nftnl_rule* r, uint32_t base, uint32_t offset, uint32_t len, uint32_t dreg) |
| 232 | + | { |
| 233 | + | struct nftnl_expr* e; |
| 234 | + | e = nftnl_expr_alloc("payload"); |
| 235 | + | |
| 236 | + | nftnl_expr_set_u32(e, NFTNL_EXPR_PAYLOAD_BASE, base); |
| 237 | + | nftnl_expr_set_u32(e, NFTNL_EXPR_PAYLOAD_OFFSET, offset); |
| 238 | + | nftnl_expr_set_u32(e, NFTNL_EXPR_PAYLOAD_LEN, len); |
| 239 | + | nftnl_expr_set_u32(e, NFTNL_EXPR_PAYLOAD_DREG, dreg); |
| 240 | + | |
| 241 | + | nftnl_rule_add_expr(r, e); |
| 242 | + | } |
| 243 | + | |
| 244 | + | void rule_add_cmp(struct nftnl_rule* r, uint32_t op, uint32_t sreg, void* data, size_t data_len) |
| 245 | + | { |
| 246 | + | struct nftnl_expr* e; |
| 247 | + | e = nftnl_expr_alloc("cmp"); |
| 248 | + | |
| 249 | + | nftnl_expr_set_u32(e, NFTA_CMP_OP, op); |
| 250 | + | nftnl_expr_set_u32(e, NFTA_CMP_SREG, sreg); |
| 251 | + | nftnl_expr_set_data(e, NFTA_CMP_DATA, data, data_len); |
| 252 | + | |
| 253 | + | nftnl_rule_add_expr(r, e); |
| 254 | + | } |
| 255 | + | |
| 256 | + | void rule_add_immediate_data(struct nftnl_rule* r, uint32_t dreg, void* data, size_t data_len) |
| 257 | + | { |
| 258 | + | struct nftnl_expr* e; |
| 259 | + | |
| 260 | + | e = nftnl_expr_alloc("immediate"); |
| 261 | + | |
| 262 | + | nftnl_expr_set_u32(e, NFTA_IMMEDIATE_DREG, dreg); |
| 263 | + | nftnl_expr_set_data(e, NFTA_IMMEDIATE_DATA, data, data_len); |
| 264 | + | |
| 265 | + | nftnl_rule_add_expr(r, e); |
| 266 | + | } |
| 267 | + | |
| 268 | + | void rule_add_immediate_verdict(struct nftnl_rule* r, uint32_t verdict, char* chain_name) |
| 269 | + | { |
| 270 | + | struct nftnl_expr* e; |
| 271 | + | e = nftnl_expr_alloc("immediate"); |
| 272 | + | |
| 273 | + | // dreg = 0 -> verdict |
| 274 | + | nftnl_expr_set_u32(e, NFTA_IMMEDIATE_DREG, NFT_REG_VERDICT); |
| 275 | + | |
| 276 | + | nftnl_expr_set_u32(e, NFTNL_EXPR_IMM_VERDICT, verdict); |
| 277 | + | |
| 278 | + | if (verdict == NFT_GOTO || verdict == NFT_JUMP) { |
| 279 | + | nftnl_expr_set_str(e, NFTNL_EXPR_IMM_CHAIN, chain_name); |
| 280 | + | } |
| 281 | + | |
| 282 | + | nftnl_rule_add_expr(r, e); |
| 283 | + | } |
| 284 | + | |
| 285 | + | int create_table(struct mnl_socket* nl, char* name, uint16_t family, int* seq, uint64_t (*result_handler)(struct mnl_socket*, int, int)) |
| 286 | + | { |
| 287 | + | |
| 288 | + | struct nftnl_table* t = build_table(name, family); |
| 289 | + | |
| 290 | + | return send_batch_request( |
| 291 | + | nl, |
| 292 | + | NFT_MSG_NEWTABLE | (NFT_TYPE_TABLE << 8), |
| 293 | + | NLM_F_CREATE, family, (void**)&t, seq, |
| 294 | + | result_handler |
| 295 | + | ); |
| 296 | + | } |
| 297 | + | |
| 298 | + | int create_set(struct mnl_socket* nl, char *table_name, char* name, uint16_t family, int* seq, uint64_t (*result_handler)(struct mnl_socket*, int, int)) { |
| 299 | + | struct nftnl_set* s = build_set(table_name, name, family); |
| 300 | + | |
| 301 | + | return send_batch_request( |
| 302 | + | nl, |
| 303 | + | NFT_MSG_NEWSET, |
| 304 | + | NLM_F_CREATE, family, (void**)&s, seq, |
| 305 | + | result_handler |
| 306 | + | ); |
| 307 | + | } |
| 308 | + | |
| 309 | + | int create_chain(struct mnl_socket* nl, char* chain_name, char* table_name, char* dev_name, uint16_t family, struct unft_base_chain_param* base_param, int* seq, uint64_t (*result_handler)(struct mnl_socket*, int, int)) |
| 310 | + | { |
| 311 | + | struct nftnl_chain* c = build_chain(chain_name, table_name, dev_name, base_param); |
| 312 | + | |
| 313 | + | return send_batch_request( |
| 314 | + | nl, |
| 315 | + | NFT_MSG_NEWCHAIN | (NFT_TYPE_CHAIN << 8), |
| 316 | + | NLM_F_CREATE, family, (void**)&c, seq, |
| 317 | + | result_handler |
| 318 | + | ); |
| 319 | + | } |
| 320 | + | |
| 321 | + | int send_packet() { |
| 322 | + | int sockfd; |
| 323 | + | struct sockaddr_in addr; |
| 324 | + | char buffer[] = "This is a test message"; |
| 325 | + | char *interface_name = "vlan.10"; // double-tagged packet |
| 326 | + | int interface_index; |
| 327 | + | struct ifreq ifr; |
| 328 | + | memset(&ifr, 0, sizeof(ifr)); |
| 329 | + | memcpy(ifr.ifr_name, interface_name, MIN(strlen(interface_name) + 1, sizeof(ifr.ifr_name))); |
| 330 | + | |
| 331 | + | sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); |
| 332 | + | if (sockfd < 0) { |
| 333 | + | perror("Error creating socket"); |
| 334 | + | return 1; |
| 335 | + | } |
| 336 | + | |
| 337 | + | // Set the SO_BINDTODEVICE socket option |
| 338 | + | if (setsockopt(sockfd, SOL_SOCKET, SO_BINDTODEVICE, (void *)&ifr, sizeof(ifr)) < 0) { |
| 339 | + | perror("Error setting SO_BINDTODEVICE socket option"); |
| 340 | + | return 1; |
| 341 | + | } |
| 342 | + | |
| 343 | + | memset(&addr, 0, sizeof(addr)); |
| 344 | + | addr.sin_family = AF_INET; |
| 345 | + | addr.sin_addr.s_addr = inet_addr("192.168.123.123"); // random destination |
| 346 | + | addr.sin_port = htons(1337); |
| 347 | + | |
| 348 | + | // Send the UDP packet |
| 349 | + | if (sendto(sockfd, buffer, sizeof(buffer), 0, (struct sockaddr*)&addr, sizeof(addr)) < 0) { |
| 350 | + | perror("Error sending UDP packet"); |
| 351 | + | return 1; |
| 352 | + | } |
| 353 | + | |
| 354 | + | close(sockfd); |
| 355 | + | return 0; |
| 356 | + | } |