1
2
3
4
5
6
7#include <arpa/inet.h>
8#include <errno.h>
9#include <limits.h>
10#include <linux/const.h>
11#include <linux/if_ether.h>
12#include <linux/ioam6.h>
13#include <linux/ipv6.h>
14#include <stdlib.h>
15#include <string.h>
16#include <unistd.h>
17
18struct ioam_config {
19 __u32 id;
20 __u64 wide;
21 __u16 ingr_id;
22 __u16 egr_id;
23 __u32 ingr_wide;
24 __u32 egr_wide;
25 __u32 ns_data;
26 __u64 ns_wide;
27 __u32 sc_id;
28 __u8 hlim;
29 char *sc_data;
30};
31
32
33
34
35
36
37static struct ioam_config node1 = {
38 .id = 1,
39 .wide = 11111111,
40 .ingr_id = 0xffff,
41 .egr_id = 101,
42 .ingr_wide = 0xffffffff,
43 .egr_wide = 101101,
44 .ns_data = 0xdeadbee0,
45 .ns_wide = 0xcafec0caf00dc0de,
46 .sc_id = 777,
47 .sc_data = "something that will be 4n-aligned",
48 .hlim = 64,
49};
50
51static struct ioam_config node2 = {
52 .id = 2,
53 .wide = 22222222,
54 .ingr_id = 201,
55 .egr_id = 202,
56 .ingr_wide = 201201,
57 .egr_wide = 202202,
58 .ns_data = 0xdeadbee1,
59 .ns_wide = 0xcafec0caf11dc0de,
60 .sc_id = 666,
61 .sc_data = "Hello there -Obi",
62 .hlim = 63,
63};
64
65static struct ioam_config node3 = {
66 .id = 3,
67 .wide = 33333333,
68 .ingr_id = 301,
69 .egr_id = 0xffff,
70 .ingr_wide = 301301,
71 .egr_wide = 0xffffffff,
72 .ns_data = 0xdeadbee2,
73 .ns_wide = 0xcafec0caf22dc0de,
74 .sc_id = 0xffffff,
75 .sc_data = NULL,
76 .hlim = 62,
77};
78
79enum {
80
81
82
83 TEST_OUT_UNDEF_NS,
84 TEST_OUT_NO_ROOM,
85 TEST_OUT_BIT0,
86 TEST_OUT_BIT1,
87 TEST_OUT_BIT2,
88 TEST_OUT_BIT3,
89 TEST_OUT_BIT4,
90 TEST_OUT_BIT5,
91 TEST_OUT_BIT6,
92 TEST_OUT_BIT7,
93 TEST_OUT_BIT8,
94 TEST_OUT_BIT9,
95 TEST_OUT_BIT10,
96 TEST_OUT_BIT11,
97 TEST_OUT_BIT22,
98 TEST_OUT_FULL_SUPP_TRACE,
99
100
101
102
103 TEST_IN_UNDEF_NS,
104 TEST_IN_NO_ROOM,
105 TEST_IN_OFLAG,
106 TEST_IN_BIT0,
107 TEST_IN_BIT1,
108 TEST_IN_BIT2,
109 TEST_IN_BIT3,
110 TEST_IN_BIT4,
111 TEST_IN_BIT5,
112 TEST_IN_BIT6,
113 TEST_IN_BIT7,
114 TEST_IN_BIT8,
115 TEST_IN_BIT9,
116 TEST_IN_BIT10,
117 TEST_IN_BIT11,
118 TEST_IN_BIT22,
119 TEST_IN_FULL_SUPP_TRACE,
120
121
122
123
124 TEST_FWD_FULL_SUPP_TRACE,
125
126 __TEST_MAX,
127};
128
129static int check_ioam_header(int tid, struct ioam6_trace_hdr *ioam6h,
130 __u32 trace_type, __u16 ioam_ns)
131{
132 if (__be16_to_cpu(ioam6h->namespace_id) != ioam_ns ||
133 __be32_to_cpu(ioam6h->type_be32) != (trace_type << 8))
134 return 1;
135
136 switch (tid) {
137 case TEST_OUT_UNDEF_NS:
138 case TEST_IN_UNDEF_NS:
139 return ioam6h->overflow ||
140 ioam6h->nodelen != 1 ||
141 ioam6h->remlen != 1;
142
143 case TEST_OUT_NO_ROOM:
144 case TEST_IN_NO_ROOM:
145 case TEST_IN_OFLAG:
146 return !ioam6h->overflow ||
147 ioam6h->nodelen != 2 ||
148 ioam6h->remlen != 1;
149
150 case TEST_OUT_BIT0:
151 case TEST_IN_BIT0:
152 case TEST_OUT_BIT1:
153 case TEST_IN_BIT1:
154 case TEST_OUT_BIT2:
155 case TEST_IN_BIT2:
156 case TEST_OUT_BIT3:
157 case TEST_IN_BIT3:
158 case TEST_OUT_BIT4:
159 case TEST_IN_BIT4:
160 case TEST_OUT_BIT5:
161 case TEST_IN_BIT5:
162 case TEST_OUT_BIT6:
163 case TEST_IN_BIT6:
164 case TEST_OUT_BIT7:
165 case TEST_IN_BIT7:
166 case TEST_OUT_BIT11:
167 case TEST_IN_BIT11:
168 return ioam6h->overflow ||
169 ioam6h->nodelen != 1 ||
170 ioam6h->remlen;
171
172 case TEST_OUT_BIT8:
173 case TEST_IN_BIT8:
174 case TEST_OUT_BIT9:
175 case TEST_IN_BIT9:
176 case TEST_OUT_BIT10:
177 case TEST_IN_BIT10:
178 return ioam6h->overflow ||
179 ioam6h->nodelen != 2 ||
180 ioam6h->remlen;
181
182 case TEST_OUT_BIT22:
183 case TEST_IN_BIT22:
184 return ioam6h->overflow ||
185 ioam6h->nodelen ||
186 ioam6h->remlen;
187
188 case TEST_OUT_FULL_SUPP_TRACE:
189 case TEST_IN_FULL_SUPP_TRACE:
190 case TEST_FWD_FULL_SUPP_TRACE:
191 return ioam6h->overflow ||
192 ioam6h->nodelen != 15 ||
193 ioam6h->remlen;
194
195 default:
196 break;
197 }
198
199 return 1;
200}
201
202static int check_ioam6_data(__u8 **p, struct ioam6_trace_hdr *ioam6h,
203 const struct ioam_config cnf)
204{
205 unsigned int len;
206 __u8 aligned;
207 __u64 raw64;
208 __u32 raw32;
209
210 if (ioam6h->type.bit0) {
211 raw32 = __be32_to_cpu(*((__u32 *)*p));
212 if (cnf.hlim != (raw32 >> 24) || cnf.id != (raw32 & 0xffffff))
213 return 1;
214 *p += sizeof(__u32);
215 }
216
217 if (ioam6h->type.bit1) {
218 raw32 = __be32_to_cpu(*((__u32 *)*p));
219 if (cnf.ingr_id != (raw32 >> 16) ||
220 cnf.egr_id != (raw32 & 0xffff))
221 return 1;
222 *p += sizeof(__u32);
223 }
224
225 if (ioam6h->type.bit2)
226 *p += sizeof(__u32);
227
228 if (ioam6h->type.bit3)
229 *p += sizeof(__u32);
230
231 if (ioam6h->type.bit4) {
232 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff)
233 return 1;
234 *p += sizeof(__u32);
235 }
236
237 if (ioam6h->type.bit5) {
238 if (__be32_to_cpu(*((__u32 *)*p)) != cnf.ns_data)
239 return 1;
240 *p += sizeof(__u32);
241 }
242
243 if (ioam6h->type.bit6) {
244 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff)
245 return 1;
246 *p += sizeof(__u32);
247 }
248
249 if (ioam6h->type.bit7) {
250 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff)
251 return 1;
252 *p += sizeof(__u32);
253 }
254
255 if (ioam6h->type.bit8) {
256 raw64 = __be64_to_cpu(*((__u64 *)*p));
257 if (cnf.hlim != (raw64 >> 56) ||
258 cnf.wide != (raw64 & 0xffffffffffffff))
259 return 1;
260 *p += sizeof(__u64);
261 }
262
263 if (ioam6h->type.bit9) {
264 if (__be32_to_cpu(*((__u32 *)*p)) != cnf.ingr_wide)
265 return 1;
266 *p += sizeof(__u32);
267
268 if (__be32_to_cpu(*((__u32 *)*p)) != cnf.egr_wide)
269 return 1;
270 *p += sizeof(__u32);
271 }
272
273 if (ioam6h->type.bit10) {
274 if (__be64_to_cpu(*((__u64 *)*p)) != cnf.ns_wide)
275 return 1;
276 *p += sizeof(__u64);
277 }
278
279 if (ioam6h->type.bit11) {
280 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff)
281 return 1;
282 *p += sizeof(__u32);
283 }
284
285 if (ioam6h->type.bit12) {
286 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff)
287 return 1;
288 *p += sizeof(__u32);
289 }
290
291 if (ioam6h->type.bit13) {
292 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff)
293 return 1;
294 *p += sizeof(__u32);
295 }
296
297 if (ioam6h->type.bit14) {
298 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff)
299 return 1;
300 *p += sizeof(__u32);
301 }
302
303 if (ioam6h->type.bit15) {
304 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff)
305 return 1;
306 *p += sizeof(__u32);
307 }
308
309 if (ioam6h->type.bit16) {
310 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff)
311 return 1;
312 *p += sizeof(__u32);
313 }
314
315 if (ioam6h->type.bit17) {
316 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff)
317 return 1;
318 *p += sizeof(__u32);
319 }
320
321 if (ioam6h->type.bit18) {
322 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff)
323 return 1;
324 *p += sizeof(__u32);
325 }
326
327 if (ioam6h->type.bit19) {
328 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff)
329 return 1;
330 *p += sizeof(__u32);
331 }
332
333 if (ioam6h->type.bit20) {
334 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff)
335 return 1;
336 *p += sizeof(__u32);
337 }
338
339 if (ioam6h->type.bit21) {
340 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff)
341 return 1;
342 *p += sizeof(__u32);
343 }
344
345 if (ioam6h->type.bit22) {
346 len = cnf.sc_data ? strlen(cnf.sc_data) : 0;
347 aligned = cnf.sc_data ? __ALIGN_KERNEL(len, 4) : 0;
348
349 raw32 = __be32_to_cpu(*((__u32 *)*p));
350 if (aligned != (raw32 >> 24) * 4 ||
351 cnf.sc_id != (raw32 & 0xffffff))
352 return 1;
353 *p += sizeof(__u32);
354
355 if (cnf.sc_data) {
356 if (strncmp((char *)*p, cnf.sc_data, len))
357 return 1;
358
359 *p += len;
360 aligned -= len;
361
362 while (aligned--) {
363 if (**p != '\0')
364 return 1;
365 *p += sizeof(__u8);
366 }
367 }
368 }
369
370 return 0;
371}
372
373static int check_ioam_header_and_data(int tid, struct ioam6_trace_hdr *ioam6h,
374 __u32 trace_type, __u16 ioam_ns)
375{
376 __u8 *p;
377
378 if (check_ioam_header(tid, ioam6h, trace_type, ioam_ns))
379 return 1;
380
381 p = ioam6h->data + ioam6h->remlen * 4;
382
383 switch (tid) {
384 case TEST_OUT_BIT0:
385 case TEST_OUT_BIT1:
386 case TEST_OUT_BIT2:
387 case TEST_OUT_BIT3:
388 case TEST_OUT_BIT4:
389 case TEST_OUT_BIT5:
390 case TEST_OUT_BIT6:
391 case TEST_OUT_BIT7:
392 case TEST_OUT_BIT8:
393 case TEST_OUT_BIT9:
394 case TEST_OUT_BIT10:
395 case TEST_OUT_BIT11:
396 case TEST_OUT_BIT22:
397 case TEST_OUT_FULL_SUPP_TRACE:
398 return check_ioam6_data(&p, ioam6h, node1);
399
400 case TEST_IN_BIT0:
401 case TEST_IN_BIT1:
402 case TEST_IN_BIT2:
403 case TEST_IN_BIT3:
404 case TEST_IN_BIT4:
405 case TEST_IN_BIT5:
406 case TEST_IN_BIT6:
407 case TEST_IN_BIT7:
408 case TEST_IN_BIT8:
409 case TEST_IN_BIT9:
410 case TEST_IN_BIT10:
411 case TEST_IN_BIT11:
412 case TEST_IN_BIT22:
413 case TEST_IN_FULL_SUPP_TRACE:
414 {
415 __u32 tmp32 = node2.egr_wide;
416 __u16 tmp16 = node2.egr_id;
417 int res;
418
419 node2.egr_id = 0xffff;
420 node2.egr_wide = 0xffffffff;
421
422 res = check_ioam6_data(&p, ioam6h, node2);
423
424 node2.egr_id = tmp16;
425 node2.egr_wide = tmp32;
426
427 return res;
428 }
429
430 case TEST_FWD_FULL_SUPP_TRACE:
431 if (check_ioam6_data(&p, ioam6h, node3))
432 return 1;
433 if (check_ioam6_data(&p, ioam6h, node2))
434 return 1;
435 return check_ioam6_data(&p, ioam6h, node1);
436
437 default:
438 break;
439 }
440
441 return 1;
442}
443
444static int str2id(const char *tname)
445{
446 if (!strcmp("out_undef_ns", tname))
447 return TEST_OUT_UNDEF_NS;
448 if (!strcmp("out_no_room", tname))
449 return TEST_OUT_NO_ROOM;
450 if (!strcmp("out_bit0", tname))
451 return TEST_OUT_BIT0;
452 if (!strcmp("out_bit1", tname))
453 return TEST_OUT_BIT1;
454 if (!strcmp("out_bit2", tname))
455 return TEST_OUT_BIT2;
456 if (!strcmp("out_bit3", tname))
457 return TEST_OUT_BIT3;
458 if (!strcmp("out_bit4", tname))
459 return TEST_OUT_BIT4;
460 if (!strcmp("out_bit5", tname))
461 return TEST_OUT_BIT5;
462 if (!strcmp("out_bit6", tname))
463 return TEST_OUT_BIT6;
464 if (!strcmp("out_bit7", tname))
465 return TEST_OUT_BIT7;
466 if (!strcmp("out_bit8", tname))
467 return TEST_OUT_BIT8;
468 if (!strcmp("out_bit9", tname))
469 return TEST_OUT_BIT9;
470 if (!strcmp("out_bit10", tname))
471 return TEST_OUT_BIT10;
472 if (!strcmp("out_bit11", tname))
473 return TEST_OUT_BIT11;
474 if (!strcmp("out_bit22", tname))
475 return TEST_OUT_BIT22;
476 if (!strcmp("out_full_supp_trace", tname))
477 return TEST_OUT_FULL_SUPP_TRACE;
478 if (!strcmp("in_undef_ns", tname))
479 return TEST_IN_UNDEF_NS;
480 if (!strcmp("in_no_room", tname))
481 return TEST_IN_NO_ROOM;
482 if (!strcmp("in_oflag", tname))
483 return TEST_IN_OFLAG;
484 if (!strcmp("in_bit0", tname))
485 return TEST_IN_BIT0;
486 if (!strcmp("in_bit1", tname))
487 return TEST_IN_BIT1;
488 if (!strcmp("in_bit2", tname))
489 return TEST_IN_BIT2;
490 if (!strcmp("in_bit3", tname))
491 return TEST_IN_BIT3;
492 if (!strcmp("in_bit4", tname))
493 return TEST_IN_BIT4;
494 if (!strcmp("in_bit5", tname))
495 return TEST_IN_BIT5;
496 if (!strcmp("in_bit6", tname))
497 return TEST_IN_BIT6;
498 if (!strcmp("in_bit7", tname))
499 return TEST_IN_BIT7;
500 if (!strcmp("in_bit8", tname))
501 return TEST_IN_BIT8;
502 if (!strcmp("in_bit9", tname))
503 return TEST_IN_BIT9;
504 if (!strcmp("in_bit10", tname))
505 return TEST_IN_BIT10;
506 if (!strcmp("in_bit11", tname))
507 return TEST_IN_BIT11;
508 if (!strcmp("in_bit22", tname))
509 return TEST_IN_BIT22;
510 if (!strcmp("in_full_supp_trace", tname))
511 return TEST_IN_FULL_SUPP_TRACE;
512 if (!strcmp("fwd_full_supp_trace", tname))
513 return TEST_FWD_FULL_SUPP_TRACE;
514
515 return -1;
516}
517
518static int ipv6_addr_equal(const struct in6_addr *a1, const struct in6_addr *a2)
519{
520 return ((a1->s6_addr32[0] ^ a2->s6_addr32[0]) |
521 (a1->s6_addr32[1] ^ a2->s6_addr32[1]) |
522 (a1->s6_addr32[2] ^ a2->s6_addr32[2]) |
523 (a1->s6_addr32[3] ^ a2->s6_addr32[3])) == 0;
524}
525
526static int get_u32(__u32 *val, const char *arg, int base)
527{
528 unsigned long res;
529 char *ptr;
530
531 if (!arg || !*arg)
532 return -1;
533 res = strtoul(arg, &ptr, base);
534
535 if (!ptr || ptr == arg || *ptr)
536 return -1;
537
538 if (res == ULONG_MAX && errno == ERANGE)
539 return -1;
540
541 if (res > 0xFFFFFFFFUL)
542 return -1;
543
544 *val = res;
545 return 0;
546}
547
548static int get_u16(__u16 *val, const char *arg, int base)
549{
550 unsigned long res;
551 char *ptr;
552
553 if (!arg || !*arg)
554 return -1;
555 res = strtoul(arg, &ptr, base);
556
557 if (!ptr || ptr == arg || *ptr)
558 return -1;
559
560 if (res == ULONG_MAX && errno == ERANGE)
561 return -1;
562
563 if (res > 0xFFFFUL)
564 return -1;
565
566 *val = res;
567 return 0;
568}
569
570static int (*func[__TEST_MAX])(int, struct ioam6_trace_hdr *, __u32, __u16) = {
571 [TEST_OUT_UNDEF_NS] = check_ioam_header,
572 [TEST_OUT_NO_ROOM] = check_ioam_header,
573 [TEST_OUT_BIT0] = check_ioam_header_and_data,
574 [TEST_OUT_BIT1] = check_ioam_header_and_data,
575 [TEST_OUT_BIT2] = check_ioam_header_and_data,
576 [TEST_OUT_BIT3] = check_ioam_header_and_data,
577 [TEST_OUT_BIT4] = check_ioam_header_and_data,
578 [TEST_OUT_BIT5] = check_ioam_header_and_data,
579 [TEST_OUT_BIT6] = check_ioam_header_and_data,
580 [TEST_OUT_BIT7] = check_ioam_header_and_data,
581 [TEST_OUT_BIT8] = check_ioam_header_and_data,
582 [TEST_OUT_BIT9] = check_ioam_header_and_data,
583 [TEST_OUT_BIT10] = check_ioam_header_and_data,
584 [TEST_OUT_BIT11] = check_ioam_header_and_data,
585 [TEST_OUT_BIT22] = check_ioam_header_and_data,
586 [TEST_OUT_FULL_SUPP_TRACE] = check_ioam_header_and_data,
587 [TEST_IN_UNDEF_NS] = check_ioam_header,
588 [TEST_IN_NO_ROOM] = check_ioam_header,
589 [TEST_IN_OFLAG] = check_ioam_header,
590 [TEST_IN_BIT0] = check_ioam_header_and_data,
591 [TEST_IN_BIT1] = check_ioam_header_and_data,
592 [TEST_IN_BIT2] = check_ioam_header_and_data,
593 [TEST_IN_BIT3] = check_ioam_header_and_data,
594 [TEST_IN_BIT4] = check_ioam_header_and_data,
595 [TEST_IN_BIT5] = check_ioam_header_and_data,
596 [TEST_IN_BIT6] = check_ioam_header_and_data,
597 [TEST_IN_BIT7] = check_ioam_header_and_data,
598 [TEST_IN_BIT8] = check_ioam_header_and_data,
599 [TEST_IN_BIT9] = check_ioam_header_and_data,
600 [TEST_IN_BIT10] = check_ioam_header_and_data,
601 [TEST_IN_BIT11] = check_ioam_header_and_data,
602 [TEST_IN_BIT22] = check_ioam_header_and_data,
603 [TEST_IN_FULL_SUPP_TRACE] = check_ioam_header_and_data,
604 [TEST_FWD_FULL_SUPP_TRACE] = check_ioam_header_and_data,
605};
606
607int main(int argc, char **argv)
608{
609 int fd, size, hoplen, tid, ret = 1;
610 struct in6_addr src, dst;
611 struct ioam6_hdr *opt;
612 struct ipv6hdr *ip6h;
613 __u8 buffer[400], *p;
614 __u16 ioam_ns;
615 __u32 tr_type;
616
617 if (argc != 7)
618 goto out;
619
620 tid = str2id(argv[2]);
621 if (tid < 0 || !func[tid])
622 goto out;
623
624 if (inet_pton(AF_INET6, argv[3], &src) != 1 ||
625 inet_pton(AF_INET6, argv[4], &dst) != 1)
626 goto out;
627
628 if (get_u32(&tr_type, argv[5], 16) ||
629 get_u16(&ioam_ns, argv[6], 0))
630 goto out;
631
632 fd = socket(AF_PACKET, SOCK_DGRAM, __cpu_to_be16(ETH_P_IPV6));
633 if (!fd)
634 goto out;
635
636 if (setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE,
637 argv[1], strlen(argv[1])))
638 goto close;
639
640recv:
641 size = recv(fd, buffer, sizeof(buffer), 0);
642 if (size <= 0)
643 goto close;
644
645 ip6h = (struct ipv6hdr *)buffer;
646
647 if (!ipv6_addr_equal(&ip6h->saddr, &src) ||
648 !ipv6_addr_equal(&ip6h->daddr, &dst))
649 goto recv;
650
651 if (ip6h->nexthdr != IPPROTO_HOPOPTS)
652 goto close;
653
654 p = buffer + sizeof(*ip6h);
655 hoplen = (p[1] + 1) << 3;
656 p += sizeof(struct ipv6_hopopt_hdr);
657
658 while (hoplen > 0) {
659 opt = (struct ioam6_hdr *)p;
660
661 if (opt->opt_type == IPV6_TLV_IOAM &&
662 opt->type == IOAM6_TYPE_PREALLOC) {
663 p += sizeof(*opt);
664 ret = func[tid](tid, (struct ioam6_trace_hdr *)p,
665 tr_type, ioam_ns);
666 break;
667 }
668
669 p += opt->opt_len + 2;
670 hoplen -= opt->opt_len + 2;
671 }
672close:
673 close(fd);
674out:
675 return ret;
676}
677
