1 // SPDX-License-Identifier: GPL-2.0+ 2 /* 3 * Author: Justin Iurman (justin.iurman@uliege.be) 4 * 5 * IOAM parser for IPv6, see ioam6.sh for details. 6 */ 7 #include <asm/byteorder.h> 8 #include <linux/const.h> 9 #include <linux/if_ether.h> 10 #include <linux/ioam6.h> 11 #include <linux/ipv6.h> 12 #include <sys/socket.h> 13 #include <stdlib.h> 14 #include <string.h> 15 #include <unistd.h> 16 17 struct node_args { 18 __u32 id; 19 __u64 wide; 20 __u16 ingr_id; 21 __u16 egr_id; 22 __u32 ingr_wide; 23 __u32 egr_wide; 24 __u32 ns_data; 25 __u64 ns_wide; 26 __u32 sc_id; 27 __u8 hop_limit; 28 __u8 *sc_data; /* NULL when sc_id = 0xffffff (default empty value) */ 29 }; 30 31 /* expected args per node, in that order */ 32 enum { 33 NODE_ARG_HOP_LIMIT, 34 NODE_ARG_ID, 35 NODE_ARG_WIDE, 36 NODE_ARG_INGR_ID, 37 NODE_ARG_INGR_WIDE, 38 NODE_ARG_EGR_ID, 39 NODE_ARG_EGR_WIDE, 40 NODE_ARG_NS_DATA, 41 NODE_ARG_NS_WIDE, 42 NODE_ARG_SC_ID, 43 __NODE_ARG_MAX, 44 }; 45 46 #define NODE_ARGS_SIZE __NODE_ARG_MAX 47 48 struct args { 49 __u16 ns_id; 50 __u32 trace_type; 51 __u8 n_node; 52 __u8 *ifname; 53 struct node_args node[0]; 54 }; 55 56 /* expected args, in that order */ 57 enum { 58 ARG_IFNAME, 59 ARG_N_NODE, 60 ARG_NS_ID, 61 ARG_TRACE_TYPE, 62 __ARG_MAX, 63 }; 64 65 #define ARGS_SIZE __ARG_MAX 66 67 int check_ioam6_node_data(__u8 **p, struct ioam6_trace_hdr *trace, __u8 hlim, 68 __u32 id, __u64 wide, __u16 ingr_id, __u32 ingr_wide, 69 __u16 egr_id, __u32 egr_wide, __u32 ns_data, 70 __u64 ns_wide, __u32 sc_id, __u8 *sc_data) 71 { 72 __u64 raw64; 73 __u32 raw32; 74 __u8 sc_len; 75 76 if (trace->type.bit0) { 77 raw32 = __be32_to_cpu(*((__u32 *)*p)); 78 if (hlim != (raw32 >> 24) || id != (raw32 & 0xffffff)) 79 return 1; 80 *p += sizeof(__u32); 81 } 82 83 if (trace->type.bit1) { 84 raw32 = __be32_to_cpu(*((__u32 *)*p)); 85 if (ingr_id != (raw32 >> 16) || egr_id != (raw32 & 0xffff)) 86 return 1; 87 *p += sizeof(__u32); 88 } 89 90 if (trace->type.bit2) 91 *p += sizeof(__u32); 92 93 if (trace->type.bit3) 94 *p += sizeof(__u32); 95 96 if (trace->type.bit4) { 97 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff) 98 return 1; 99 *p += sizeof(__u32); 100 } 101 102 if (trace->type.bit5) { 103 if (__be32_to_cpu(*((__u32 *)*p)) != ns_data) 104 return 1; 105 *p += sizeof(__u32); 106 } 107 108 if (trace->type.bit6) { 109 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff) 110 return 1; 111 *p += sizeof(__u32); 112 } 113 114 if (trace->type.bit7) { 115 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff) 116 return 1; 117 *p += sizeof(__u32); 118 } 119 120 if (trace->type.bit8) { 121 raw64 = __be64_to_cpu(*((__u64 *)*p)); 122 if (hlim != (raw64 >> 56) || wide != (raw64 & 0xffffffffffffff)) 123 return 1; 124 *p += sizeof(__u64); 125 } 126 127 if (trace->type.bit9) { 128 if (__be32_to_cpu(*((__u32 *)*p)) != ingr_wide) 129 return 1; 130 *p += sizeof(__u32); 131 132 if (__be32_to_cpu(*((__u32 *)*p)) != egr_wide) 133 return 1; 134 *p += sizeof(__u32); 135 } 136 137 if (trace->type.bit10) { 138 if (__be64_to_cpu(*((__u64 *)*p)) != ns_wide) 139 return 1; 140 *p += sizeof(__u64); 141 } 142 143 if (trace->type.bit11) { 144 if (__be32_to_cpu(*((__u32 *)*p)) != 0xffffffff) 145 return 1; 146 *p += sizeof(__u32); 147 } 148 149 if (trace->type.bit22) { 150 raw32 = __be32_to_cpu(*((__u32 *)*p)); 151 sc_len = sc_data ? __ALIGN_KERNEL(strlen(sc_data), 4) : 0; 152 if (sc_len != (raw32 >> 24) * 4 || sc_id != (raw32 & 0xffffff)) 153 return 1; 154 *p += sizeof(__u32); 155 156 if (sc_data) { 157 if (strncmp(*p, sc_data, strlen(sc_data))) 158 return 1; 159 160 *p += strlen(sc_data); 161 sc_len -= strlen(sc_data); 162 163 while (sc_len--) { 164 if (**p != '\0') 165 return 1; 166 *p += sizeof(__u8); 167 } 168 } 169 } 170 171 return 0; 172 } 173 174 int check_ioam6_trace(struct ioam6_trace_hdr *trace, struct args *args) 175 { 176 __u8 *p; 177 int i; 178 179 if (__be16_to_cpu(trace->namespace_id) != args->ns_id || 180 __be32_to_cpu(trace->type_be32) != args->trace_type) 181 return 1; 182 183 p = trace->data + trace->remlen * 4; 184 185 for (i = args->n_node - 1; i >= 0; i--) { 186 if (check_ioam6_node_data(&p, trace, 187 args->node[i].hop_limit, 188 args->node[i].id, 189 args->node[i].wide, 190 args->node[i].ingr_id, 191 args->node[i].ingr_wide, 192 args->node[i].egr_id, 193 args->node[i].egr_wide, 194 args->node[i].ns_data, 195 args->node[i].ns_wide, 196 args->node[i].sc_id, 197 args->node[i].sc_data)) 198 return 1; 199 } 200 201 return 0; 202 } 203 204 int parse_node_args(int *argcp, char ***argvp, struct node_args *node) 205 { 206 char **argv = *argvp; 207 208 if (*argcp < NODE_ARGS_SIZE) 209 return 1; 210 211 node->hop_limit = strtoul(argv[NODE_ARG_HOP_LIMIT], NULL, 10); 212 if (!node->hop_limit) { 213 node->hop_limit = strtoul(argv[NODE_ARG_HOP_LIMIT], NULL, 16); 214 if (!node->hop_limit) 215 return 1; 216 } 217 218 node->id = strtoul(argv[NODE_ARG_ID], NULL, 10); 219 if (!node->id) { 220 node->id = strtoul(argv[NODE_ARG_ID], NULL, 16); 221 if (!node->id) 222 return 1; 223 } 224 225 node->wide = strtoull(argv[NODE_ARG_WIDE], NULL, 10); 226 if (!node->wide) { 227 node->wide = strtoull(argv[NODE_ARG_WIDE], NULL, 16); 228 if (!node->wide) 229 return 1; 230 } 231 232 node->ingr_id = strtoul(argv[NODE_ARG_INGR_ID], NULL, 10); 233 if (!node->ingr_id) { 234 node->ingr_id = strtoul(argv[NODE_ARG_INGR_ID], NULL, 16); 235 if (!node->ingr_id) 236 return 1; 237 } 238 239 node->ingr_wide = strtoul(argv[NODE_ARG_INGR_WIDE], NULL, 10); 240 if (!node->ingr_wide) { 241 node->ingr_wide = strtoul(argv[NODE_ARG_INGR_WIDE], NULL, 16); 242 if (!node->ingr_wide) 243 return 1; 244 } 245 246 node->egr_id = strtoul(argv[NODE_ARG_EGR_ID], NULL, 10); 247 if (!node->egr_id) { 248 node->egr_id = strtoul(argv[NODE_ARG_EGR_ID], NULL, 16); 249 if (!node->egr_id) 250 return 1; 251 } 252 253 node->egr_wide = strtoul(argv[NODE_ARG_EGR_WIDE], NULL, 10); 254 if (!node->egr_wide) { 255 node->egr_wide = strtoul(argv[NODE_ARG_EGR_WIDE], NULL, 16); 256 if (!node->egr_wide) 257 return 1; 258 } 259 260 node->ns_data = strtoul(argv[NODE_ARG_NS_DATA], NULL, 16); 261 if (!node->ns_data) 262 return 1; 263 264 node->ns_wide = strtoull(argv[NODE_ARG_NS_WIDE], NULL, 16); 265 if (!node->ns_wide) 266 return 1; 267 268 node->sc_id = strtoul(argv[NODE_ARG_SC_ID], NULL, 10); 269 if (!node->sc_id) { 270 node->sc_id = strtoul(argv[NODE_ARG_SC_ID], NULL, 16); 271 if (!node->sc_id) 272 return 1; 273 } 274 275 *argcp -= NODE_ARGS_SIZE; 276 *argvp += NODE_ARGS_SIZE; 277 278 if (node->sc_id != 0xffffff) { 279 if (!*argcp) 280 return 1; 281 282 node->sc_data = argv[NODE_ARG_SC_ID + 1]; 283 284 *argcp -= 1; 285 *argvp += 1; 286 } 287 288 return 0; 289 } 290 291 struct args *parse_args(int argc, char **argv) 292 { 293 struct args *args; 294 int n_node, i; 295 296 if (argc < ARGS_SIZE) 297 goto out; 298 299 n_node = strtoul(argv[ARG_N_NODE], NULL, 10); 300 if (!n_node || n_node > 10) 301 goto out; 302 303 args = calloc(1, sizeof(*args) + n_node * sizeof(struct node_args)); 304 if (!args) 305 goto out; 306 307 args->ns_id = strtoul(argv[ARG_NS_ID], NULL, 10); 308 if (!args->ns_id) 309 goto free; 310 311 args->trace_type = strtoul(argv[ARG_TRACE_TYPE], NULL, 16); 312 if (!args->trace_type) 313 goto free; 314 315 args->n_node = n_node; 316 args->ifname = argv[ARG_IFNAME]; 317 318 argv += ARGS_SIZE; 319 argc -= ARGS_SIZE; 320 321 for (i = 0; i < n_node; i++) { 322 if (parse_node_args(&argc, &argv, &args->node[i])) 323 goto free; 324 } 325 326 if (argc) 327 goto free; 328 329 return args; 330 free: 331 free(args); 332 out: 333 return NULL; 334 } 335 336 int main(int argc, char **argv) 337 { 338 int ret, fd, pkts, size, hoplen, found; 339 struct ioam6_trace_hdr *ioam6h; 340 struct ioam6_hdr *opt; 341 struct ipv6hdr *ip6h; 342 __u8 buffer[400], *p; 343 struct args *args; 344 345 args = parse_args(argc - 1, argv + 1); 346 if (!args) { 347 ret = 1; 348 goto out; 349 } 350 351 fd = socket(AF_PACKET, SOCK_DGRAM, __cpu_to_be16(ETH_P_IPV6)); 352 if (!fd) { 353 ret = 1; 354 goto out; 355 } 356 357 if (setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, 358 args->ifname, strlen(args->ifname))) { 359 ret = 1; 360 goto close; 361 } 362 363 pkts = 0; 364 found = 0; 365 while (pkts < 3 && !found) { 366 size = recv(fd, buffer, sizeof(buffer), 0); 367 ip6h = (struct ipv6hdr *)buffer; 368 pkts++; 369 370 if (ip6h->nexthdr == IPPROTO_HOPOPTS) { 371 p = buffer + sizeof(*ip6h); 372 hoplen = (p[1] + 1) << 3; 373 374 p += sizeof(struct ipv6_hopopt_hdr); 375 while (hoplen > 0) { 376 opt = (struct ioam6_hdr *)p; 377 378 if (opt->opt_type == IPV6_TLV_IOAM && 379 opt->type == IOAM6_TYPE_PREALLOC) { 380 found = 1; 381 382 p += sizeof(*opt); 383 ioam6h = (struct ioam6_trace_hdr *)p; 384 385 ret = check_ioam6_trace(ioam6h, args); 386 break; 387 } 388 389 p += opt->opt_len + 2; 390 hoplen -= opt->opt_len + 2; 391 } 392 } 393 } 394 395 if (!found) 396 ret = 1; 397 close: 398 close(fd); 399 out: 400 free(args); 401 return ret; 402 } 403