1 // SPDX-License-Identifier: GPL-2.0+ 2 /* 3 * IPv6 IOAM implementation 4 * 5 * Author: 6 * Justin Iurman <justin.iurman@uliege.be> 7 */ 8 9 #include <linux/errno.h> 10 #include <linux/types.h> 11 #include <linux/kernel.h> 12 #include <linux/net.h> 13 #include <linux/ioam6.h> 14 #include <linux/ioam6_genl.h> 15 #include <linux/rhashtable.h> 16 #include <linux/netdevice.h> 17 18 #include <net/addrconf.h> 19 #include <net/genetlink.h> 20 #include <net/ioam6.h> 21 #include <net/sch_generic.h> 22 23 static void ioam6_ns_release(struct ioam6_namespace *ns) 24 { 25 kfree_rcu(ns, rcu); 26 } 27 28 static void ioam6_sc_release(struct ioam6_schema *sc) 29 { 30 kfree_rcu(sc, rcu); 31 } 32 33 static void ioam6_free_ns(void *ptr, void *arg) 34 { 35 struct ioam6_namespace *ns = (struct ioam6_namespace *)ptr; 36 37 if (ns) 38 ioam6_ns_release(ns); 39 } 40 41 static void ioam6_free_sc(void *ptr, void *arg) 42 { 43 struct ioam6_schema *sc = (struct ioam6_schema *)ptr; 44 45 if (sc) 46 ioam6_sc_release(sc); 47 } 48 49 static int ioam6_ns_cmpfn(struct rhashtable_compare_arg *arg, const void *obj) 50 { 51 const struct ioam6_namespace *ns = obj; 52 53 return (ns->id != *(__be16 *)arg->key); 54 } 55 56 static int ioam6_sc_cmpfn(struct rhashtable_compare_arg *arg, const void *obj) 57 { 58 const struct ioam6_schema *sc = obj; 59 60 return (sc->id != *(u32 *)arg->key); 61 } 62 63 static const struct rhashtable_params rht_ns_params = { 64 .key_len = sizeof(__be16), 65 .key_offset = offsetof(struct ioam6_namespace, id), 66 .head_offset = offsetof(struct ioam6_namespace, head), 67 .automatic_shrinking = true, 68 .obj_cmpfn = ioam6_ns_cmpfn, 69 }; 70 71 static const struct rhashtable_params rht_sc_params = { 72 .key_len = sizeof(u32), 73 .key_offset = offsetof(struct ioam6_schema, id), 74 .head_offset = offsetof(struct ioam6_schema, head), 75 .automatic_shrinking = true, 76 .obj_cmpfn = ioam6_sc_cmpfn, 77 }; 78 79 static struct genl_family ioam6_genl_family; 80 81 static const struct nla_policy ioam6_genl_policy_addns[] = { 82 [IOAM6_ATTR_NS_ID] = { .type = NLA_U16 }, 83 [IOAM6_ATTR_NS_DATA] = { .type = NLA_U32 }, 84 [IOAM6_ATTR_NS_DATA_WIDE] = { .type = NLA_U64 }, 85 }; 86 87 static const struct nla_policy ioam6_genl_policy_delns[] = { 88 [IOAM6_ATTR_NS_ID] = { .type = NLA_U16 }, 89 }; 90 91 static const struct nla_policy ioam6_genl_policy_addsc[] = { 92 [IOAM6_ATTR_SC_ID] = { .type = NLA_U32 }, 93 [IOAM6_ATTR_SC_DATA] = { .type = NLA_BINARY, 94 .len = IOAM6_MAX_SCHEMA_DATA_LEN }, 95 }; 96 97 static const struct nla_policy ioam6_genl_policy_delsc[] = { 98 [IOAM6_ATTR_SC_ID] = { .type = NLA_U32 }, 99 }; 100 101 static const struct nla_policy ioam6_genl_policy_ns_sc[] = { 102 [IOAM6_ATTR_NS_ID] = { .type = NLA_U16 }, 103 [IOAM6_ATTR_SC_ID] = { .type = NLA_U32 }, 104 [IOAM6_ATTR_SC_NONE] = { .type = NLA_FLAG }, 105 }; 106 107 static int ioam6_genl_addns(struct sk_buff *skb, struct genl_info *info) 108 { 109 struct ioam6_pernet_data *nsdata; 110 struct ioam6_namespace *ns; 111 u64 data64; 112 u32 data32; 113 __be16 id; 114 int err; 115 116 if (!info->attrs[IOAM6_ATTR_NS_ID]) 117 return -EINVAL; 118 119 id = cpu_to_be16(nla_get_u16(info->attrs[IOAM6_ATTR_NS_ID])); 120 nsdata = ioam6_pernet(genl_info_net(info)); 121 122 mutex_lock(&nsdata->lock); 123 124 ns = rhashtable_lookup_fast(&nsdata->namespaces, &id, rht_ns_params); 125 if (ns) { 126 err = -EEXIST; 127 goto out_unlock; 128 } 129 130 ns = kzalloc(sizeof(*ns), GFP_KERNEL); 131 if (!ns) { 132 err = -ENOMEM; 133 goto out_unlock; 134 } 135 136 ns->id = id; 137 138 if (!info->attrs[IOAM6_ATTR_NS_DATA]) 139 data32 = IOAM6_U32_UNAVAILABLE; 140 else 141 data32 = nla_get_u32(info->attrs[IOAM6_ATTR_NS_DATA]); 142 143 if (!info->attrs[IOAM6_ATTR_NS_DATA_WIDE]) 144 data64 = IOAM6_U64_UNAVAILABLE; 145 else 146 data64 = nla_get_u64(info->attrs[IOAM6_ATTR_NS_DATA_WIDE]); 147 148 ns->data = cpu_to_be32(data32); 149 ns->data_wide = cpu_to_be64(data64); 150 151 err = rhashtable_lookup_insert_fast(&nsdata->namespaces, &ns->head, 152 rht_ns_params); 153 if (err) 154 kfree(ns); 155 156 out_unlock: 157 mutex_unlock(&nsdata->lock); 158 return err; 159 } 160 161 static int ioam6_genl_delns(struct sk_buff *skb, struct genl_info *info) 162 { 163 struct ioam6_pernet_data *nsdata; 164 struct ioam6_namespace *ns; 165 struct ioam6_schema *sc; 166 __be16 id; 167 int err; 168 169 if (!info->attrs[IOAM6_ATTR_NS_ID]) 170 return -EINVAL; 171 172 id = cpu_to_be16(nla_get_u16(info->attrs[IOAM6_ATTR_NS_ID])); 173 nsdata = ioam6_pernet(genl_info_net(info)); 174 175 mutex_lock(&nsdata->lock); 176 177 ns = rhashtable_lookup_fast(&nsdata->namespaces, &id, rht_ns_params); 178 if (!ns) { 179 err = -ENOENT; 180 goto out_unlock; 181 } 182 183 sc = rcu_dereference_protected(ns->schema, 184 lockdep_is_held(&nsdata->lock)); 185 186 err = rhashtable_remove_fast(&nsdata->namespaces, &ns->head, 187 rht_ns_params); 188 if (err) 189 goto out_unlock; 190 191 if (sc) 192 rcu_assign_pointer(sc->ns, NULL); 193 194 ioam6_ns_release(ns); 195 196 out_unlock: 197 mutex_unlock(&nsdata->lock); 198 return err; 199 } 200 201 static int __ioam6_genl_dumpns_element(struct ioam6_namespace *ns, 202 u32 portid, 203 u32 seq, 204 u32 flags, 205 struct sk_buff *skb, 206 u8 cmd) 207 { 208 struct ioam6_schema *sc; 209 u64 data64; 210 u32 data32; 211 void *hdr; 212 213 hdr = genlmsg_put(skb, portid, seq, &ioam6_genl_family, flags, cmd); 214 if (!hdr) 215 return -ENOMEM; 216 217 data32 = be32_to_cpu(ns->data); 218 data64 = be64_to_cpu(ns->data_wide); 219 220 if (nla_put_u16(skb, IOAM6_ATTR_NS_ID, be16_to_cpu(ns->id)) || 221 (data32 != IOAM6_U32_UNAVAILABLE && 222 nla_put_u32(skb, IOAM6_ATTR_NS_DATA, data32)) || 223 (data64 != IOAM6_U64_UNAVAILABLE && 224 nla_put_u64_64bit(skb, IOAM6_ATTR_NS_DATA_WIDE, 225 data64, IOAM6_ATTR_PAD))) 226 goto nla_put_failure; 227 228 rcu_read_lock(); 229 230 sc = rcu_dereference(ns->schema); 231 if (sc && nla_put_u32(skb, IOAM6_ATTR_SC_ID, sc->id)) { 232 rcu_read_unlock(); 233 goto nla_put_failure; 234 } 235 236 rcu_read_unlock(); 237 238 genlmsg_end(skb, hdr); 239 return 0; 240 241 nla_put_failure: 242 genlmsg_cancel(skb, hdr); 243 return -EMSGSIZE; 244 } 245 246 static int ioam6_genl_dumpns_start(struct netlink_callback *cb) 247 { 248 struct ioam6_pernet_data *nsdata = ioam6_pernet(sock_net(cb->skb->sk)); 249 struct rhashtable_iter *iter = (struct rhashtable_iter *)cb->args[0]; 250 251 if (!iter) { 252 iter = kmalloc(sizeof(*iter), GFP_KERNEL); 253 if (!iter) 254 return -ENOMEM; 255 256 cb->args[0] = (long)iter; 257 } 258 259 rhashtable_walk_enter(&nsdata->namespaces, iter); 260 261 return 0; 262 } 263 264 static int ioam6_genl_dumpns_done(struct netlink_callback *cb) 265 { 266 struct rhashtable_iter *iter = (struct rhashtable_iter *)cb->args[0]; 267 268 rhashtable_walk_exit(iter); 269 kfree(iter); 270 271 return 0; 272 } 273 274 static int ioam6_genl_dumpns(struct sk_buff *skb, struct netlink_callback *cb) 275 { 276 struct rhashtable_iter *iter; 277 struct ioam6_namespace *ns; 278 int err; 279 280 iter = (struct rhashtable_iter *)cb->args[0]; 281 rhashtable_walk_start(iter); 282 283 for (;;) { 284 ns = rhashtable_walk_next(iter); 285 286 if (IS_ERR(ns)) { 287 if (PTR_ERR(ns) == -EAGAIN) 288 continue; 289 err = PTR_ERR(ns); 290 goto done; 291 } else if (!ns) { 292 break; 293 } 294 295 err = __ioam6_genl_dumpns_element(ns, 296 NETLINK_CB(cb->skb).portid, 297 cb->nlh->nlmsg_seq, 298 NLM_F_MULTI, 299 skb, 300 IOAM6_CMD_DUMP_NAMESPACES); 301 if (err) 302 goto done; 303 } 304 305 err = skb->len; 306 307 done: 308 rhashtable_walk_stop(iter); 309 return err; 310 } 311 312 static int ioam6_genl_addsc(struct sk_buff *skb, struct genl_info *info) 313 { 314 struct ioam6_pernet_data *nsdata; 315 int len, len_aligned, err; 316 struct ioam6_schema *sc; 317 u32 id; 318 319 if (!info->attrs[IOAM6_ATTR_SC_ID] || !info->attrs[IOAM6_ATTR_SC_DATA]) 320 return -EINVAL; 321 322 id = nla_get_u32(info->attrs[IOAM6_ATTR_SC_ID]); 323 nsdata = ioam6_pernet(genl_info_net(info)); 324 325 mutex_lock(&nsdata->lock); 326 327 sc = rhashtable_lookup_fast(&nsdata->schemas, &id, rht_sc_params); 328 if (sc) { 329 err = -EEXIST; 330 goto out_unlock; 331 } 332 333 len = nla_len(info->attrs[IOAM6_ATTR_SC_DATA]); 334 len_aligned = ALIGN(len, 4); 335 336 sc = kzalloc(sizeof(*sc) + len_aligned, GFP_KERNEL); 337 if (!sc) { 338 err = -ENOMEM; 339 goto out_unlock; 340 } 341 342 sc->id = id; 343 sc->len = len_aligned; 344 sc->hdr = cpu_to_be32(sc->id | ((u8)(sc->len / 4) << 24)); 345 nla_memcpy(sc->data, info->attrs[IOAM6_ATTR_SC_DATA], len); 346 347 err = rhashtable_lookup_insert_fast(&nsdata->schemas, &sc->head, 348 rht_sc_params); 349 if (err) 350 goto free_sc; 351 352 out_unlock: 353 mutex_unlock(&nsdata->lock); 354 return err; 355 free_sc: 356 kfree(sc); 357 goto out_unlock; 358 } 359 360 static int ioam6_genl_delsc(struct sk_buff *skb, struct genl_info *info) 361 { 362 struct ioam6_pernet_data *nsdata; 363 struct ioam6_namespace *ns; 364 struct ioam6_schema *sc; 365 int err; 366 u32 id; 367 368 if (!info->attrs[IOAM6_ATTR_SC_ID]) 369 return -EINVAL; 370 371 id = nla_get_u32(info->attrs[IOAM6_ATTR_SC_ID]); 372 nsdata = ioam6_pernet(genl_info_net(info)); 373 374 mutex_lock(&nsdata->lock); 375 376 sc = rhashtable_lookup_fast(&nsdata->schemas, &id, rht_sc_params); 377 if (!sc) { 378 err = -ENOENT; 379 goto out_unlock; 380 } 381 382 ns = rcu_dereference_protected(sc->ns, lockdep_is_held(&nsdata->lock)); 383 384 err = rhashtable_remove_fast(&nsdata->schemas, &sc->head, 385 rht_sc_params); 386 if (err) 387 goto out_unlock; 388 389 if (ns) 390 rcu_assign_pointer(ns->schema, NULL); 391 392 ioam6_sc_release(sc); 393 394 out_unlock: 395 mutex_unlock(&nsdata->lock); 396 return err; 397 } 398 399 static int __ioam6_genl_dumpsc_element(struct ioam6_schema *sc, 400 u32 portid, u32 seq, u32 flags, 401 struct sk_buff *skb, u8 cmd) 402 { 403 struct ioam6_namespace *ns; 404 void *hdr; 405 406 hdr = genlmsg_put(skb, portid, seq, &ioam6_genl_family, flags, cmd); 407 if (!hdr) 408 return -ENOMEM; 409 410 if (nla_put_u32(skb, IOAM6_ATTR_SC_ID, sc->id) || 411 nla_put(skb, IOAM6_ATTR_SC_DATA, sc->len, sc->data)) 412 goto nla_put_failure; 413 414 rcu_read_lock(); 415 416 ns = rcu_dereference(sc->ns); 417 if (ns && nla_put_u16(skb, IOAM6_ATTR_NS_ID, be16_to_cpu(ns->id))) { 418 rcu_read_unlock(); 419 goto nla_put_failure; 420 } 421 422 rcu_read_unlock(); 423 424 genlmsg_end(skb, hdr); 425 return 0; 426 427 nla_put_failure: 428 genlmsg_cancel(skb, hdr); 429 return -EMSGSIZE; 430 } 431 432 static int ioam6_genl_dumpsc_start(struct netlink_callback *cb) 433 { 434 struct ioam6_pernet_data *nsdata = ioam6_pernet(sock_net(cb->skb->sk)); 435 struct rhashtable_iter *iter = (struct rhashtable_iter *)cb->args[0]; 436 437 if (!iter) { 438 iter = kmalloc(sizeof(*iter), GFP_KERNEL); 439 if (!iter) 440 return -ENOMEM; 441 442 cb->args[0] = (long)iter; 443 } 444 445 rhashtable_walk_enter(&nsdata->schemas, iter); 446 447 return 0; 448 } 449 450 static int ioam6_genl_dumpsc_done(struct netlink_callback *cb) 451 { 452 struct rhashtable_iter *iter = (struct rhashtable_iter *)cb->args[0]; 453 454 rhashtable_walk_exit(iter); 455 kfree(iter); 456 457 return 0; 458 } 459 460 static int ioam6_genl_dumpsc(struct sk_buff *skb, struct netlink_callback *cb) 461 { 462 struct rhashtable_iter *iter; 463 struct ioam6_schema *sc; 464 int err; 465 466 iter = (struct rhashtable_iter *)cb->args[0]; 467 rhashtable_walk_start(iter); 468 469 for (;;) { 470 sc = rhashtable_walk_next(iter); 471 472 if (IS_ERR(sc)) { 473 if (PTR_ERR(sc) == -EAGAIN) 474 continue; 475 err = PTR_ERR(sc); 476 goto done; 477 } else if (!sc) { 478 break; 479 } 480 481 err = __ioam6_genl_dumpsc_element(sc, 482 NETLINK_CB(cb->skb).portid, 483 cb->nlh->nlmsg_seq, 484 NLM_F_MULTI, 485 skb, 486 IOAM6_CMD_DUMP_SCHEMAS); 487 if (err) 488 goto done; 489 } 490 491 err = skb->len; 492 493 done: 494 rhashtable_walk_stop(iter); 495 return err; 496 } 497 498 static int ioam6_genl_ns_set_schema(struct sk_buff *skb, struct genl_info *info) 499 { 500 struct ioam6_namespace *ns, *ns_ref; 501 struct ioam6_schema *sc, *sc_ref; 502 struct ioam6_pernet_data *nsdata; 503 __be16 ns_id; 504 u32 sc_id; 505 int err; 506 507 if (!info->attrs[IOAM6_ATTR_NS_ID] || 508 (!info->attrs[IOAM6_ATTR_SC_ID] && 509 !info->attrs[IOAM6_ATTR_SC_NONE])) 510 return -EINVAL; 511 512 ns_id = cpu_to_be16(nla_get_u16(info->attrs[IOAM6_ATTR_NS_ID])); 513 nsdata = ioam6_pernet(genl_info_net(info)); 514 515 mutex_lock(&nsdata->lock); 516 517 ns = rhashtable_lookup_fast(&nsdata->namespaces, &ns_id, rht_ns_params); 518 if (!ns) { 519 err = -ENOENT; 520 goto out_unlock; 521 } 522 523 if (info->attrs[IOAM6_ATTR_SC_NONE]) { 524 sc = NULL; 525 } else { 526 sc_id = nla_get_u32(info->attrs[IOAM6_ATTR_SC_ID]); 527 sc = rhashtable_lookup_fast(&nsdata->schemas, &sc_id, 528 rht_sc_params); 529 if (!sc) { 530 err = -ENOENT; 531 goto out_unlock; 532 } 533 } 534 535 sc_ref = rcu_dereference_protected(ns->schema, 536 lockdep_is_held(&nsdata->lock)); 537 if (sc_ref) 538 rcu_assign_pointer(sc_ref->ns, NULL); 539 rcu_assign_pointer(ns->schema, sc); 540 541 if (sc) { 542 ns_ref = rcu_dereference_protected(sc->ns, 543 lockdep_is_held(&nsdata->lock)); 544 if (ns_ref) 545 rcu_assign_pointer(ns_ref->schema, NULL); 546 rcu_assign_pointer(sc->ns, ns); 547 } 548 549 err = 0; 550 551 out_unlock: 552 mutex_unlock(&nsdata->lock); 553 return err; 554 } 555 556 static const struct genl_ops ioam6_genl_ops[] = { 557 { 558 .cmd = IOAM6_CMD_ADD_NAMESPACE, 559 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 560 .doit = ioam6_genl_addns, 561 .flags = GENL_ADMIN_PERM, 562 .policy = ioam6_genl_policy_addns, 563 .maxattr = ARRAY_SIZE(ioam6_genl_policy_addns) - 1, 564 }, 565 { 566 .cmd = IOAM6_CMD_DEL_NAMESPACE, 567 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 568 .doit = ioam6_genl_delns, 569 .flags = GENL_ADMIN_PERM, 570 .policy = ioam6_genl_policy_delns, 571 .maxattr = ARRAY_SIZE(ioam6_genl_policy_delns) - 1, 572 }, 573 { 574 .cmd = IOAM6_CMD_DUMP_NAMESPACES, 575 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 576 .start = ioam6_genl_dumpns_start, 577 .dumpit = ioam6_genl_dumpns, 578 .done = ioam6_genl_dumpns_done, 579 .flags = GENL_ADMIN_PERM, 580 }, 581 { 582 .cmd = IOAM6_CMD_ADD_SCHEMA, 583 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 584 .doit = ioam6_genl_addsc, 585 .flags = GENL_ADMIN_PERM, 586 .policy = ioam6_genl_policy_addsc, 587 .maxattr = ARRAY_SIZE(ioam6_genl_policy_addsc) - 1, 588 }, 589 { 590 .cmd = IOAM6_CMD_DEL_SCHEMA, 591 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 592 .doit = ioam6_genl_delsc, 593 .flags = GENL_ADMIN_PERM, 594 .policy = ioam6_genl_policy_delsc, 595 .maxattr = ARRAY_SIZE(ioam6_genl_policy_delsc) - 1, 596 }, 597 { 598 .cmd = IOAM6_CMD_DUMP_SCHEMAS, 599 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 600 .start = ioam6_genl_dumpsc_start, 601 .dumpit = ioam6_genl_dumpsc, 602 .done = ioam6_genl_dumpsc_done, 603 .flags = GENL_ADMIN_PERM, 604 }, 605 { 606 .cmd = IOAM6_CMD_NS_SET_SCHEMA, 607 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 608 .doit = ioam6_genl_ns_set_schema, 609 .flags = GENL_ADMIN_PERM, 610 .policy = ioam6_genl_policy_ns_sc, 611 .maxattr = ARRAY_SIZE(ioam6_genl_policy_ns_sc) - 1, 612 }, 613 }; 614 615 static struct genl_family ioam6_genl_family __ro_after_init = { 616 .name = IOAM6_GENL_NAME, 617 .version = IOAM6_GENL_VERSION, 618 .netnsok = true, 619 .parallel_ops = true, 620 .ops = ioam6_genl_ops, 621 .n_ops = ARRAY_SIZE(ioam6_genl_ops), 622 .module = THIS_MODULE, 623 }; 624 625 struct ioam6_namespace *ioam6_namespace(struct net *net, __be16 id) 626 { 627 struct ioam6_pernet_data *nsdata = ioam6_pernet(net); 628 629 return rhashtable_lookup_fast(&nsdata->namespaces, &id, rht_ns_params); 630 } 631 632 static void __ioam6_fill_trace_data(struct sk_buff *skb, 633 struct ioam6_namespace *ns, 634 struct ioam6_trace_hdr *trace, 635 struct ioam6_schema *sc, 636 u8 sclen, bool is_input) 637 { 638 struct __kernel_sock_timeval ts; 639 u64 raw64; 640 u32 raw32; 641 u16 raw16; 642 u8 *data; 643 u8 byte; 644 645 data = trace->data + trace->remlen * 4 - trace->nodelen * 4 - sclen * 4; 646 647 /* hop_lim and node_id */ 648 if (trace->type.bit0) { 649 byte = ipv6_hdr(skb)->hop_limit; 650 if (is_input) 651 byte--; 652 653 raw32 = dev_net(skb_dst(skb)->dev)->ipv6.sysctl.ioam6_id; 654 655 *(__be32 *)data = cpu_to_be32((byte << 24) | raw32); 656 data += sizeof(__be32); 657 } 658 659 /* ingress_if_id and egress_if_id */ 660 if (trace->type.bit1) { 661 if (!skb->dev) 662 raw16 = IOAM6_U16_UNAVAILABLE; 663 else 664 raw16 = (__force u16)__in6_dev_get(skb->dev)->cnf.ioam6_id; 665 666 *(__be16 *)data = cpu_to_be16(raw16); 667 data += sizeof(__be16); 668 669 if (skb_dst(skb)->dev->flags & IFF_LOOPBACK) 670 raw16 = IOAM6_U16_UNAVAILABLE; 671 else 672 raw16 = (__force u16)__in6_dev_get(skb_dst(skb)->dev)->cnf.ioam6_id; 673 674 *(__be16 *)data = cpu_to_be16(raw16); 675 data += sizeof(__be16); 676 } 677 678 /* timestamp seconds */ 679 if (trace->type.bit2) { 680 if (!skb->dev) { 681 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 682 } else { 683 if (!skb->tstamp) 684 __net_timestamp(skb); 685 686 skb_get_new_timestamp(skb, &ts); 687 *(__be32 *)data = cpu_to_be32((u32)ts.tv_sec); 688 } 689 data += sizeof(__be32); 690 } 691 692 /* timestamp subseconds */ 693 if (trace->type.bit3) { 694 if (!skb->dev) { 695 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 696 } else { 697 if (!skb->tstamp) 698 __net_timestamp(skb); 699 700 if (!trace->type.bit2) 701 skb_get_new_timestamp(skb, &ts); 702 703 *(__be32 *)data = cpu_to_be32((u32)ts.tv_usec); 704 } 705 data += sizeof(__be32); 706 } 707 708 /* transit delay */ 709 if (trace->type.bit4) { 710 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 711 data += sizeof(__be32); 712 } 713 714 /* namespace data */ 715 if (trace->type.bit5) { 716 *(__be32 *)data = ns->data; 717 data += sizeof(__be32); 718 } 719 720 /* queue depth */ 721 if (trace->type.bit6) { 722 struct netdev_queue *queue; 723 struct Qdisc *qdisc; 724 __u32 qlen, backlog; 725 726 if (skb_dst(skb)->dev->flags & IFF_LOOPBACK) { 727 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 728 } else { 729 queue = skb_get_tx_queue(skb_dst(skb)->dev, skb); 730 qdisc = rcu_dereference(queue->qdisc); 731 qdisc_qstats_qlen_backlog(qdisc, &qlen, &backlog); 732 733 *(__be32 *)data = cpu_to_be32(backlog); 734 } 735 data += sizeof(__be32); 736 } 737 738 /* checksum complement */ 739 if (trace->type.bit7) { 740 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 741 data += sizeof(__be32); 742 } 743 744 /* hop_lim and node_id (wide) */ 745 if (trace->type.bit8) { 746 byte = ipv6_hdr(skb)->hop_limit; 747 if (is_input) 748 byte--; 749 750 raw64 = dev_net(skb_dst(skb)->dev)->ipv6.sysctl.ioam6_id_wide; 751 752 *(__be64 *)data = cpu_to_be64(((u64)byte << 56) | raw64); 753 data += sizeof(__be64); 754 } 755 756 /* ingress_if_id and egress_if_id (wide) */ 757 if (trace->type.bit9) { 758 if (!skb->dev) 759 raw32 = IOAM6_U32_UNAVAILABLE; 760 else 761 raw32 = __in6_dev_get(skb->dev)->cnf.ioam6_id_wide; 762 763 *(__be32 *)data = cpu_to_be32(raw32); 764 data += sizeof(__be32); 765 766 if (skb_dst(skb)->dev->flags & IFF_LOOPBACK) 767 raw32 = IOAM6_U32_UNAVAILABLE; 768 else 769 raw32 = __in6_dev_get(skb_dst(skb)->dev)->cnf.ioam6_id_wide; 770 771 *(__be32 *)data = cpu_to_be32(raw32); 772 data += sizeof(__be32); 773 } 774 775 /* namespace data (wide) */ 776 if (trace->type.bit10) { 777 *(__be64 *)data = ns->data_wide; 778 data += sizeof(__be64); 779 } 780 781 /* buffer occupancy */ 782 if (trace->type.bit11) { 783 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 784 data += sizeof(__be32); 785 } 786 787 /* bit12 undefined: filled with empty value */ 788 if (trace->type.bit12) { 789 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 790 data += sizeof(__be32); 791 } 792 793 /* bit13 undefined: filled with empty value */ 794 if (trace->type.bit13) { 795 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 796 data += sizeof(__be32); 797 } 798 799 /* bit14 undefined: filled with empty value */ 800 if (trace->type.bit14) { 801 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 802 data += sizeof(__be32); 803 } 804 805 /* bit15 undefined: filled with empty value */ 806 if (trace->type.bit15) { 807 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 808 data += sizeof(__be32); 809 } 810 811 /* bit16 undefined: filled with empty value */ 812 if (trace->type.bit16) { 813 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 814 data += sizeof(__be32); 815 } 816 817 /* bit17 undefined: filled with empty value */ 818 if (trace->type.bit17) { 819 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 820 data += sizeof(__be32); 821 } 822 823 /* bit18 undefined: filled with empty value */ 824 if (trace->type.bit18) { 825 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 826 data += sizeof(__be32); 827 } 828 829 /* bit19 undefined: filled with empty value */ 830 if (trace->type.bit19) { 831 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 832 data += sizeof(__be32); 833 } 834 835 /* bit20 undefined: filled with empty value */ 836 if (trace->type.bit20) { 837 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 838 data += sizeof(__be32); 839 } 840 841 /* bit21 undefined: filled with empty value */ 842 if (trace->type.bit21) { 843 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE); 844 data += sizeof(__be32); 845 } 846 847 /* opaque state snapshot */ 848 if (trace->type.bit22) { 849 if (!sc) { 850 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE >> 8); 851 } else { 852 *(__be32 *)data = sc->hdr; 853 data += sizeof(__be32); 854 855 memcpy(data, sc->data, sc->len); 856 } 857 } 858 } 859 860 /* called with rcu_read_lock() */ 861 void ioam6_fill_trace_data(struct sk_buff *skb, 862 struct ioam6_namespace *ns, 863 struct ioam6_trace_hdr *trace, 864 bool is_input) 865 { 866 struct ioam6_schema *sc; 867 u8 sclen = 0; 868 869 /* Skip if Overflow flag is set 870 */ 871 if (trace->overflow) 872 return; 873 874 /* NodeLen does not include Opaque State Snapshot length. We need to 875 * take it into account if the corresponding bit is set (bit 22) and 876 * if the current IOAM namespace has an active schema attached to it 877 */ 878 sc = rcu_dereference(ns->schema); 879 if (trace->type.bit22) { 880 sclen = sizeof_field(struct ioam6_schema, hdr) / 4; 881 882 if (sc) 883 sclen += sc->len / 4; 884 } 885 886 /* If there is no space remaining, we set the Overflow flag and we 887 * skip without filling the trace 888 */ 889 if (!trace->remlen || trace->remlen < trace->nodelen + sclen) { 890 trace->overflow = 1; 891 return; 892 } 893 894 __ioam6_fill_trace_data(skb, ns, trace, sc, sclen, is_input); 895 trace->remlen -= trace->nodelen + sclen; 896 } 897 898 static int __net_init ioam6_net_init(struct net *net) 899 { 900 struct ioam6_pernet_data *nsdata; 901 int err = -ENOMEM; 902 903 nsdata = kzalloc(sizeof(*nsdata), GFP_KERNEL); 904 if (!nsdata) 905 goto out; 906 907 mutex_init(&nsdata->lock); 908 net->ipv6.ioam6_data = nsdata; 909 910 err = rhashtable_init(&nsdata->namespaces, &rht_ns_params); 911 if (err) 912 goto free_nsdata; 913 914 err = rhashtable_init(&nsdata->schemas, &rht_sc_params); 915 if (err) 916 goto free_rht_ns; 917 918 out: 919 return err; 920 free_rht_ns: 921 rhashtable_destroy(&nsdata->namespaces); 922 free_nsdata: 923 kfree(nsdata); 924 net->ipv6.ioam6_data = NULL; 925 goto out; 926 } 927 928 static void __net_exit ioam6_net_exit(struct net *net) 929 { 930 struct ioam6_pernet_data *nsdata = ioam6_pernet(net); 931 932 rhashtable_free_and_destroy(&nsdata->namespaces, ioam6_free_ns, NULL); 933 rhashtable_free_and_destroy(&nsdata->schemas, ioam6_free_sc, NULL); 934 935 kfree(nsdata); 936 } 937 938 static struct pernet_operations ioam6_net_ops = { 939 .init = ioam6_net_init, 940 .exit = ioam6_net_exit, 941 }; 942 943 int __init ioam6_init(void) 944 { 945 int err = register_pernet_subsys(&ioam6_net_ops); 946 if (err) 947 goto out; 948 949 err = genl_register_family(&ioam6_genl_family); 950 if (err) 951 goto out_unregister_pernet_subsys; 952 953 #ifdef CONFIG_IPV6_IOAM6_LWTUNNEL 954 err = ioam6_iptunnel_init(); 955 if (err) 956 goto out_unregister_genl; 957 #endif 958 959 pr_info("In-situ OAM (IOAM) with IPv6\n"); 960 961 out: 962 return err; 963 #ifdef CONFIG_IPV6_IOAM6_LWTUNNEL 964 out_unregister_genl: 965 genl_unregister_family(&ioam6_genl_family); 966 #endif 967 out_unregister_pernet_subsys: 968 unregister_pernet_subsys(&ioam6_net_ops); 969 goto out; 970 } 971 972 void ioam6_exit(void) 973 { 974 #ifdef CONFIG_IPV6_IOAM6_LWTUNNEL 975 ioam6_iptunnel_exit(); 976 #endif 977 genl_unregister_family(&ioam6_genl_family); 978 unregister_pernet_subsys(&ioam6_net_ops); 979 } 980