1 /* SPDX-License-Identifier: LGPL-2.1-only */
2 /*
3 * Copyright (c) 2013 Cong Wang <xiyou.wangcong@gmail.com>
4 *
5 * Stolen from tests/test-complex-HTB-with-hash-filters.c
6 */
7
8 #include "nl-default.h"
9
10 #include <stdio.h>
11
12 #include <linux/if_ether.h>
13 #include <linux/tc_act/tc_mirred.h>
14 #include <linux/netlink.h>
15
16 #include <netlink/route/link.h>
17 #include <netlink/route/tc.h>
18 #include <netlink/route/qdisc.h>
19 #include <netlink/route/cls/u32.h>
20 #include <netlink/route/classifier.h>
21 #include <netlink/route/action.h>
22 #include <netlink/route/act/mirred.h>
23 #include <netlink/route/act/skbedit.h>
24 #include <netlink/route/class.h>
25 #include <netlink/attr.h>
26
27 #define TC_HANDLE(maj, min) (TC_H_MAJ((maj) << 16) | TC_H_MIN(min))
28
29 /* some functions are copied from iproute-tc tool */
get_u32(__u32 * val,const char * arg,int base)30 static int get_u32(__u32 *val, const char *arg, int base)
31 {
32 unsigned long res;
33 char *ptr;
34
35 if (!arg || !*arg)
36 return -1;
37 res = strtoul(arg, &ptr, base);
38 if (!ptr || ptr == arg || *ptr || res > 0xFFFFFFFFUL)
39 return -1;
40 *val = res;
41 return 0;
42 }
43
get_u32_handle(__u32 * handle,const char * str)44 static int get_u32_handle(__u32 *handle, const char *str)
45 {
46 __u32 htid=0, hash=0, nodeid=0;
47 char *tmp = strchr(str, ':');
48
49 if (tmp == NULL) {
50 if (memcmp("0x", str, 2) == 0)
51 return get_u32(handle, str, 16);
52 return -1;
53 }
54 htid = strtoul(str, &tmp, 16);
55 if (tmp == str && *str != ':' && *str != 0)
56 return -1;
57 if (htid>=0x1000)
58 return -1;
59 if (*tmp) {
60 str = tmp+1;
61 hash = strtoul(str, &tmp, 16);
62 if (tmp == str && *str != ':' && *str != 0)
63 return -1;
64 if (hash>=0x100)
65 return -1;
66 if (*tmp) {
67 str = tmp+1;
68 nodeid = strtoul(str, &tmp, 16);
69 if (tmp == str && *str != 0)
70 return -1;
71 if (nodeid>=0x1000)
72 return -1;
73 }
74 }
75 *handle = (htid<<20)|(hash<<12)|nodeid;
76 return 0;
77 }
78
get_u32_parse_handle(const char * cHandle)79 static uint32_t get_u32_parse_handle(const char *cHandle)
80 {
81 uint32_t handle=0;
82
83 if(get_u32_handle(&handle, cHandle)) {
84 printf ("Illegal \"ht\"\n");
85 return -1;
86 }
87
88 if (handle && TC_U32_NODE(handle)) {
89 printf("\"link\" must be a hash table.\n");
90 return -1;
91 }
92 return handle;
93 }
94
95 /*
96 * Function that adds a new filter and attach it to a hash table
97 * and set next hash table link with hash mask
98 *
99 */
100 static
u32_add_filter_on_ht_with_hashmask(struct nl_sock * sock,struct rtnl_link * rtnlLink,uint32_t prio,uint32_t keyval,uint32_t keymask,int keyoff,int keyoffmask,uint32_t htid,uint32_t htlink,uint32_t hmask,uint32_t hoffset,struct rtnl_act * act,struct rtnl_act * act2)101 int u32_add_filter_on_ht_with_hashmask(struct nl_sock *sock, struct rtnl_link *rtnlLink, uint32_t prio,
102 uint32_t keyval, uint32_t keymask, int keyoff, int keyoffmask,
103 uint32_t htid, uint32_t htlink, uint32_t hmask, uint32_t hoffset, struct rtnl_act *act, struct rtnl_act *act2)
104 {
105 struct rtnl_cls *cls;
106 int err;
107
108 cls=rtnl_cls_alloc();
109 if (!(cls)) {
110 printf("Can not allocate classifier\n");
111 nl_socket_free(sock);
112 exit(1);
113 }
114
115 rtnl_tc_set_link(TC_CAST(cls), rtnlLink);
116
117 if ((err = rtnl_tc_set_kind(TC_CAST(cls), "u32"))) {
118 printf("Can not set classifier as u32\n");
119 return 1;
120 }
121
122 rtnl_cls_set_prio(cls, prio);
123 rtnl_cls_set_protocol(cls, ETH_P_IP);
124
125 rtnl_tc_set_parent(TC_CAST(cls), TC_HANDLE(0xffff, 0));
126
127 if (htid)
128 rtnl_u32_set_hashtable(cls, htid);
129
130 rtnl_u32_add_key_uint32(cls, keyval, keymask, keyoff, keyoffmask);
131
132 rtnl_u32_set_hashmask(cls, hmask, hoffset);
133
134 rtnl_u32_set_link(cls, htlink);
135
136 rtnl_u32_add_action(cls, act);
137
138 rtnl_u32_add_action(cls, act2);
139
140
141 if ((err = rtnl_cls_add(sock, cls, NLM_F_CREATE))) {
142 printf("Can not add classifier: %s\n", nl_geterror(err));
143 return -1;
144 }
145 rtnl_cls_put(cls);
146 return 0;
147 }
148
149 /*
150 * function that creates a new hash table
151 */
152 static
u32_add_ht(struct nl_sock * sock,struct rtnl_link * rtnlLink,uint32_t prio,uint32_t htid,uint32_t divisor)153 int u32_add_ht(struct nl_sock *sock, struct rtnl_link *rtnlLink, uint32_t prio, uint32_t htid, uint32_t divisor)
154 {
155
156 int err;
157 struct rtnl_cls *cls;
158
159 cls=rtnl_cls_alloc();
160 if (!(cls)) {
161 printf("Can not allocate classifier\n");
162 nl_socket_free(sock);
163 exit(1);
164 }
165
166 rtnl_tc_set_link(TC_CAST(cls), rtnlLink);
167
168 if ((err = rtnl_tc_set_kind(TC_CAST(cls), "u32"))) {
169 printf("Can not set classifier as u32\n");
170 return 1;
171 }
172
173 rtnl_cls_set_prio(cls, prio);
174 rtnl_cls_set_protocol(cls, ETH_P_IP);
175 rtnl_tc_set_parent(TC_CAST(cls), TC_HANDLE(0xffff, 0));
176
177 rtnl_u32_set_handle(cls, htid, 0x0, 0x0);
178 //printf("htid: 0x%X\n", htid);
179 rtnl_u32_set_divisor(cls, divisor);
180
181 if ((err = rtnl_cls_add(sock, cls, NLM_F_CREATE))) {
182 printf("Can not add classifier: %s\n", nl_geterror(err));
183 return -1;
184 }
185 rtnl_cls_put(cls);
186 return 0;
187 }
188
189 /*
190 * function that adds a new ingress qdisc and set the default class for unclassified traffic
191 */
192 static
qdisc_add_ingress(struct nl_sock * sock,struct rtnl_link * rtnlLink)193 int qdisc_add_ingress(struct nl_sock *sock, struct rtnl_link *rtnlLink)
194 {
195
196 struct rtnl_qdisc *qdisc;
197 int err;
198
199 /* Allocation of a qdisc object */
200 if (!(qdisc = rtnl_qdisc_alloc())) {
201 printf("Can not allocate Qdisc\n");
202 return -1;
203 }
204
205 //rtnl_tc_set_ifindex(TC_CAST(qdisc), master_index);
206 rtnl_tc_set_link(TC_CAST(qdisc), rtnlLink);
207 rtnl_tc_set_parent(TC_CAST(qdisc), TC_H_ROOT);
208
209 //printf("Delete current qdisc\n");
210 rtnl_qdisc_delete(sock, qdisc);
211 //rtnl_qdisc_put(qdisc);
212
213 rtnl_tc_set_handle(TC_CAST(qdisc), TC_HANDLE(0xffff, 0));
214
215 if ((err = rtnl_tc_set_kind(TC_CAST(qdisc), "ingress"))) {
216 printf("Can not allocate ingress\n");
217 return -1;
218 }
219
220 /* Submit request to kernel and wait for response */
221 if ((err = rtnl_qdisc_add(sock, qdisc, NLM_F_CREATE))) {
222 printf("Can not allocate ingress Qdisc\n");
223 return -1;
224 }
225
226 /* Return the qdisc object to free memory resources */
227 rtnl_qdisc_put(qdisc);
228
229 return 0;
230 }
231
main(void)232 int main(void)
233 {
234 struct nl_sock *sock;
235 struct rtnl_link *link;
236 uint32_t ht, htlink, htid, direction;
237 char chashlink[16]="";
238 int err;
239 struct nl_cache *link_cache;
240 struct rtnl_act *act, *act2;
241 uint32_t i;
242
243 if (!(sock = nl_socket_alloc())) {
244 printf("Unable to allocate netlink socket\n");
245 exit(1);
246 }
247
248 if ((err = nl_connect(sock, NETLINK_ROUTE)) < 0 ) {
249 printf("Nu s-a putut conecta la NETLINK!\n");
250 nl_socket_free(sock);
251 exit(1);
252 }
253
254 if ((err = rtnl_link_alloc_cache(sock, AF_UNSPEC, &link_cache)) < 0) {
255 printf("Unable to allocate link cache: %s\n",
256 nl_geterror(err));
257 nl_socket_free(sock);
258 exit(1);
259 }
260
261 /* lookup interface index of eth0 */
262 if (!(link = rtnl_link_get_by_name(link_cache, "eth0"))) {
263 /* error */
264 printf("Interface not found\n");
265 nl_socket_free(sock);
266 exit(1);
267 }
268
269 err=qdisc_add_ingress(sock, link);
270 //printf("Add main hash table\n");
271
272 /* create u32 first hash filter table
273 *
274 */
275 /* formula calcul handle:
276 * uint32_t handle = (htid << 20) | (hash << 12) | nodeid;
277 */
278
279 /*
280 * Upper limit of number of hash tables: 4096 (0xFFF)
281 * Number of hashes in a table: 256 values (0xFF)
282 *
283 */
284
285 /* using 256 values for hash table
286 * each entry in hash table match a byte from IP address specified later by a hash key
287 */
288
289 for (i = 1; i <= 0xf; i++)
290 u32_add_ht(sock, link, 1, i, 256);
291
292 /*
293 * attach a u32 filter to the first hash
294 * that redirects all traffic and make a hash key
295 * from the fist byte of the IP address
296 *
297 */
298
299 //divisor=0x0; // unused here
300 //handle = 0x0; // unused here
301 //hash = 0x0; // unused here
302 //htid = 0x0; // unused here
303 //nodeid = 0x0; // unused here
304
305 // direction = 12 -> source IP
306 // direction = 16 -> destination IP
307 direction = 16;
308
309 /*
310 * which hash table will use
311 * in our case is hash table no 1 defined previous
312 *
313 * There are 2 posibilities to set the the hash table:
314 * 1. Using function get_u32_handle and sent a string in
315 * format 10: where 10 is number of the hash table
316 * 2. Create your own value in format: 0xa00000
317 *
318 */
319 strcpy(chashlink, "1:");
320 //printf("Hash Link: %s\n", chashlink);
321 //chashlink=malloc(sizeof(char) *
322 htlink = 0x0; // is used by get_u32_handle to return the correct value of hash table (link)
323
324 if(get_u32_handle(&htlink, chashlink)) {
325 printf ("Illegal \"link\"");
326 nl_socket_free(sock);
327 exit(1);
328 }
329 //printf ("hash link : 0x%X\n", htlink);
330 //printf ("hash link test : %u\n", (htlink && TC_U32_NODE(htlink)));
331
332 if (htlink && TC_U32_NODE(htlink)) {
333 printf("\"link\" must be a hash table.\n");
334 nl_socket_free(sock);
335 exit(1);
336 }
337
338 /* the hash mask will hit the hash table (link) no 1: in our case
339 */
340
341 /* set the hash key mask */
342 //hashmask = 0xFF000000UL; // the mask that is used to match the hash in specific table, in our case for example 1:a with mean the first byte which is 10 in hash table 1
343
344 /* Here we add a hash filter which match the first byte (see the hashmask value)
345 * of the source IP (offset 12 in the packet header)
346 * You can use also offset 16 to match the destination IP
347 */
348
349 /*
350 * Also we need a filter to match our rule
351 * This mean that we will put a 0.0.0.0/0 filter in our first rule
352 * that match the offset 12 (source IP)
353 * Also you can put offset 16 to match the destination IP
354 */
355
356 u32_add_filter_on_ht_with_hashmask(sock, link, 1,
357 0x0, 0x0, direction, 0,
358 0, htlink, 0xff000000, direction, NULL, NULL);
359
360 /*
361 * For each first byte that we need to match we will create a new hash table
362 * For example: you have those clases: 10.0.0.0/24 and 172.16.0.0/23
363 * For byte 10 and byte 172 will create a separate hash table that will match the second
364 * byte from each class.
365 *
366 */
367
368
369 /*
370 * Now we will create other filter under (ATENTION) our first hash table (link) 1:
371 * Previous rule redirects the trafic according the hash mask to hash table (link) no 1:
372 * Here we will match the hash tables from 1:0 to 1:ff. Under each hash table we will attach
373 * other rules that matches next byte from IP source/destination IP and we will repeat the
374 * previous steps.
375 *
376 */
377 act = rtnl_act_alloc();
378 if (!act) {
379 printf("rtnl_act_alloc() returns %p\n", act);
380 return -1;
381 }
382 rtnl_tc_set_kind(TC_CAST(act), "skbedit");
383 rtnl_skbedit_set_queue_mapping(act, 4);
384 rtnl_skbedit_set_action(act, TC_ACT_PIPE);
385
386 act2 = rtnl_act_alloc();
387 if (!act2) {
388 printf("rtnl_act_alloc() returns %p\n", act2);
389 return -1;
390 }
391 rtnl_tc_set_kind(TC_CAST(act2), "mirred");
392 rtnl_mirred_set_action(act2, TCA_EGRESS_REDIR);
393 rtnl_mirred_set_policy(act2, TC_ACT_STOLEN);
394 rtnl_mirred_set_ifindex(act2, rtnl_link_name2i(link_cache, "eth1"));
395 // /8 check
396
397 // 10.0.0.0/8
398 ht=get_u32_parse_handle("1:a:");
399 htid = (ht&0xFFFFF000);
400 htlink=get_u32_parse_handle("2:");
401
402 u32_add_filter_on_ht_with_hashmask(sock, link, 1,
403 0x0a000000, 0xff000000, direction, 0,
404 htid, htlink, 0x00ff0000, direction, act, act2);
405
406 rtnl_act_put(act);
407 nl_socket_free(sock);
408 return 0;
409 }
410